test_fcl_octomap_distance: run more tfs, identity

Run more transforms when testing octomap distance, and be sure
to test identity transforms as well.

Also improve test feedback by using floating point tests so the values
are printed when they don't match.

test/test_fcl_octomap_distance.cpp

@@ -121,9 +121,9 @@ template <typename S>
 void test_octomap_bvh_rss_d_distance_rss()
 {
 #ifdef NDEBUG
-  octomap_distance_test_BVH<RSS<S>>(5);
+  octomap_distance_test_BVH<RSS<S>>(15);
 #else
-  octomap_distance_test_BVH<RSS<S>>(1, 1.0);
+  octomap_distance_test_BVH<RSS<S>>(15, 1.0);
 #endif
 }
 
@@ -137,9 +137,9 @@ template <typename S>
 void test_octomap_bvh_obb_d_distance_obb()
 {
 #ifdef NDEBUG
-  octomap_distance_test_BVH<OBBRSS<S>>(5);
+  octomap_distance_test_BVH<OBBRSS<S>>(15);
 #else
-  octomap_distance_test_BVH<OBBRSS<S>>(1, 1.0);
+  octomap_distance_test_BVH<OBBRSS<S>>(15, 1.0);
 #endif
 }
 
@@ -153,9 +153,9 @@ template <typename S>
 void test_octomap_bvh_kios_d_distance_kios()
 {
 #ifdef NDEBUG
-  octomap_distance_test_BVH<kIOS<S>>(5);
+  octomap_distance_test_BVH<kIOS<S>>(15);
 #else
-  octomap_distance_test_BVH<kIOS<S>>(1, 1.0);
+  octomap_distance_test_BVH<kIOS<S>>(15, 1.0);
 #endif
 }
 
@@ -186,26 +186,39 @@ void octomap_distance_test_BVH(std::size_t n, double resolution)
   std::shared_ptr<CollisionGeometry<S>> tree_ptr(tree);
 
   aligned_vector<Transform3<S>> transforms;
-  S extents[] = {-10, -10, 10, 10, 10, 10};
+  S extents[] = {-10, -10, -10, 10, 10, 10};
 
   test::generateRandomTransforms(extents, transforms, n);
+  if (n > 1)
+  {
+    // Be sure to test identity
+    transforms[n - 1] = Transform3<S>::Identity();
+    transforms[n / 2 - 1] = Transform3<S>::Identity();
+    transforms[n / 2] = Transform3<S>::Identity();
+  }
 
   for(std::size_t i = 0; i < n; ++i)
   {
-    Transform3<S> tf(transforms[i]);
+    Transform3<S> tf1(transforms[i]);
+    Transform3<S> tf2(transforms[n-1-i]);
 
-    CollisionObject<S> obj1(m1_ptr, tf);
-    CollisionObject<S> obj2(tree_ptr, tf);
+    CollisionObject<S> obj1(m1_ptr, tf1);
+    CollisionObject<S> obj2(tree_ptr, tf2);
     DefaultDistanceData<S> cdata;
+    DefaultDistanceData<S> cdata1b;
     cdata.request.enable_nearest_points = true;
+    cdata1b.request.enable_nearest_points = true;
     S dist1 = std::numeric_limits<S>::max();
+    S dist1b = std::numeric_limits<S>::max();
     DefaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
+    DefaultDistanceFunction(&obj2, &obj1, &cdata1b, dist1b);
+    EXPECT_NEAR(dist1, dist1b, 1e-6);
 
 
     std::vector<CollisionObject<S>*> boxes;
     test::generateBoxesFromOctomap(boxes, *tree);
     for(std::size_t j = 0; j < boxes.size(); ++j)
-      boxes[j]->setTransform(tf * boxes[j]->getTransform());
+      boxes[j]->setTransform(tf2 * boxes[j]->getTransform());
 
     DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>();
     manager->registerObjects(boxes);
@@ -216,18 +229,29 @@ void octomap_distance_test_BVH(std::size_t n, double resolution)
     S dist2 = cdata2.result.min_distance;
 
     for(std::size_t j = 0; j < boxes.size(); ++j)
+    {
+      DefaultDistanceData<S> cdatab;
+      S dist = std::numeric_limits<S>::max();
+      DefaultDistanceFunction(&obj1, boxes[j], &cdatab, dist);
       delete boxes[j];
+    }
     delete manager;
 
     std::cout << dist1 << " " << dist2 << std::endl;
-    EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001);
+    EXPECT_NEAR(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<S> nearestPointDistance = cdata.result.nearest_points[0] - cdata.result.nearest_points[1];
-    EXPECT_NEAR(nearestPointDistance.norm(), dist1, 0.001);
+    // Only check the nearest point distance for a non-collision. For a collision,
+    // the nearest points may be tangential and the equal to the (potentially
+    // fake) signed distance returned by the distance check.
+    if (dist1 >= 0.0)
+    {
+      EXPECT_NEAR(nearestPointDistance.norm(), dist1, 0.001);
+    }
   }
 }
 
@@ -299,9 +323,9 @@ void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, boo
   std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl;
 
   if(cdata.result.min_distance < 0)
-    EXPECT_TRUE(cdata2.result.min_distance <= 0);
+    EXPECT_LE(cdata2.result.min_distance, 0);
   else
-    EXPECT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3);
+    EXPECT_NEAR(cdata.result.min_distance, cdata2.result.min_distance, 1e-3);
 
   delete manager;
   delete manager2;