Distance calculation bug between Box and OcTree geometries persists in FCL 0.7.0

[whyscience]

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

[whyscience]

code is here

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2016, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** @author Eric
 *
 *  Test to verify and document the known distance calculation bug between
 *  Box and OcTree geometries reported in GitHub issue #639.
 *
 *  This bug affects distance calculations between geometric shapes and OcTree,
 *  returning incorrect results. Testing confirms this issue still exists in
 *  FCL 0.7.0, contrary to the original issue description.
 *
 *  The test creates a specific scenario with a Box and OcTree that should have
 *  a known distance, then compares FCL's calculated distance with a manual
 *  calculation to detect the discrepancy.
 */

#include <gtest/gtest.h>

#include "fcl/config.h"
#include "fcl/geometry/octree/octree.h"
#include "fcl/geometry/shape/box.h"
#include "fcl/narrowphase/distance.h"

#if FCL_HAVE_OCTOMAP
#include <octomap/octomap.h>
#include <octomap/OcTreeKey.h>
#include <iostream>
#include <memory>

using namespace fcl;

// Helper functions for debugging
template <typename S>
void print_vector(const Vector3<S>& v, const std::string& name) {
    std::cout << name << ": (" << v[0] << ", " << v[1] << ", " << v[2] << ")" << std::endl;
}

template <typename S>
void print_transform(const Transform3<S>& tf, const std::string& name) {
    const auto& R = tf.rotation();
    const auto& T = tf.translation();
    std::cout << name << " Transform:" << std::endl;
    std::cout << "  - Rotation:\n"
              << "    " << R(0,0) << " " << R(0,1) << " " << R(0,2) << "\n"
              << "    " << R(1,0) << " " << R(1,1) << " " << R(1,2) << "\n"
              << "    " << R(2,0) << " " << R(2,1) << " " << R(2,2) << std::endl;
    std::cout << "  - Translation: (" << T[0] << ", " << T[1] << ", " << T[2] << ")" << std::endl;
}

GTEST_TEST(FCL_OCTREE, test_octree_distance_bug_verification)
{
    using S = double;

    // 1. Create geometries
    // Box representing "drone" with size 1x1x1
    auto drone_geometry = std::make_shared<Box<S>>(1.0, 1.0, 1.0);

    // Create OctoMap and fill with points
    S octree_resolution = 0.1;
    auto octomap_tree_data = std::make_shared<octomap::OcTree>(octree_resolution);

    // Add specified points
    octomap_tree_data->updateNode(octomap::point3d(1.0, 0.0, 0.0), true);
    octomap_tree_data->updateNode(octomap::point3d(9.0, 0.0, 0.0), true);
    octomap_tree_data->updateNode(octomap::point3d(12.0, 0.0, 1.0), true);
    octomap_tree_data->updateInnerOccupancy();

    std::cout << "OctoMap created with 3 points." << std::endl;
    std::cout << "OctoMap resolution: " << octomap_tree_data->getResolution() << std::endl;
    std::cout << "OctoMap size: " << octomap_tree_data->size() << " nodes" << std::endl;

    // Wrap octomap::OcTree into fcl::OcTree
    std::shared_ptr<const octomap::OcTree> octomap_tree_const_ptr = octomap_tree_data;
    auto tree_geometry = std::make_shared<OcTree<S>>(octomap_tree_const_ptr);

    // 2. Create collision objects
    CollisionObject<S> drone_object(drone_geometry);
    CollisionObject<S> tree_object(tree_geometry);

    // 3. Set transforms (poses)
    // Move "drone" to world coordinates (10, 0, 0)
    Vector3<S> drone_pos(10.0, 0.0, 0.0);
    drone_object.setTranslation(drone_pos);

    std::cout << "\n--- Initial Setup ---" << std::endl;
    print_transform(drone_object.getTransform(), "Drone");
    print_transform(tree_object.getTransform(), "Tree (OcTree)");
    std::cout << "---------------------\n" << std::endl;

    // 4. Perform distance query
    DistanceRequest<S> request;
    request.enable_nearest_points = true;
    request.gjk_solver_type = GJKSolverType::GST_LIBCCD;

    DistanceResult<S> result;

    distance(&drone_object, &tree_object, request, result);

    // 5. Result analysis and verification
    std::cout << "--- FCL Result ---" << std::endl;
    std::cout << "FCL min_distance: " << result.min_distance << std::endl;
    print_vector(result.nearest_points[0], "Nearest point on drone (local)");
    print_vector(result.nearest_points[1], "Nearest point on tree (local/world for OcTree)");
    std::cout << "------------------\n" << std::endl;

    std::cout << "--- Manual Verification ---" << std::endl;
    const Transform3<S>& drone_transform = drone_object.getTransform();
    const Transform3<S>& tree_transform = tree_object.getTransform();

    // Transform local nearest points to world coordinates
    Vector3<S> drone_point_world = drone_transform * result.nearest_points[0];
    Vector3<S> tree_point_world = tree_transform * result.nearest_points[1];

    print_vector(drone_point_world, "Nearest point on drone (WORLD)");
    print_vector(tree_point_world, "Nearest point on tree (WORLD)");

    S manual_distance = (drone_point_world - tree_point_world).norm();

    std::cout << "Manual calculated distance (world): " << manual_distance << std::endl;
    std::cout << "-------------------------\n" << std::endl;

    // 6. Final comparison
    std::cout << "--- Comparison ---" << std::endl;
    std::cout << "FCL min_distance: " << result.min_distance << std::endl;
    std::cout << "Manual distance:  " << manual_distance << std::endl;

    S tolerance = 1e-4; // Tolerance for OcTree
    bool distances_match = std::abs(result.min_distance - manual_distance) < tolerance;

    if (distances_match) {
        std::cout << "\nSUCCESS: Distances match! The verification logic seems correct with OcTree." << std::endl;
    } else {
        std::cout << "\nFAILURE: Distances DO NOT match with OcTree! Discrepancy detected." << std::endl;
        std::cout << "Difference: " << std::abs(result.min_distance - manual_distance) << std::endl;
    }

    // This test documents the known bug mentioned in the issue
    // https://github.com/flexible-collision-library/fcl/issues/639
    //
    // IMPORTANT: Testing shows this bug still exists in FCL 0.7.0, contrary to the
    // original issue description that suggested it was fixed in 0.6.0+
    //
    // The distance calculation between Box and OcTree geometries produces incorrect
    // results. In our test case:
    // - FCL reports distance: ~0.4
    // - Correct manual calculation: ~9.6
    // - Discrepancy: ~9.2 units
    //
    // This test will FAIL to document the ongoing bug. If you want the test to
    // pass and just document the behavior, comment out the EXPECT_TRUE below.
    EXPECT_TRUE(distances_match) << "Distance calculation discrepancy detected. "
                                 << "FCL distance: " << result.min_distance
                                 << ", Manual distance: " << manual_distance
                                 << ", Difference: " << std::abs(result.min_distance - manual_distance)
                                 << ". This confirms the bug still exists in FCL 0.7.0 "
                                 << "(see https://github.com/flexible-collision-library/fcl/issues/639)";
}

#endif // FCL_HAVE_OCTOMAP

//==============================================================================
int main(int argc, char* argv[])
{
#if FCL_HAVE_OCTOMAP
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
#else
  std::cout << "This test requires FCL to be compiled with OctoMap support" << std::endl;
  return 0;
#endif
}