Merge branch 'francocipollone/poc_line_string_kd_tree' into francocip…

…ollone/use_closest_point_for_2d_projection_case
maliput · Feb 13, 2023 · 312aa01 · 312aa01
2 parents ea54b92 + 6018eab
commit 312aa01
Show file tree

Hide file tree

Showing 4 changed files with 46 additions and 27 deletions.
diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
@@ -35,6 +35,7 @@
 #include <map>
 #include <vector>
 
+#include <maliput/common/logger.h>
 #include <maliput/common/maliput_throw.h>
 #include <maliput/math/kd_tree.h>
 #include <maliput/math/vector.h>
@@ -186,6 +187,21 @@ class LineString final {
   /// @throws maliput::common::assertion_error When there are less than two points.
   template <typename Iterator>
   LineString(Iterator begin, Iterator end) : coordinates_(begin, end) {
+    // Remove consecutive points that are numerically the same.
+    // Duplicated points creates zero length segments, which leads to a wrong lookup when querying the segment map.
+    std::vector<std::size_t> remove_idx;
+    for (std::size_t idx{}; idx < coordinates_.size() - 1; ++idx) {
+      const double segment_length = DistanceFunction()(coordinates_[idx], coordinates_[idx + 1]);
+      if (segment_length <= std::numeric_limits<double>::epsilon()) {
+        maliput::log()->warn("LineString: consecutive points are numerically the same, removing duplicated point: {}",
+                             coordinates_[idx + 1]);
+        remove_idx.push_back(idx + 1);
+      }
+    }
+    // Remove the duplicated points.
+    for (auto it = remove_idx.rbegin(); it != remove_idx.rend(); ++it) {
+      coordinates_.erase(coordinates_.begin() + *it);
+    }
     MALIPUT_THROW_UNLESS(coordinates_.size() > 1);
     // Fill up the segments collection and the points collection.
     double p = 0;

diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
@@ -361,39 +361,38 @@ ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const mali
   // If the idx is the first or last then obtain the first or last segment.
   // Otherwise, obtain the segment that contains the nearest point.
   MALIPUT_THROW_UNLESS(nearest_point.idx() != std::nullopt);
-  if (nearest_point.idx().value() == 0) {
-    closest_segment = line_string.segments().begin()->second;
-    segment_closest_point_result = GetClosestPointToSegment(line_string[closest_segment->idx_start],
-                                                            line_string[closest_segment->idx_end], xyz, tolerance);
-  } else if (nearest_point.idx().value() == line_string.size() - 1) {
-    closest_segment = (--line_string.segments().end())->second;
+  const std::size_t nearest_idx = nearest_point.idx().value();
+  if (nearest_idx == 0 || nearest_idx == line_string.size() - 1) {
+    closest_segment =
+        nearest_idx == 0 ? line_string.segments().begin()->second : (--line_string.segments().end())->second;
     segment_closest_point_result = GetClosestPointToSegment(line_string[closest_segment->idx_start],
                                                             line_string[closest_segment->idx_end], xyz, tolerance);
   } else {
     // The closest segment will be the one that contains the nearest point, whether it is the start or end point of the
     // segment.
-    const maliput::math::Vector3& nearest_coordinate{line_string[nearest_point.idx().value()]};
-    const maliput::math::Vector3& previous_nearest_point{line_string[nearest_point.idx().value() - 1]};
-    const maliput::math::Vector3& next_nearest_point{line_string[nearest_point.idx().value() + 1]};
+    const maliput::math::Vector3& nearest_coordinate{line_string[nearest_idx]};
+    const maliput::math::Vector3& previous_nearest_point{line_string[nearest_idx - 1]};
+    const maliput::math::Vector3& next_nearest_point{line_string[nearest_idx + 1]};
 
     const auto previous_segment_closest_point_res =
         GetClosestPointToSegment(previous_nearest_point, nearest_coordinate, xyz, tolerance);
     const auto next_segment_closest_point_res =
         GetClosestPointToSegment(nearest_coordinate, next_nearest_point, xyz, tolerance);
     MALIPUT_THROW_UNLESS(nearest_point.p() != std::nullopt);
-    if (previous_segment_closest_point_res.distance < next_segment_closest_point_res.distance) {
-      closest_segment = LineString3d::Segment{
-          nearest_point.idx().value() - 1, nearest_point.idx().value(),
-          LineString3d::Segment::Interval{line_string_points.at(nearest_point.idx().value() - 1).p().value(),
-                                          nearest_point.p().value()}};
-      segment_closest_point_result = previous_segment_closest_point_res;
-    } else {
-      closest_segment = LineString3d::Segment{
-          nearest_point.idx().value(), nearest_point.idx().value() + 1,
-          LineString3d::Segment::Interval{nearest_point.p().value(),
-                                          line_string_points.at(nearest_point.idx().value() + 1).p().value()}};
-      segment_closest_point_result = next_segment_closest_point_res;
-    }
+
+    // Compares the distance between the nearest point and the closest point to the previous segment and the next
+    // segment.
+    segment_closest_point_result = previous_segment_closest_point_res.distance < next_segment_closest_point_res.distance
+                                       ? previous_segment_closest_point_res
+                                       : next_segment_closest_point_res;
+    closest_segment =
+        previous_segment_closest_point_res.distance < next_segment_closest_point_res.distance
+            ? LineString3d::Segment{nearest_idx - 1, nearest_idx,
+                                    LineString3d::Segment::Interval{line_string_points.at(nearest_idx - 1).p().value(),
+                                                                    nearest_point.p().value()}}
+            : LineString3d::Segment{nearest_idx, nearest_idx + 1,
+                                    LineString3d::Segment::Interval{line_string_points.at(nearest_idx).p().value(),
+                                                                    nearest_point.p().value()}};
   }
   return {segment_closest_point_result.p + closest_segment->p_interval.min, segment_closest_point_result.point,
           segment_closest_point_result.distance, closest_segment.value()};

diff --git a/test/geometry/line_string_test.cc b/test/geometry/line_string_test.cc
@@ -193,6 +193,15 @@ TEST_F(LineString3dTest, Api) {
   EXPECT_NEAR(2. * std::sqrt(2.), dut.length(), kTolerance);
 }
 
+TEST_F(LineString3dTest, SameConsecutivePoints) {
+  const LineString3d dut(std::vector<Vector3>{p1, p2, p3, p3, p2, p1, p1, p2});
+  const std::vector<Vector3> expected_points{p1, p2, p3, p2, p1, p2};
+  ASSERT_EQ(expected_points.size(), dut.size());
+  for (size_t i = 0; i < dut.size() - 1; ++i) {
+    EXPECT_EQ(expected_points.at(i), dut.at(i));
+  }
+}
+
 TEST_F(LineString3dTest, Segments) {
   const LineString3d dut(std::vector<Vector3>{p1, p2, p3});
   const auto segments = dut.segments();

diff --git a/test/geometry/utility/geometry_test.cc b/test/geometry/utility/geometry_test.cc
@@ -330,11 +330,6 @@ class GetSlopeAtPSpecialCasesTest : public testing::Test {
   static constexpr double kTolerance{1e-12};
 };
 
-TEST_F(GetSlopeAtPSpecialCasesTest, Throw) {
-  const LineString3d kNoLength{{0., 0., 0.}, {0., 0., 0.}};
-  EXPECT_THROW(GetSlopeAtP(kNoLength, 0., kTolerance), maliput::common::assertion_error);
-}
-
 TEST_F(GetSlopeAtPSpecialCasesTest, InfinitySlope) {
   const double inf{std::numeric_limits<double>::infinity()};
   const LineString3d kOnlyZ{{0., 0., 0.}, {0., 0., 100.}, {0., 0., 0.}};