fcl

fcl copied to clipboard

Summary

Distance calculations between geometric shapes (specifically Box) and OcTree return incorrect results in FCL 0.7.0.

Environment

• FCL Version: 0.7.0
• Operating System: Linux (Ubuntu)
• Compiler: GCC
• OctoMap Support: Enabled
• Build Type: Release/Debug (both affected)

Description

When calculating distances between a Box geometry and an OcTree, FCL returns significantly incorrect distance values. Our testing shows a discrepancy of approximately 9+ units between the FCL-calculated distance and the correct manual calculation.

Expected Behavior

Distance calculations should return accurate results consistent with manual geometric calculations.

Actual Behavior

FCL returns incorrect distance values when one of the geometries is an OcTree.

Reproduction

Test Case Setup

// Create a Box (1x1x1) positioned at (10, 0, 0)
auto drone_geometry = std::make_shared<Box<double>>(1.0, 1.0, 1.0);
CollisionObject<double> drone_object(drone_geometry);
drone_object.setTranslation(Vector3<double>(10.0, 0.0, 0.0));

// Create OcTree with points at (1,0,0), (9,0,0), (12,0,1)
auto octomap_tree = std::make_shared<octomap::OcTree>(0.1);
octomap_tree->updateNode(octomap::point3d(1.0, 0.0, 0.0), true);
octomap_tree->updateNode(octomap::point3d(9.0, 0.0, 0.0), true);
octomap_tree->updateNode(octomap::point3d(12.0, 0.0, 1.0), true);
octomap_tree->updateInnerOccupancy();

auto tree_geometry = std::make_shared<OcTree<double>>(octomap_tree);
CollisionObject<double> tree_object(tree_geometry);

// Calculate distance
DistanceRequest<double> request;
request.enable_nearest_points = true;
DistanceResult<double> result;
distance(&drone_object, &tree_object, request, result);

Results

• FCL Calculated Distance: ~0.4
• Manual Calculation: ~9.6
• Discrepancy: ~9.2 units (2300% error)

Detailed Output

FCL min_distance: 0.39999999999999858
Nearest point on drone (local): (9.1, 0.1, 0.1)
Nearest point on tree (local/world for OcTree): (9.5, 0.1, 0.1)

Manual Verification:
Nearest point on drone (WORLD): (19.1, 0.1, 0.1)
Nearest point on tree (WORLD): (9.5, 0.1, 0.1)
Manual calculated distance (world): 9.6000000000000014

Analysis

The issue appears to be related to coordinate frame transformations. The nearest points reported by FCL suggest that:

1. The Box's nearest point is being calculated in local coordinates but interpreted incorrectly
2. The transformation from local to world coordinates is not being applied properly for one or both geometries
3. The OcTree geometry may have different coordinate frame handling compared to other primitive shapes

Impact

This bug affects:

• Robotic path planning applications using OcTree representations
• Collision avoidance systems
• Any application requiring accurate distance measurements involving OcTree geometries

Workaround

Currently, no reliable workaround exists other than avoiding distance calculations between primitive shapes and OcTree geometries, or implementing manual distance calculation methods.

Test Case

I have created a comprehensive test case that reproduces this bug and can be used for regression testing:

• File: test/test_fcl_octree_distance_bug.cpp
• Purpose: Documents and verifies the bug behavior
• Status: Test fails as expected, confirming the bug exists

The test includes detailed debugging output and can be easily modified to pass (for documentation purposes) or fail (for active bug detection).

Additional Information

This issue was originally reported in #639 and was believed to be resolved in FCL 0.6.0+. However, our testing with FCL 0.7.0 confirms that the problem persists.

The bug appears to be specific to OcTree geometries, as similar tests with other geometry combinations (Box-Box, etc.) work correctly.

References

• Original issue: #639
• FCL Documentation: Distance Computation
• OctoMap Integration: FCL OctoMap Support

---

Priority: High - This affects core functionality for applications using OcTree representations.

Labels: bug, distance, octree, geometry, regression