Skips segment creation when it is zero length.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
maliput · Feb 7, 2023 · 8d2d32b · 8d2d32b
1 parent 4b03a2b
commit 8d2d32b
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
@@ -153,6 +153,11 @@ class LineString final {
     double p = 0;
     for (std::size_t idx{}; idx < coordinates_.size() - 1; ++idx) {
       const double segment_length = DistanceFunction()(coordinates_[idx], coordinates_[idx + 1]);
+      // Add the segment. If the points are numerically the same, do not add the segment to avoid
+      // a wrong lookup when querying the segment map.
+      if (segment_length <= std::numeric_limits<double>::epsilon()) {
+        continue;
+      }
       const typename Segment::Interval interval{p, p + segment_length};
       segments_.emplace(interval, Segment{idx, idx + 1, interval});
       p += segment_length;

diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
@@ -306,8 +306,8 @@ double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance)
 template <typename CoordinateT>
 BoundPointsResult GetBoundPointsAtP(const LineString<CoordinateT>& line_string, double p, double tolerance) {
   p = maliput::common::RangeValidator::GetAbsoluteEpsilonValidator(0., line_string.length(), tolerance, kEpsilon)(p);
-  const auto segment = line_string.segments().at({p});
-  return {segment.idx_start, segment.idx_end, segment.p_interval.min};
+  const auto segment_itr = line_string.segments().find({p});
+  return {segment_itr->second.idx_start, segment_itr->second.idx_end, segment_itr->second.p_interval.min};
 }
 
 double Get2DHeadingAtP(const LineString3d& line_string, double p, double tolerance) {