Merge pull request #1475 from tier4/fix/math-closest-point

Fix bug in `math::geometry::getClosestPoses`

common/math/geometry/src/bounding_box.cpp

@@ -62,22 +62,12 @@ std::optional<std::pair<geometry_msgs::msg::Pose, geometry_msgs::msg::Pose>> get
   const auto poly0 = toPolygon2D(pose0, bbox0);
   const auto poly1 = toPolygon2D(pose1, bbox1);
 
-  if (boost::geometry::intersects(poly0, poly1)) {
-    return std::nullopt;
-  }
-  if (boost::geometry::intersects(poly1, poly0)) {
-    return std::nullopt;
-  }
   if (boost::geometry::disjoint(poly0, poly1)) {
     auto point0 = boost_point();
     auto point1 = boost_point();
     auto min_distance = boost::numeric::bounds<double>::highest();
 
-    auto segments = boost::make_iterator_range(
-      boost::geometry::segments_begin(poly0), boost::geometry::segments_end(poly0));
-    auto points = boost::make_iterator_range(
-      boost::geometry::points_begin(poly1), boost::geometry::points_end(poly1));
-    auto findNearestPointInSegment = [&](const auto & segment, const auto & points) {
+    auto findNearestPointToSegment = [&](const auto & segment, const auto & points) {
       for (auto && point : points) {
         auto nearest_point_from_segment =
           pointToSegmentProjection(point, *segment.first, *segment.second);
@@ -90,9 +80,18 @@ std::optional<std::pair<geometry_msgs::msg::Pose, geometry_msgs::msg::Pose>> get
       }
     };
 
-    for (auto && segment : segments) {
-      findNearestPointInSegment(segment, points);
-    }
+    auto findNearestPointInPolygon = [&](const auto & poly0, const auto & poly1) {
+      auto segments = boost::make_iterator_range(
+        boost::geometry::segments_begin(poly0), boost::geometry::segments_end(poly0));
+      auto points = boost::geometry::exterior_ring(poly1);
+
+      for (auto && segment : segments) {
+        findNearestPointToSegment(segment, points);
+      }
+    };
+
+    findNearestPointInPolygon(poly0, poly1);
+    findNearestPointInPolygon(poly1, poly0);
 
     return std::make_pair(toPose(point0), toPose(point1));
   }

common/math/geometry/test/src/test_bounding_box.cpp

@@ -19,6 +19,7 @@
 #include <scenario_simulator_exception/exception.hpp>
 
 #include "expect_eq_macros.hpp"
+#include "geometry/distance.hpp"
 #include "test_utils.hpp"
 
 TEST(BoundingBox, getPointsFromBboxDefault)
@@ -83,7 +84,8 @@ TEST(BoundingBox, toPolygon2D_onlyTranslation)
 }
 
 /**
- * @note Test obtaining polygon from bounding box with full transformation applied (translation + rotation).
+ * @note Test obtaining polygon from bounding box with full transformation applied (translation +
+ * rotation).
  */
 TEST(BoundingBox, toPolygon2D_fullPose)
 {
@@ -135,6 +137,65 @@ TEST(BoundingBox, getPolygonDistanceWithoutCollision)
   EXPECT_DOUBLE_EQ(ans.value(), 3.0);
 }
 
+TEST(BoundingBox, getClosestPoses)
+{
+  traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0);
+  geometry_msgs::msg::Pose pose0;
+  geometry_msgs::msg::Pose pose1 = makePose(5.0, 5.0);
+
+  {
+    const auto actual = math::geometry::getClosestPoses(pose0, bbox, pose1, bbox);
+    ASSERT_TRUE(actual);
+
+    geometry_msgs::msg::Pose expected_pose0 = makePose(0.5, 0.5);
+    geometry_msgs::msg::Pose expected_pose1 = makePose(4.5, 4.5);
+    EXPECT_POSE_EQ(actual.value().first, expected_pose1);
+    EXPECT_POSE_EQ(actual.value().second, expected_pose0);
+  }
+
+  {  // reverse order
+    const auto actual = math::geometry::getClosestPoses(pose1, bbox, pose0, bbox);
+    ASSERT_TRUE(actual);
+
+    geometry_msgs::msg::Pose expected_pose0 = makePose(0.5, 0.5);
+    geometry_msgs::msg::Pose expected_pose1 = makePose(4.5, 4.5);
+    EXPECT_POSE_EQ(actual.value().first, expected_pose0);
+    EXPECT_POSE_EQ(actual.value().second, expected_pose1);
+  }
+}
+
+TEST(BoundingBox, getClosestPosesWithAlmostTouch)
+{
+  traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(1.0, 1.0, 1.0);
+  geometry_msgs::msg::Pose pose0;
+  traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 10.0, 1.0);
+  geometry_msgs::msg::Pose pose1 = makePose(1.1, 0.0);
+
+  {
+    const auto actual = math::geometry::getClosestPoses(pose0, bbox0, pose1, bbox1);
+    ASSERT_TRUE(actual);
+    const auto distance = math::geometry::getDistance(actual.value().first, actual.value().second);
+    EXPECT_NEAR(distance, 0.1, 0.0001);
+  }
+
+  {  // reverse order
+    const auto actual = math::geometry::getClosestPoses(pose1, bbox1, pose0, bbox0);
+    ASSERT_TRUE(actual);
+    const auto distance = math::geometry::getDistance(actual.value().first, actual.value().second);
+    EXPECT_NEAR(distance, 0.1, 0.0001);
+  }
+}
+
+TEST(BoundingBox, getClosestPosesWithIntersection)
+{
+  traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0);
+  geometry_msgs::msg::Pose pose0;
+  geometry_msgs::msg::Pose pose1 = makePose(0.6, 0.6);
+
+  const auto actual = math::geometry::getClosestPoses(pose0, bbox, pose1, bbox);
+  ASSERT_FALSE(actual);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);