From 457419020b92a3f9733556c22f1bbb51aee620a3 Mon Sep 17 00:00:00 2001 From: Sebastian Riedel Date: Thu, 28 Nov 2019 23:55:51 +0100 Subject: [PATCH 1/4] fix missing nearest points in Octree<->Mesh distance result --- .../fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h b/include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h index 3f76e9f52..9c6669648 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h +++ b/include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h @@ -506,7 +506,7 @@ bool OcTreeSolver::OcTreeMeshDistanceRecurse(const OcTree* Vector3 closest_p1, closest_p2; solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2); - dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id); + dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id, closest_p1, closest_p2); return drequest->isSatisfied(*dresult); } From eb1e0b205980781bac1bc2586c7dee2f601235a9 Mon Sep 17 00:00:00 2001 From: AlbertoMarnetto-Work <58592261+AlbertoMarnetto-Work@users.noreply.github.com> Date: Mon, 9 Dec 2019 11:04:32 +0100 Subject: [PATCH 2/4] Extend test_fcl_octomap_distance, check nearest_points S. #427 --- test/test_fcl_octomap_distance.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/test/test_fcl_octomap_distance.cpp b/test/test_fcl_octomap_distance.cpp index db430ce81..3a5e6880d 100644 --- a/test/test_fcl_octomap_distance.cpp +++ b/test/test_fcl_octomap_distance.cpp @@ -196,6 +196,7 @@ void octomap_distance_test_BVH(std::size_t n, double resolution) CollisionObject obj1(m1_ptr, tf); CollisionObject obj2(tree_ptr, tf); test::DistanceData cdata; + cdata.request.enable_nearest_points = true; S dist1 = std::numeric_limits::max(); test::defaultDistanceFunction(&obj1, &obj2, &cdata, dist1); @@ -219,6 +220,13 @@ void octomap_distance_test_BVH(std::size_t n, double resolution) std::cout << dist1 << " " << dist2 << std::endl; EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001); + + // Check that the nearest points are consistent with the distance + // Note that we cannot compare the result with the "boxes" approximation, + // since the problem is ill-posed (i.e. the nearest points may differ widely + // for slightly different geometries) + Vector3 nearestPointDistance = cdata.result.nearest_points[0] - cdata.result.nearest_points[1]; + EXPECT_NEAR(nearestPointDistance.norm(), dist1, 0.001); } } From 9973422dbf6220bf888f590a88e27c8780c13ba3 Mon Sep 17 00:00:00 2001 From: Sebastian Riedel Date: Mon, 9 Dec 2019 13:11:11 +0100 Subject: [PATCH 3/4] add test for octomap distance nearest point result (however not all tests pass) --- test/test_fcl_octomap_distance.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/test/test_fcl_octomap_distance.cpp b/test/test_fcl_octomap_distance.cpp index db430ce81..57a3d793f 100644 --- a/test/test_fcl_octomap_distance.cpp +++ b/test/test_fcl_octomap_distance.cpp @@ -65,6 +65,8 @@ void octomap_distance_test_BVH(std::size_t n, double resolution = 0.1); template void test_octomap_distance() { + // -- compares result of (manager of primitives <-> octomap) + // against (manager of primitives <-> manager of box primitives from octomap) #ifdef NDEBUG octomap_distance_test(200, 100, false, false); octomap_distance_test(200, 1000, false, false); @@ -83,6 +85,8 @@ GTEST_TEST(FCL_OCTOMAP, test_octomap_distance) template void test_octomap_distance_mesh() { + // -- compares result of (manager of meshes <-> octomap) + // against (manager of meshes <-> manager of box meshes from octomap) #ifdef NDEBUG octomap_distance_test(200, 100, true, true); octomap_distance_test(200, 1000, true, true); @@ -101,6 +105,8 @@ GTEST_TEST(FCL_OCTOMAP, test_octomap_distance_mesh) template void test_octomap_distance_mesh_octomap_box() { + // -- compares result of (manager of meshes <-> octomap) + // against (manager of meshes <-> manager of box primitives from octomap) #ifdef NDEBUG octomap_distance_test(200, 100, true, false); octomap_distance_test(200, 1000, true, false); @@ -219,6 +225,13 @@ void octomap_distance_test_BVH(std::size_t n, double resolution) std::cout << dist1 << " " << dist2 << std::endl; EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001); + + std::cout << "cdata, np0:\n" << cdata.result.nearest_points[0] << std::endl; + std::cout << "cdata, np1:\n" << cdata.result.nearest_points[1] << std::endl; + std::cout << "cdata2, np0:\n" << cdata2.result.nearest_points[0] << std::endl; + std::cout << "cdata2, np1:\n" << cdata2.result.nearest_points[1] << std::endl; + EXPECT_NEAR((cdata.result.nearest_points[0] - cdata2.result.nearest_points[1]).norm(), 0.0, 1e-6); + EXPECT_NEAR((cdata.result.nearest_points[1] - cdata2.result.nearest_points[0]).norm(), 0.0, 1e-6); } } @@ -294,6 +307,13 @@ void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, boo else EXPECT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3); + std::cout << "cdata, np0:\n" << cdata.result.nearest_points[0] << std::endl; + std::cout << "cdata, np1:\n" << cdata.result.nearest_points[1] << std::endl; + std::cout << "cdata2, np0:\n" << cdata2.result.nearest_points[0] << std::endl; + std::cout << "cdata2, np1:\n" << cdata2.result.nearest_points[1] << std::endl; + EXPECT_NEAR((cdata.result.nearest_points[0] - cdata2.result.nearest_points[0]).norm(), 0.0, 1e-6); + EXPECT_NEAR((cdata.result.nearest_points[1] - cdata2.result.nearest_points[1]).norm(), 0.0, 1e-6); + delete manager; delete manager2; for(size_t i = 0; i < boxes.size(); ++i) From aa00f5fb0bbbb64d6697d9ece5705732e844d76c Mon Sep 17 00:00:00 2001 From: Sebastian Riedel Date: Fri, 10 Jan 2020 21:08:54 +0100 Subject: [PATCH 4/4] revert changed test to original state (new test too brittle) --- test/test_fcl_octomap_distance.cpp | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/test/test_fcl_octomap_distance.cpp b/test/test_fcl_octomap_distance.cpp index 57a3d793f..db430ce81 100644 --- a/test/test_fcl_octomap_distance.cpp +++ b/test/test_fcl_octomap_distance.cpp @@ -65,8 +65,6 @@ void octomap_distance_test_BVH(std::size_t n, double resolution = 0.1); template void test_octomap_distance() { - // -- compares result of (manager of primitives <-> octomap) - // against (manager of primitives <-> manager of box primitives from octomap) #ifdef NDEBUG octomap_distance_test(200, 100, false, false); octomap_distance_test(200, 1000, false, false); @@ -85,8 +83,6 @@ GTEST_TEST(FCL_OCTOMAP, test_octomap_distance) template void test_octomap_distance_mesh() { - // -- compares result of (manager of meshes <-> octomap) - // against (manager of meshes <-> manager of box meshes from octomap) #ifdef NDEBUG octomap_distance_test(200, 100, true, true); octomap_distance_test(200, 1000, true, true); @@ -105,8 +101,6 @@ GTEST_TEST(FCL_OCTOMAP, test_octomap_distance_mesh) template void test_octomap_distance_mesh_octomap_box() { - // -- compares result of (manager of meshes <-> octomap) - // against (manager of meshes <-> manager of box primitives from octomap) #ifdef NDEBUG octomap_distance_test(200, 100, true, false); octomap_distance_test(200, 1000, true, false); @@ -225,13 +219,6 @@ void octomap_distance_test_BVH(std::size_t n, double resolution) std::cout << dist1 << " " << dist2 << std::endl; EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001); - - std::cout << "cdata, np0:\n" << cdata.result.nearest_points[0] << std::endl; - std::cout << "cdata, np1:\n" << cdata.result.nearest_points[1] << std::endl; - std::cout << "cdata2, np0:\n" << cdata2.result.nearest_points[0] << std::endl; - std::cout << "cdata2, np1:\n" << cdata2.result.nearest_points[1] << std::endl; - EXPECT_NEAR((cdata.result.nearest_points[0] - cdata2.result.nearest_points[1]).norm(), 0.0, 1e-6); - EXPECT_NEAR((cdata.result.nearest_points[1] - cdata2.result.nearest_points[0]).norm(), 0.0, 1e-6); } } @@ -307,13 +294,6 @@ void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, boo else EXPECT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3); - std::cout << "cdata, np0:\n" << cdata.result.nearest_points[0] << std::endl; - std::cout << "cdata, np1:\n" << cdata.result.nearest_points[1] << std::endl; - std::cout << "cdata2, np0:\n" << cdata2.result.nearest_points[0] << std::endl; - std::cout << "cdata2, np1:\n" << cdata2.result.nearest_points[1] << std::endl; - EXPECT_NEAR((cdata.result.nearest_points[0] - cdata2.result.nearest_points[0]).norm(), 0.0, 1e-6); - EXPECT_NEAR((cdata.result.nearest_points[1] - cdata2.result.nearest_points[1]).norm(), 0.0, 1e-6); - delete manager; delete manager2; for(size_t i = 0; i < boxes.size(); ++i)