Merge branch 'main' into francocipollone/poc_line_string_kd_tree

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
maliput · Feb 13, 2023 · 6018eab · 6018eab
2 parents cf00b78 + 1e5e203
commit 6018eab
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 5 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/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.}};