diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h index 7f26905..f213fe1 100644 --- a/include/maliput_sparse/geometry/line_string.h +++ b/include/maliput_sparse/geometry/line_string.h @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -186,6 +187,21 @@ class LineString final { /// @throws maliput::common::assertion_error When there are less than two points. template 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 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::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 index 8cb6ebe..d9bce60 100644 --- 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{p1, p2, p3, p3, p2, p1, p1, p2}); + const std::vector 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{p1, p2, p3}); const auto segments = dut.segments(); diff --git a/test/geometry/utility/geometry_test.cc b/test/geometry/utility/geometry_test.cc index 2e11561..bedc662 100644 --- 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::infinity()}; const LineString3d kOnlyZ{{0., 0., 0.}, {0., 0., 100.}, {0., 0., 0.}};