From 1652b4c1faace67e2fe3fd631730c2afd08ca41a Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Wed, 1 Feb 2023 21:12:17 -0300
Subject: [PATCH 01/12] Implements segments interval.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/maliput_sparse/geometry/line_string.h | 58 +++++++++--
 src/geometry/utility/geometry.cc              | 17 +---
 test/geometry/line_string_test.cc             | 96 +++++++++++++++++++
 3 files changed, 147 insertions(+), 24 deletions(-)

diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
index 85ec037..71ff404 100644
--- a/include/maliput_sparse/geometry/line_string.h
+++ b/include/maliput_sparse/geometry/line_string.h
@@ -32,6 +32,7 @@
 #include <cmath>
 #include <initializer_list>
 #include <iterator>
+#include <map>
 #include <vector>
 
 #include <maliput/common/maliput_throw.h>
@@ -74,6 +75,39 @@ class LineString final {
   using iterator = typename std::vector<CoordinateT>::iterator;
   using const_iterator = typename std::vector<CoordinateT>::const_iterator;
 
+  struct Segment {
+    struct Interval {
+      /// Creates a Interval.
+      /// @param min_in Is the minimum value of the interval.
+      /// @param max_in Is the maximum value of the interval.
+      /// @throw maliput::common::assertion_error When `min_in` is greater than `max_in`.
+      Interval(double min_in, double max_in) : min(min_in), max(max_in) { MALIPUT_THROW_UNLESS(min_in <= max_in); }
+
+      /// Creates a Interval where
+      /// the minimum value is equal to the maximum value.
+      /// @param min_max Is the minimum and maximum value of the interval.
+      Interval(double min_max) : min(min_max), max(min_max) {}
+
+      // Less than operator.
+      bool operator<(const Interval& rhs) const {
+        if (min < rhs.min) {
+          return max <= rhs.max ? true : false;
+        } else {
+          return false;
+        }
+      }
+
+      const double min{};
+      const double max{};
+    };
+
+    const const_iterator start;
+    const const_iterator end;
+    const Segment::Interval p_interval;
+  };
+
+  using Segments = std::map<typename Segment::Interval, Segment>;
+
   /// Constructs a LineString from a std::vector.
   ///
   /// This function calls LineString(coordinates.begin, coordinates.end)
@@ -102,7 +136,15 @@ class LineString final {
   template <typename Iterator>
   LineString(Iterator begin, Iterator end) : coordinates_(begin, end) {
     MALIPUT_THROW_UNLESS(coordinates_.size() > 1);
-    length_ = ComputeLength();
+    // Fill up the segments collection
+    double p = 0;
+    for (auto it = coordinates_.cbegin(); it != coordinates_.cend() - 1; ++it) {
+      const double segment_length = DistanceFunction()(*it, *(it + 1));
+      const typename Segment::Interval interval{p, p + segment_length};
+      segments_.emplace(interval, Segment{it, it + 1, interval});
+      p += segment_length;
+    }
+    length_ = p;
   }
 
   /// @return The first point in the LineString.
@@ -121,6 +163,10 @@ class LineString final {
   /// @return The accumulated length between consecutive points in this LineString by means of DistanceFunction.
   double length() const { return length_; }
 
+  /// Return the segments of this LineString.
+  /// @return A vector of segments.
+  const Segments& segments() const { return segments_; }
+
   /// @returns begin iterator of the underlying collection.
   iterator begin() { return coordinates_.begin(); }
   /// @returns begin const iterator of the underlying collection.
@@ -139,16 +185,8 @@ class LineString final {
   }
 
  private:
-  // @return The accumulated Length of this LineString.
-  const double ComputeLength() const {
-    double accumulated_{0.};
-    for (size_t i = 0; i < size() - 1; ++i) {
-      accumulated_ += DistanceFunction()(at(i), at(i + 1));
-    }
-    return accumulated_;
-  }
-
   std::vector<CoordinateT> coordinates_{};
+  Segments segments_{};
   double length_{};
 };
 
diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
index 04c761e..202df37 100644
--- a/src/geometry/utility/geometry.cc
+++ b/src/geometry/utility/geometry.cc
@@ -314,20 +314,9 @@ double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance)
 template <typename CoordinateT>
 BoundPointsResult<CoordinateT> GetBoundPointsAtP(const LineString<CoordinateT>& line_string, double p,
                                                  double tolerance) {
-  maliput::common::RangeValidator::GetAbsoluteEpsilonValidator(0., line_string.length(), tolerance, kEpsilon)(p);
-  BoundPointsResult<CoordinateT> result;
-  double current_cumulative_length = 0.0;
-  for (auto first = line_string.begin(), second = std::next(line_string.begin()); second != line_string.end();
-       ++first, ++second) {
-    const auto p1 = *first;
-    const auto p2 = *second;
-    const double current_length = (p1 - p2).norm();
-    if (current_cumulative_length + current_length >= p) {
-      return {first, second, current_cumulative_length};
-    }
-    current_cumulative_length += current_length;
-  }
-  return {line_string.end() - 1, line_string.end() - 2, line_string.length()};
+  p = maliput::common::RangeValidator::GetAbsoluteEpsilonValidator(0., line_string.length(), tolerance, kEpsilon)(p);
+  const auto segment = line_string.segments().at(typename LineString<CoordinateT>::Segment::Interval{p});
+  return {segment.start, segment.end, segment.p_interval.min};
 }
 
 double Get2DHeadingAtP(const LineString3d& line_string, double p, double tolerance) {
diff --git a/test/geometry/line_string_test.cc b/test/geometry/line_string_test.cc
index 2a9b07f..7ba7f5a 100644
--- a/test/geometry/line_string_test.cc
+++ b/test/geometry/line_string_test.cc
@@ -54,6 +54,81 @@ struct SquaredDistanceFunction {
   double operator()(const VectorT& v1, const VectorT& v2) const { return std::pow((v1 - v2).norm(), 2.); }
 };
 
+class SegmentIntervalTest : public ::testing::Test {};
+
+TEST_F(SegmentIntervalTest, Constructors) {
+  EXPECT_NO_THROW((LineString3d::Segment::Interval{0., 1.}));
+  EXPECT_THROW((LineString3d::Segment::Interval{1., 0.}), maliput::common::assertion_error);
+}
+
+TEST_F(SegmentIntervalTest, ValuesUsing2ArgsConstructor) {
+  const LineString3d::Segment::Interval dut(0., 1.);
+  EXPECT_EQ(0., dut.min);
+  EXPECT_EQ(1., dut.max);
+}
+
+TEST_F(SegmentIntervalTest, ValuesUsing1ArgsConstructor) {
+  const LineString3d::Segment::Interval dut(10.);
+  EXPECT_EQ(10., dut.min);
+  EXPECT_EQ(10., dut.max);
+}
+
+TEST_F(SegmentIntervalTest, OperatorLessThan) {
+  const LineString3d::Segment::Interval dut(0., 10.);
+  // True because dut's min is less than the other's min.
+  EXPECT_TRUE(dut < LineString3d::Segment::Interval(5., 15.));
+  // False because dut's min is greater than the other's max.
+  EXPECT_FALSE(dut < LineString3d::Segment::Interval(-5., 0.));
+  // True because both p are greater than dut's.
+  EXPECT_TRUE(dut < LineString3d::Segment::Interval(15., 20.));
+  // False because the intervals are equal.
+  EXPECT_FALSE(dut < LineString3d::Segment::Interval(0., 10.));
+}
+
+TEST_F(SegmentIntervalTest, Map) {
+  std::map<LineString3d::Segment::Interval, int> map{
+      {LineString3d::Segment::Interval(0., 10.), 0},  {LineString3d::Segment::Interval(10., 20.), 1},
+      {LineString3d::Segment::Interval(20., 30.), 2}, {LineString3d::Segment::Interval(30., 40.), 3},
+      {LineString3d::Segment::Interval(40., 50.), 4}, {LineString3d::Segment::Interval(50., 60.), 5},
+      {LineString3d::Segment::Interval(60., 70.), 6}, {LineString3d::Segment::Interval(70., 80.), 7},
+      {LineString3d::Segment::Interval(80., 90.), 8}, {LineString3d::Segment::Interval(90., 100.), 9},
+  };
+  EXPECT_EQ(0, map.at(LineString3d::Segment::Interval(0.)));
+  EXPECT_EQ(0, map.at(LineString3d::Segment::Interval(5.)));
+  EXPECT_EQ(1, map.at(LineString3d::Segment::Interval(10.)));
+  EXPECT_EQ(1, map.at(LineString3d::Segment::Interval(15.)));
+  EXPECT_EQ(2, map.at(LineString3d::Segment::Interval(20.)));
+}
+
+class SegmentTest : public ::testing::Test {
+ public:
+  const std::vector<maliput::math::Vector3> points_3d_ = {
+      maliput::math::Vector3(0., 0., 0.),
+      maliput::math::Vector3(10., 0., 0.),
+      maliput::math::Vector3(20., 0., 0.),
+  };
+  LineString3d::Segment::Interval interval_1{0., 10.};
+  LineString3d::Segment::Interval interval_2{10., 20.};
+};
+
+TEST_F(SegmentTest, Constructor) {
+  EXPECT_NO_THROW((LineString3d::Segment{points_3d_.begin(), points_3d_.begin() + 1, interval_1}));
+}
+
+TEST_F(SegmentTest, Values) {
+  const LineString3d::Segment dut_1{points_3d_.begin(), points_3d_.begin() + 1, interval_1};
+  EXPECT_EQ(points_3d_.begin(), dut_1.start);
+  EXPECT_EQ(points_3d_.begin() + 1, dut_1.end);
+  EXPECT_EQ(interval_1.min, dut_1.p_interval.min);
+  EXPECT_EQ(interval_1.max, dut_1.p_interval.max);
+
+  const LineString3d::Segment dut_2{points_3d_.begin() + 1, points_3d_.end(), interval_2};
+  EXPECT_EQ(points_3d_.begin() + 1, dut_2.start);
+  EXPECT_EQ(points_3d_.end(), dut_2.end);
+  EXPECT_EQ(interval_2.min, dut_2.p_interval.min);
+  EXPECT_EQ(interval_2.max, dut_2.p_interval.max);
+}
+
 class LineString3dTest : public ::testing::Test {
  public:
   const Vector3 p1{Vector3::UnitX()};
@@ -85,6 +160,27 @@ TEST_F(LineString3dTest, Api) {
   EXPECT_NEAR(2. * std::sqrt(2.), dut.length(), kTolerance);
 }
 
+TEST_F(LineString3dTest, Segments) {
+  const LineString3d dut(std::vector<Vector3>{p1, p2, p3});
+  const auto segments = dut.segments();
+  ASSERT_EQ(static_cast<size_t>(2), segments.size());
+
+  const double p_in_p1_p2 = (p1 - p2).norm() / 2.;
+  const double p_in_p2_p3 = (p1 - p2).norm() + (p2 - p3).norm() / 2.;
+
+  const auto segment_p1_p2 = segments.at(LineString3d::Segment::Interval(p_in_p1_p2));
+  EXPECT_TRUE(p1 == *segment_p1_p2.start);
+  EXPECT_TRUE(p2 == *segment_p1_p2.end);
+  EXPECT_NEAR(0., segment_p1_p2.p_interval.min, kTolerance);
+  EXPECT_NEAR((p1 - p2).norm(), segment_p1_p2.p_interval.max, kTolerance);
+
+  const auto segment_p2_p3 = segments.at(LineString3d::Segment::Interval(p_in_p2_p3));
+  EXPECT_TRUE(p2 == *segment_p2_p3.start);
+  EXPECT_TRUE(p3 == *segment_p2_p3.end);
+  EXPECT_NEAR((p1 - p2).norm(), segment_p2_p3.p_interval.min, kTolerance);
+  EXPECT_NEAR((p1 - p2).norm() + (p2 - p3).norm(), segment_p2_p3.p_interval.max, kTolerance);
+}
+
 TEST_F(LineString3dTest, LengthInjectedDistanceFunction) {
   const LineString<Vector3, SquaredDistanceFunction<Vector3>> dut(std::vector<Vector3>{p1, p2, p3});
   EXPECT_NEAR(4., dut.length(), 1e-14);

From 20dabfbf7b7e1fa5f7602fde520478917836b956 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Thu, 2 Feb 2023 13:35:24 -0300
Subject: [PATCH 02/12] Adds some improvements.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/maliput_sparse/geometry/line_string.h | 10 +--
 .../geometry/utility/geometry.h               | 12 +---
 src/geometry/utility/geometry.cc              | 72 +++++++++----------
 test/base/lane_test.cc                        |  8 +++
 test/geometry/line_string_test.cc             | 24 +++----
 test/geometry/utility/geometry_test.cc        | 38 +++++-----
 6 files changed, 82 insertions(+), 82 deletions(-)

diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
index 71ff404..09ac279 100644
--- a/include/maliput_sparse/geometry/line_string.h
+++ b/include/maliput_sparse/geometry/line_string.h
@@ -101,8 +101,8 @@ class LineString final {
       const double max{};
     };
 
-    const const_iterator start;
-    const const_iterator end;
+    const std::size_t idx_start;
+    const std::size_t idx_end;
     const Segment::Interval p_interval;
   };
 
@@ -138,10 +138,10 @@ class LineString final {
     MALIPUT_THROW_UNLESS(coordinates_.size() > 1);
     // Fill up the segments collection
     double p = 0;
-    for (auto it = coordinates_.cbegin(); it != coordinates_.cend() - 1; ++it) {
-      const double segment_length = DistanceFunction()(*it, *(it + 1));
+    for (std::size_t idx{}; idx < coordinates_.size() - 1; ++idx) {
+      const double segment_length = DistanceFunction()(coordinates_[idx], coordinates_[idx + 1]);
       const typename Segment::Interval interval{p, p + segment_length};
-      segments_.emplace(interval, Segment{it, it + 1, interval});
+      segments_.emplace(interval, Segment{idx, idx + 1, interval});
       p += segment_length;
     }
     length_ = p;
diff --git a/include/maliput_sparse/geometry/utility/geometry.h b/include/maliput_sparse/geometry/utility/geometry.h
index 552889e..d7548de 100644
--- a/include/maliput_sparse/geometry/utility/geometry.h
+++ b/include/maliput_sparse/geometry/utility/geometry.h
@@ -38,18 +38,13 @@ namespace geometry {
 namespace utility {
 
 /// Holds the result of #GetBoundPointsAtP method.
-/// @tparam CoordinateT The coordinate type.
-template <typename CoordinateT>
 struct BoundPointsResult {
-  typename LineString<CoordinateT>::const_iterator first;
-  typename LineString<CoordinateT>::const_iterator second;
+  std::size_t idx_start;
+  std::size_t idx_end;
   // Length up to first.
   double length;
 };
 
-using BoundPointsResult3d = BoundPointsResult<maliput::math::Vector3>;
-using BoundPointsResult2d = BoundPointsResult<maliput::math::Vector2>;
-
 /// Holds the result of #GetClosestPoint method.
 /// @tparam CoordinateT The coordinate type.
 template <typename CoordinateT>
@@ -101,8 +96,7 @@ double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance);
 /// @param tolerance tolerance.
 /// @throws maliput::common::assertion_error When `p ∉ [0., line_string.length()]`.
 template <typename CoordinateT = maliput::math::Vector3>
-BoundPointsResult<CoordinateT> GetBoundPointsAtP(const LineString<CoordinateT>& line_string, double p,
-                                                 double tolerance);
+BoundPointsResult GetBoundPointsAtP(const LineString<CoordinateT>& line_string, double p, double tolerance);
 
 /// Returns the heading of a @p line_string for a given @p p .
 /// @param line_string LineString to be computed the heading from.
diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
index 202df37..0df91df 100644
--- a/src/geometry/utility/geometry.cc
+++ b/src/geometry/utility/geometry.cc
@@ -293,49 +293,51 @@ CoordinateT InterpolatedPointAtP(const LineString<CoordinateT>& line_string, dou
   // https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/include/lanelet2_core/geometry/impl/LineString.h#L618
   if (p < 0) return line_string.first();
   if (p >= line_string.length()) return line_string.last();
-  const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance);
-  const double partial_length{(*line_string_points_length.first - *line_string_points_length.second).norm()};
-  const double remaining_distance = p - line_string_points_length.length;
+  const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance);
+  const CoordinateT& start = line_string[bound_points.idx_start];
+  const CoordinateT& end = line_string[bound_points.idx_end];
+  const CoordinateT d_segment{end - start};
+  const double remaining_distance = p - bound_points.length;
   if (remaining_distance < kEpsilon) {
-    return *line_string_points_length.first;
+    return start;
   }
-  return *line_string_points_length.first +
-         remaining_distance / partial_length * (*line_string_points_length.second - *line_string_points_length.first);
+  return start + d_segment.normalized() * remaining_distance;
 }
 
 double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance) {
   const BoundPointsResult bound_points = GetBoundPointsAtP(line_string, p, tolerance);
-  const double dist{(To2D(*bound_points.second) - To2D(*bound_points.first)).norm()};
-  const double delta_z{bound_points.second->z() - bound_points.first->z()};
-  MALIPUT_THROW_UNLESS(*bound_points.second != *bound_points.first);
+  const maliput::math::Vector3& start = line_string[bound_points.idx_start];
+  const maliput::math::Vector3& end = line_string[bound_points.idx_end];
+  const double delta_z{end.z() - start.z()};
+  const double dist{(To2D(end) - To2D(start)).norm()};
+  MALIPUT_THROW_UNLESS(start != end);
   return delta_z / dist;
 }
 
 template <typename CoordinateT>
-BoundPointsResult<CoordinateT> GetBoundPointsAtP(const LineString<CoordinateT>& line_string, double p,
-                                                 double tolerance) {
+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(typename LineString<CoordinateT>::Segment::Interval{p});
-  return {segment.start, segment.end, segment.p_interval.min};
+  return {segment.idx_start, segment.idx_end, segment.p_interval.min};
 }
 
 double Get2DHeadingAtP(const LineString3d& line_string, double p, double tolerance) {
-  const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance);
-  const Vector3 heading_vector{*line_string_points_length.second - *line_string_points_length.first};
+  const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance);
+  const Vector3 heading_vector{line_string[bound_points.idx_end] - line_string[bound_points.idx_start]};
   return std::atan2(heading_vector.y(), heading_vector.x());
 }
 
 Vector2 Get2DTangentAtP(const LineString3d& line_string, double p, double tolerance) {
   // const double heading = Get2DHeadingAtP(line_string, p);
   // return {std::cos(heading), std::sin(heading)};
-  const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance);
-  const Vector2 d_xy{To2D(*line_string_points_length.second) - To2D(*line_string_points_length.first)};
+  const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance);
+  const Vector2 d_xy{To2D(line_string[bound_points.idx_end]) - To2D(line_string[bound_points.idx_start])};
   return (d_xy / (To2D(line_string).length())).normalized();
 }
 
 Vector3 GetTangentAtP(const LineString3d& line_string, double p, double tolerance) {
-  const auto line_string_points_length = GetBoundPointsAtP(line_string, p, tolerance);
-  const Vector3 d_xyz{*line_string_points_length.second - *line_string_points_length.first};
+  const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance);
+  const Vector3 d_xyz{line_string[bound_points.idx_end] - line_string[bound_points.idx_start]};
   return (d_xyz / (line_string.length())).normalized();
 }
 
@@ -343,11 +345,14 @@ template <typename CoordinateT>
 ClosestPointResult<CoordinateT> GetClosestPointToSegment(const std::pair<CoordinateT, CoordinateT>& segment,
                                                          const CoordinateT& coordinate, double tolerance) {
   const CoordinateT d_segment{segment.second - segment.first};
+  const CoordinateT d_segment_normalized{d_segment.normalized()};
   const CoordinateT d_coordinate_to_first{coordinate - segment.first};
 
-  const double unsaturated_p = d_coordinate_to_first.dot(d_segment.normalized());
+  const double unsaturated_p = d_coordinate_to_first.dot(d_segment_normalized);
   const double p = std::clamp(unsaturated_p, 0., d_segment.norm());
-  const CoordinateT point = InterpolatedPointAtP(LineString<CoordinateT>{segment.first, segment.second}, p, tolerance);
+
+  // point at p
+  const CoordinateT point = p * d_segment_normalized + segment.first;
   const double distance = (coordinate - point).norm();
   return {p, point, distance};
 }
@@ -380,23 +385,24 @@ ClosestPointResult3d GetClosestPointUsing2dProjection(const LineString3d& line_s
        ++first, ++second) {
     // If points are under numeric tolerance, skip segment.
     if ((*first - *second).norm() < kEpsilon) continue;
-    const maliput::math::Vector2 first_2d = To2D(*first);
-    const maliput::math::Vector2 second_2d = To2D(*second);
+    const double segment_3d_length = (*second - *first).norm();
     const maliput::math::Vector2 xy = To2D(xyz);
-    const Segment2d segment_2d{first_2d, second_2d};
+    const Segment2d segment_2d{To2D(*first), To2D(*second)};
     const auto closest_point_res = GetClosestPointToSegment(segment_2d, xy, tolerance);
     if (closest_point_res.distance < result.distance) {
-      const LineString3d segment_linestring_3d{*first, *second};
-      const double scale_p = segment_linestring_3d.length() / (segment_2d.first - segment_2d.second).norm();
-      const maliput::math::Vector3 closest_point_3d{
-          closest_point_res.point.x(), closest_point_res.point.y(),
-          InterpolatedPointAtP(segment_linestring_3d, closest_point_res.p * scale_p, tolerance).z()};
+      // Retrieve the z coordinate from the 3d line string.
+      const double scale_p = segment_3d_length / (segment_2d.first - segment_2d.second).norm();
+      const double p_3d = closest_point_res.p * scale_p;
+      const double z_coordinate = first->z() + ((*second - *first).normalized() * p_3d).z();
+      // Compound closest point in 3d.
+      const maliput::math::Vector3 closest_point_3d{closest_point_res.point.x(), closest_point_res.point.y(),
+                                                    z_coordinate};
 
       result.distance = (xyz - closest_point_3d).norm();
       result.point = closest_point_3d;
-      result.p = length + closest_point_res.p * scale_p;
+      result.p = length + p_3d;
     }
-    length += (*second - *first).norm();  //> Adds segment length
+    length += segment_3d_length;  //> Adds segment length
   }
   return result;
 }
@@ -414,18 +420,12 @@ double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double
 
 // Explicit instantiations
 
-template BoundPointsResult3d GetBoundPointsAtP(const LineString3d&, double, double);
-template BoundPointsResult2d GetBoundPointsAtP(const LineString2d&, double, double);
-
 template maliput::math::Vector3 InterpolatedPointAtP(const LineString3d&, double, double);
 template maliput::math::Vector2 InterpolatedPointAtP(const LineString2d&, double, double);
 
 template ClosestPointResult3d GetClosestPointToSegment(const Segment3d&, const maliput::math::Vector3&, double);
 template ClosestPointResult2d GetClosestPointToSegment(const Segment2d&, const maliput::math::Vector2&, double);
 
-template class BoundPointsResult<maliput::math::Vector3>;
-template class BoundPointsResult<maliput::math::Vector2>;
-
 template class ClosestPointResult<maliput::math::Vector3>;
 template class ClosestPointResult<maliput::math::Vector2>;
 
diff --git a/test/base/lane_test.cc b/test/base/lane_test.cc
index 15867ef..2bbe659 100644
--- a/test/base/lane_test.cc
+++ b/test/base/lane_test.cc
@@ -95,12 +95,14 @@ std::vector<LaneTestCase> LaneTestCases() {
               {
                   {50., 0., 0.},
                   {50., -2., -2.},
+                  {100. + (102. * std::sqrt(2.) / 2.), 0., 0.},
                   {100. + 102. * std::sqrt(2.), 0., 0.},
                   {100. + 102. * std::sqrt(2.) + 98., -1., 4.},
               } /* srh */,
               {
                   {50., 0., 100.},
                   {50., -2., 98.},
+                  {151., 51., 100.},
                   {202., 102., 100.},
                   {203., 200., 104.},
               } /* expected_backend_pos */,
@@ -109,6 +111,7 @@ std::vector<LaneTestCase> LaneTestCases() {
                   Rotation::FromRpy(0., 0., 0.),
                   Rotation::FromRpy(0., 0., M_PI / 4.),
                   Rotation::FromRpy(0., 0., M_PI / 2.),
+                  Rotation::FromRpy(0., 0., M_PI / 2.),
               } /* expected_rotation */,
               100. + 102. * std::sqrt(2.) + 98. /* expected_length */,
               {{
@@ -117,6 +120,11 @@ std::vector<LaneTestCase> LaneTestCases() {
                {
                    {50., -2., -2.} /* lane_position */, {50., -2., 98.} /* nearest_position */, 0. /* distance */
                },
+               {
+                   {100. + (102. * std::sqrt(2.) / 2.), 0., 0.} /* lane_position */,
+                   {151., 51., 100.} /* nearest_position */,
+                   0. /* distance */
+               },
                {
                    {100. + 102. * std::sqrt(2.), 0., 0.} /* lane_position */,
                    {202., 102., 100.} /* nearest_position */,
diff --git a/test/geometry/line_string_test.cc b/test/geometry/line_string_test.cc
index 7ba7f5a..603d116 100644
--- a/test/geometry/line_string_test.cc
+++ b/test/geometry/line_string_test.cc
@@ -111,20 +111,18 @@ class SegmentTest : public ::testing::Test {
   LineString3d::Segment::Interval interval_2{10., 20.};
 };
 
-TEST_F(SegmentTest, Constructor) {
-  EXPECT_NO_THROW((LineString3d::Segment{points_3d_.begin(), points_3d_.begin() + 1, interval_1}));
-}
+TEST_F(SegmentTest, Constructor) { EXPECT_NO_THROW((LineString3d::Segment{0, 1, interval_1})); }
 
 TEST_F(SegmentTest, Values) {
-  const LineString3d::Segment dut_1{points_3d_.begin(), points_3d_.begin() + 1, interval_1};
-  EXPECT_EQ(points_3d_.begin(), dut_1.start);
-  EXPECT_EQ(points_3d_.begin() + 1, dut_1.end);
+  const LineString3d::Segment dut_1{0, 1, interval_1};
+  EXPECT_EQ(static_cast<std::size_t>(0), dut_1.idx_start);
+  EXPECT_EQ(static_cast<std::size_t>(1), dut_1.idx_end);
   EXPECT_EQ(interval_1.min, dut_1.p_interval.min);
   EXPECT_EQ(interval_1.max, dut_1.p_interval.max);
 
-  const LineString3d::Segment dut_2{points_3d_.begin() + 1, points_3d_.end(), interval_2};
-  EXPECT_EQ(points_3d_.begin() + 1, dut_2.start);
-  EXPECT_EQ(points_3d_.end(), dut_2.end);
+  const LineString3d::Segment dut_2{1, 2, interval_2};
+  EXPECT_EQ(static_cast<std::size_t>(1), dut_2.idx_start);
+  EXPECT_EQ(static_cast<std::size_t>(2), dut_2.idx_end);
   EXPECT_EQ(interval_2.min, dut_2.p_interval.min);
   EXPECT_EQ(interval_2.max, dut_2.p_interval.max);
 }
@@ -169,14 +167,14 @@ TEST_F(LineString3dTest, Segments) {
   const double p_in_p2_p3 = (p1 - p2).norm() + (p2 - p3).norm() / 2.;
 
   const auto segment_p1_p2 = segments.at(LineString3d::Segment::Interval(p_in_p1_p2));
-  EXPECT_TRUE(p1 == *segment_p1_p2.start);
-  EXPECT_TRUE(p2 == *segment_p1_p2.end);
+  EXPECT_TRUE(p1 == dut[segment_p1_p2.idx_start]);
+  EXPECT_TRUE(p2 == dut[segment_p1_p2.idx_end]);
   EXPECT_NEAR(0., segment_p1_p2.p_interval.min, kTolerance);
   EXPECT_NEAR((p1 - p2).norm(), segment_p1_p2.p_interval.max, kTolerance);
 
   const auto segment_p2_p3 = segments.at(LineString3d::Segment::Interval(p_in_p2_p3));
-  EXPECT_TRUE(p2 == *segment_p2_p3.start);
-  EXPECT_TRUE(p3 == *segment_p2_p3.end);
+  EXPECT_TRUE(p2 == dut[segment_p2_p3.idx_start]);
+  EXPECT_TRUE(p3 == dut[segment_p2_p3.idx_end]);
   EXPECT_NEAR((p1 - p2).norm(), segment_p2_p3.p_interval.min, kTolerance);
   EXPECT_NEAR((p1 - p2).norm() + (p2 - p3).norm(), segment_p2_p3.p_interval.max, kTolerance);
 }
diff --git a/test/geometry/utility/geometry_test.cc b/test/geometry/utility/geometry_test.cc
index 2ce3d36..c37f994 100644
--- a/test/geometry/utility/geometry_test.cc
+++ b/test/geometry/utility/geometry_test.cc
@@ -242,34 +242,34 @@ class GetBoundPointsAtPTest : public ::testing::Test {
 TEST_F(GetBoundPointsAtPTest, Test) {
   {
     const double p{0.};
-    const BoundPointsResult3d expected_result{line_string.begin(), line_string.begin() + 1, 0.};
-    const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance);
-    EXPECT_EQ(expected_result.first, dut.first);
-    EXPECT_EQ(expected_result.second, dut.second);
+    const BoundPointsResult expected_result{0, 1, 0.};
+    const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance);
+    EXPECT_EQ(expected_result.idx_start, dut.idx_start);
+    EXPECT_EQ(expected_result.idx_end, dut.idx_end);
     EXPECT_EQ(expected_result.length, dut.length);
   }
   {
     const double p{7.5};
-    const BoundPointsResult3d expected_result{line_string.begin() + 1, line_string.begin() + 2, 7.};
-    const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance);
-    EXPECT_EQ(expected_result.first, dut.first);
-    EXPECT_EQ(expected_result.second, dut.second);
+    const BoundPointsResult expected_result{1, 2, 7.};
+    const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance);
+    EXPECT_EQ(expected_result.idx_start, dut.idx_start);
+    EXPECT_EQ(expected_result.idx_end, dut.idx_end);
     EXPECT_EQ(expected_result.length, dut.length);
   }
   {
     const double p{12.5};
-    const BoundPointsResult3d expected_result{line_string.begin() + 2, line_string.begin() + 3, 12.};
-    const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance);
-    EXPECT_EQ(expected_result.first, dut.first);
-    EXPECT_EQ(expected_result.second, dut.second);
+    const BoundPointsResult expected_result{2, 3, 12.};
+    const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance);
+    EXPECT_EQ(expected_result.idx_start, dut.idx_start);
+    EXPECT_EQ(expected_result.idx_end, dut.idx_end);
     EXPECT_EQ(expected_result.length, dut.length);
   }
   {
     const double p{19.5};
-    const BoundPointsResult3d expected_result{line_string.begin() + 3, line_string.begin() + 4, 19.};
-    const BoundPointsResult3d dut = GetBoundPointsAtP(line_string, p, kTolerance);
-    EXPECT_EQ(expected_result.first, dut.first);
-    EXPECT_EQ(expected_result.second, dut.second);
+    const BoundPointsResult expected_result{3, 4, 19.};
+    const BoundPointsResult dut = GetBoundPointsAtP(line_string, p, kTolerance);
+    EXPECT_EQ(expected_result.idx_start, dut.idx_start);
+    EXPECT_EQ(expected_result.idx_end, dut.idx_end);
     EXPECT_EQ(expected_result.length, dut.length);
   }
 }
@@ -291,19 +291,19 @@ std::vector<SlopeTestCase> SlopeTestCases() {
       {
           // LineString with different z values along y axis.
           LineString3d{{0., 0., 0.}, {5., 0., 5.}, {10., 0., 5.}, {15., 0., 10.}} /* line string*/,
-          {0., std::sqrt(2.) * 5, std::sqrt(2.) * 5 + 5., 2 * std::sqrt(2.) * 5 + 5.} /* ps */,
+          {0., std::sqrt(2.) * 5. / 2., std::sqrt(2.) * 5. + 5. / 2., 2 * std::sqrt(2.) * 5. + 5.} /* ps */,
           {1., 1., 0., 1.} /* expected_slopes */
       },
       {
           // LineString with different z values along x axis.
           LineString3d{{0., 0., 0.}, {0., 5., 5.}, {0., 10., 5.}, {0., 15., 10.}} /* line string*/,
-          {0., std::sqrt(2.) * 5, std::sqrt(2.) * 5 + 5., 2 * std::sqrt(2.) * 5 + 5.} /* ps */,
+          {0., std::sqrt(2.) * 5. / 2., std::sqrt(2.) * 5. + 5. / 2., 2 * std::sqrt(2.) * 5. + 5.} /* ps */,
           {1., 1., 0., 1.} /* expected_slopes */
       },
       {
           // LineString with different z values.
           LineString3d{{0., 0., 0.}, {6., 3., 2.}, {10., 6., 2.}, {16., 9., 4.}, {10., 6., 2.}} /* line string*/,
-          {0., 7., 12., 19., 26.} /* ps */,
+          {0., 3.5, 10.5, 17., 26.} /* ps */,
           {2. / std::sqrt(45), 2. / std::sqrt(45), 0., 2. / std::sqrt(45), -2. / std::sqrt(45)} /* expected_slopes */
       },
   };

From 9b0250d717a05f33ac9b7eec93d0adfbec92a83a Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Thu, 2 Feb 2023 16:42:37 -0300
Subject: [PATCH 03/12] Removes use of to 2d conversion in the Get2DTangentAtP
 method.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 src/geometry/utility/geometry.cc | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
index 0df91df..aad02c2 100644
--- a/src/geometry/utility/geometry.cc
+++ b/src/geometry/utility/geometry.cc
@@ -328,11 +328,8 @@ double Get2DHeadingAtP(const LineString3d& line_string, double p, double toleran
 }
 
 Vector2 Get2DTangentAtP(const LineString3d& line_string, double p, double tolerance) {
-  // const double heading = Get2DHeadingAtP(line_string, p);
-  // return {std::cos(heading), std::sin(heading)};
-  const auto bound_points = GetBoundPointsAtP(line_string, p, tolerance);
-  const Vector2 d_xy{To2D(line_string[bound_points.idx_end]) - To2D(line_string[bound_points.idx_start])};
-  return (d_xy / (To2D(line_string).length())).normalized();
+  const double heading = Get2DHeadingAtP(line_string, p, tolerance);
+  return {std::cos(heading), std::sin(heading)};
 }
 
 Vector3 GetTangentAtP(const LineString3d& line_string, double p, double tolerance) {

From 86e370220f58dd97201179fefa02f9db266250eb Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Thu, 2 Feb 2023 17:36:40 -0300
Subject: [PATCH 04/12] Use parallel for_each for the get closest point
 methods.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/maliput_sparse/geometry/line_string.h | 12 ++-
 src/geometry/utility/geometry.cc              | 97 ++++++++++---------
 2 files changed, 56 insertions(+), 53 deletions(-)

diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
index 09ac279..5ec0712 100644
--- a/include/maliput_sparse/geometry/line_string.h
+++ b/include/maliput_sparse/geometry/line_string.h
@@ -77,6 +77,8 @@ class LineString final {
 
   struct Segment {
     struct Interval {
+      // Interval() = default;
+
       /// Creates a Interval.
       /// @param min_in Is the minimum value of the interval.
       /// @param max_in Is the maximum value of the interval.
@@ -97,13 +99,13 @@ class LineString final {
         }
       }
 
-      const double min{};
-      const double max{};
+      double min{};
+      double max{};
     };
 
-    const std::size_t idx_start;
-    const std::size_t idx_end;
-    const Segment::Interval p_interval;
+    std::size_t idx_start;
+    std::size_t idx_end;
+    Segment::Interval p_interval;
   };
 
   using Segments = std::map<typename Segment::Interval, Segment>;
diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
index aad02c2..5c6190f 100644
--- a/src/geometry/utility/geometry.cc
+++ b/src/geometry/utility/geometry.cc
@@ -62,6 +62,7 @@
 #include <cmath>
 #include <numeric>
 
+#include <execution>
 #include <maliput/common/range_validator.h>
 
 namespace maliput_sparse {
@@ -84,14 +85,6 @@ Vector2 To2D(const Vector3& vector) { return {vector.x(), vector.y()}; }
 
 Segment2d To2D(const Segment3d& segment) { return {To2D(segment.first), To2D(segment.second)}; }
 
-LineString2d To2D(const LineString3d& line_string) {
-  std::vector<Vector2> points;
-  for (const auto& point : line_string) {
-    points.push_back(To2D(point));
-  }
-  return LineString2d{points};
-}
-
 // Determines whether two line segments intersects.
 //
 // Based on https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line_segment
@@ -356,52 +349,60 @@ ClosestPointResult<CoordinateT> GetClosestPointToSegment(const std::pair<Coordin
 
 ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const maliput::math::Vector3& xyz,
                                      double tolerance) {
-  ClosestPointResult3d result;
-  result.distance = std::numeric_limits<double>::max();
-  double length{};
-  for (auto first = line_string.begin(), second = std::next(line_string.begin()); second != line_string.end();
-       ++first, ++second) {
-    // If points are under numeric tolerance, skip segment.
-    if ((*first - *second).norm() < kEpsilon) continue;
-    const auto closest_point_res = GetClosestPointToSegment(Segment3d{*first, *second}, xyz, tolerance);
-    if (closest_point_res.distance < result.distance) {
-      result = closest_point_res;
-      result.p += length;
+  std::optional<LineString3d::Segment> closest_segment{std::nullopt};
+  ClosestPointResult3d segment_closest_point_result;
+  segment_closest_point_result.distance = std::numeric_limits<double>::max();
+
+  const auto& segments = line_string.segments();
+  std::for_each(std::execution::par, segments.begin(), segments.end(), [&](const auto& segment) {
+    const auto& start = line_string[segment.second.idx_start];
+    const auto& end = line_string[segment.second.idx_end];
+    const auto current_closest_point_res = GetClosestPointToSegment(Segment3d{start, end}, xyz, tolerance);
+    if (current_closest_point_res.distance < segment_closest_point_result.distance) {
+      segment_closest_point_result = current_closest_point_res;
+      closest_segment = {segment.second.idx_start, segment.second.idx_end, segment.second.p_interval};
     }
-    length += (*second - *first).norm();  //> Adds segment length
-  }
-  return result;
+  });
+
+  return {segment_closest_point_result.p + closest_segment->p_interval.min, segment_closest_point_result.point,
+          segment_closest_point_result.distance};
 }
 
 ClosestPointResult3d GetClosestPointUsing2dProjection(const LineString3d& line_string,
                                                       const maliput::math::Vector3& xyz, double tolerance) {
-  ClosestPointResult3d result;
-  result.distance = std::numeric_limits<double>::max();
-  double length{};
-  for (auto first = line_string.begin(), second = std::next(line_string.begin()); second != line_string.end();
-       ++first, ++second) {
-    // If points are under numeric tolerance, skip segment.
-    if ((*first - *second).norm() < kEpsilon) continue;
-    const double segment_3d_length = (*second - *first).norm();
-    const maliput::math::Vector2 xy = To2D(xyz);
-    const Segment2d segment_2d{To2D(*first), To2D(*second)};
-    const auto closest_point_res = GetClosestPointToSegment(segment_2d, xy, tolerance);
-    if (closest_point_res.distance < result.distance) {
-      // Retrieve the z coordinate from the 3d line string.
-      const double scale_p = segment_3d_length / (segment_2d.first - segment_2d.second).norm();
-      const double p_3d = closest_point_res.p * scale_p;
-      const double z_coordinate = first->z() + ((*second - *first).normalized() * p_3d).z();
-      // Compound closest point in 3d.
-      const maliput::math::Vector3 closest_point_3d{closest_point_res.point.x(), closest_point_res.point.y(),
-                                                    z_coordinate};
-
-      result.distance = (xyz - closest_point_3d).norm();
-      result.point = closest_point_3d;
-      result.p = length + p_3d;
+  const maliput::math::Vector2 xy = To2D(xyz);
+
+  // Find the closest segment in 2D
+  std::optional<LineString3d::Segment> closest_segment{std::nullopt};
+  ClosestPointResult2d segment_closest_point_result;
+  segment_closest_point_result.distance = std::numeric_limits<double>::max();
+
+  const auto& segments = line_string.segments();
+  std::for_each(std::execution::par, segments.begin(), segments.end(), [&](const auto& segment) {
+    const auto& start = line_string[segment.second.idx_start];
+    const auto& end = line_string[segment.second.idx_end];
+    const Segment2d segment_2d{To2D(start), To2D(end)};
+    const auto current_closest_point_res = GetClosestPointToSegment(segment_2d, xy, tolerance);
+    if (current_closest_point_res.distance < segment_closest_point_result.distance) {
+      segment_closest_point_result = current_closest_point_res;
+      closest_segment = {segment.second.idx_start, segment.second.idx_end, segment.second.p_interval};
     }
-    length += segment_3d_length;  //> Adds segment length
-  }
-  return result;
+  });
+
+  // Analyze closest segment and compute the closest point in 3D.
+  const auto& start_point = line_string[closest_segment->idx_start];
+  const auto& end_point = line_string[closest_segment->idx_end];
+  const double segment_3d_length = (end_point - start_point).norm();
+  const Segment2d segment_2d{To2D(start_point), To2D(end_point)};
+
+  const double scale_p = segment_3d_length / (segment_2d.first - segment_2d.second).norm();
+  const double p_3d = segment_closest_point_result.p * scale_p;
+  const double z_coordinate = start_point.z() + ((end_point - start_point).normalized() * p_3d).z();
+  // Compound closest point in 3d.
+  const maliput::math::Vector3 closest_point_3d{segment_closest_point_result.point.x(),
+                                                segment_closest_point_result.point.y(), z_coordinate};
+
+  return {p_3d + closest_segment->p_interval.min, closest_point_3d, (xyz - closest_point_3d).norm()};
 }
 
 double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double tolerance) {

From 88674927e4a2933c267d4c8264a59993118d224b Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Fri, 3 Feb 2023 10:33:05 -0300
Subject: [PATCH 05/12] Adds docstrings.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/maliput_sparse/geometry/line_string.h | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
index 5ec0712..8e81a8e 100644
--- a/include/maliput_sparse/geometry/line_string.h
+++ b/include/maliput_sparse/geometry/line_string.h
@@ -75,7 +75,13 @@ class LineString final {
   using iterator = typename std::vector<CoordinateT>::iterator;
   using const_iterator = typename std::vector<CoordinateT>::const_iterator;
 
+  /// A segment of a LineString.
+  /// A segment is defined by a:
+  /// - start index: index of the first coordinate of the segment.
+  /// - end index: index of the last coordinate of the segment, in general it is the start index + 1.
   struct Segment {
+    /// Defines an interval in the P value of the parametrized LineString.
+    /// The Less than operator is defined to allow the use of this struct as a key in a collection like std::map.
     struct Interval {
       // Interval() = default;
 
@@ -99,12 +105,17 @@ class LineString final {
         }
       }
 
+      // Min p value.
       double min{};
+      // Max p value.
       double max{};
     };
 
+    /// Start index of the segment.
     std::size_t idx_start;
+    /// End index of the segment.
     std::size_t idx_end;
+    /// Interval of the segment.
     Segment::Interval p_interval;
   };
 

From 2bc66685d8ea98183dc765a394585a3fe6077b52 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Fri, 3 Feb 2023 10:45:04 -0300
Subject: [PATCH 06/12] Fixes TBB dependency.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 CMakeLists.txt              | 3 +++
 src/geometry/CMakeLists.txt | 6 ++++--
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3bb60b6..612cb29 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,9 @@ message(STATUS "\n\n====== Finding 3rd Party Packages ======\n")
 find_package(ament_cmake REQUIRED)
 find_package(maliput REQUIRED)
 
+# TODO(francocipollone): Remove this once we move to GCC 10.
+find_package(TBB REQUIRED)
+
 ##############################################################################
 # Project Configuration
 ##############################################################################
diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt
index 107e841..62fb195 100644
--- a/src/geometry/CMakeLists.txt
+++ b/src/geometry/CMakeLists.txt
@@ -24,8 +24,10 @@ target_include_directories(geometry
 
 target_link_libraries(geometry
   PUBLIC
-  maliput::common
-  maliput::math
+    maliput::common
+    maliput::math
+  PRIVATE
+    TBB::tbb
 )
 
 ##############################################################################

From 1d2c300a8a1c94fc7294291eef525f4269a09613 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Fri, 3 Feb 2023 10:59:28 -0300
Subject: [PATCH 07/12] Adds tbb to the package xml.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/package.xml b/package.xml
index d815b99..1f68db5 100644
--- a/package.xml
+++ b/package.xml
@@ -11,6 +11,7 @@
   <doc_depend>ament_cmake_doxygen</doc_depend>
 
   <depend>maliput</depend>
+  <depend>tbb</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>

From 9f8120059c61274907ded5908d33b78a6d7fbc85 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Mon, 6 Feb 2023 11:05:54 -0300
Subject: [PATCH 08/12] Removes parallel execution due to race conditions.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 CMakeLists.txt                   | 3 ---
 package.xml                      | 1 -
 src/geometry/CMakeLists.txt      | 2 --
 src/geometry/utility/geometry.cc | 5 ++---
 4 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 612cb29..3bb60b6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,9 +14,6 @@ message(STATUS "\n\n====== Finding 3rd Party Packages ======\n")
 find_package(ament_cmake REQUIRED)
 find_package(maliput REQUIRED)
 
-# TODO(francocipollone): Remove this once we move to GCC 10.
-find_package(TBB REQUIRED)
-
 ##############################################################################
 # Project Configuration
 ##############################################################################
diff --git a/package.xml b/package.xml
index 1f68db5..d815b99 100644
--- a/package.xml
+++ b/package.xml
@@ -11,7 +11,6 @@
   <doc_depend>ament_cmake_doxygen</doc_depend>
 
   <depend>maliput</depend>
-  <depend>tbb</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt
index 62fb195..c024a89 100644
--- a/src/geometry/CMakeLists.txt
+++ b/src/geometry/CMakeLists.txt
@@ -26,8 +26,6 @@ target_link_libraries(geometry
   PUBLIC
     maliput::common
     maliput::math
-  PRIVATE
-    TBB::tbb
 )
 
 ##############################################################################
diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
index 5c6190f..aba0b09 100644
--- a/src/geometry/utility/geometry.cc
+++ b/src/geometry/utility/geometry.cc
@@ -62,7 +62,6 @@
 #include <cmath>
 #include <numeric>
 
-#include <execution>
 #include <maliput/common/range_validator.h>
 
 namespace maliput_sparse {
@@ -354,7 +353,7 @@ ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const mali
   segment_closest_point_result.distance = std::numeric_limits<double>::max();
 
   const auto& segments = line_string.segments();
-  std::for_each(std::execution::par, segments.begin(), segments.end(), [&](const auto& segment) {
+  std::for_each(segments.begin(), segments.end(), [&](const auto& segment) {
     const auto& start = line_string[segment.second.idx_start];
     const auto& end = line_string[segment.second.idx_end];
     const auto current_closest_point_res = GetClosestPointToSegment(Segment3d{start, end}, xyz, tolerance);
@@ -378,7 +377,7 @@ ClosestPointResult3d GetClosestPointUsing2dProjection(const LineString3d& line_s
   segment_closest_point_result.distance = std::numeric_limits<double>::max();
 
   const auto& segments = line_string.segments();
-  std::for_each(std::execution::par, segments.begin(), segments.end(), [&](const auto& segment) {
+  std::for_each(segments.begin(), segments.end(), [&](const auto& segment) {
     const auto& start = line_string[segment.second.idx_start];
     const auto& end = line_string[segment.second.idx_end];
     const Segment2d segment_2d{To2D(start), To2D(end)};

From 4b03a2b262125a7cfe65793ba5ad73668e383e20 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Mon, 6 Feb 2023 11:42:59 -0300
Subject: [PATCH 09/12] Addresses comments.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/maliput_sparse/geometry/line_string.h | 10 +++++-----
 src/geometry/utility/geometry.cc              |  7 ++-----
 2 files changed, 7 insertions(+), 10 deletions(-)

diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
index 8e81a8e..6bc7204 100644
--- a/include/maliput_sparse/geometry/line_string.h
+++ b/include/maliput_sparse/geometry/line_string.h
@@ -78,13 +78,13 @@ class LineString final {
   /// A segment of a LineString.
   /// A segment is defined by a:
   /// - start index: index of the first coordinate of the segment.
-  /// - end index: index of the last coordinate of the segment, in general it is the start index + 1.
+  /// - end index: index of the last coordinate of the segment, it is expected to be `start index + 1` as `LineString`'s
+  /// constructor
+  ///              builds the segments from consecutive coordinates.
   struct Segment {
-    /// Defines an interval in the P value of the parametrized LineString.
+    /// Defines an interval in the @f$ p @f$ value of the parametrized LineString.
     /// The Less than operator is defined to allow the use of this struct as a key in a collection like std::map.
     struct Interval {
-      // Interval() = default;
-
       /// Creates a Interval.
       /// @param min_in Is the minimum value of the interval.
       /// @param max_in Is the maximum value of the interval.
@@ -96,7 +96,7 @@ class LineString final {
       /// @param min_max Is the minimum and maximum value of the interval.
       Interval(double min_max) : min(min_max), max(min_max) {}
 
-      // Less than operator.
+      /// Less than operator.
       bool operator<(const Interval& rhs) const {
         if (min < rhs.min) {
           return max <= rhs.max ? true : false;
diff --git a/src/geometry/utility/geometry.cc b/src/geometry/utility/geometry.cc
index aba0b09..7c75b88 100644
--- a/src/geometry/utility/geometry.cc
+++ b/src/geometry/utility/geometry.cc
@@ -290,10 +290,7 @@ CoordinateT InterpolatedPointAtP(const LineString<CoordinateT>& line_string, dou
   const CoordinateT& end = line_string[bound_points.idx_end];
   const CoordinateT d_segment{end - start};
   const double remaining_distance = p - bound_points.length;
-  if (remaining_distance < kEpsilon) {
-    return start;
-  }
-  return start + d_segment.normalized() * remaining_distance;
+  return remaining_distance < kEpsilon ? start : start + d_segment.normalized() * remaining_distance;
 }
 
 double GetSlopeAtP(const LineString3d& line_string, double p, double tolerance) {
@@ -309,7 +306,7 @@ 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(typename LineString<CoordinateT>::Segment::Interval{p});
+  const auto segment = line_string.segments().at({p});
   return {segment.idx_start, segment.idx_end, segment.p_interval.min};
 }
 

From 8d2d32b90c660c10d01ec81ba07a339b9b47bd08 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Tue, 7 Feb 2023 11:35:24 -0300
Subject: [PATCH 10/12] Skips segment creation when it is zero length.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/maliput_sparse/geometry/line_string.h | 5 +++++
 src/geometry/utility/geometry.cc              | 4 ++--
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
index 6bc7204..093595c 100644
--- 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
index 7c75b88..08c6ba6 100644
--- 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) {

From f48371a6c1b11895cf57b012b4e3bb6b1f62e131 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Wed, 8 Feb 2023 18:40:50 -0300
Subject: [PATCH 11/12] Deals with consecutive points.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/maliput_sparse/geometry/line_string.h | 21 ++++++++++++++-----
 test/geometry/line_string_test.cc             |  9 ++++++++
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/include/maliput_sparse/geometry/line_string.h b/include/maliput_sparse/geometry/line_string.h
index 093595c..16ca699 100644
--- 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/vector.h>
 
@@ -148,16 +149,26 @@ 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
     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/test/geometry/line_string_test.cc b/test/geometry/line_string_test.cc
index 603d116..49b0236 100644
--- a/test/geometry/line_string_test.cc
+++ b/test/geometry/line_string_test.cc
@@ -158,6 +158,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();

From c8a2c8d24ba8d795a5dda4d07e61909e59e7aa05 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Wed, 8 Feb 2023 19:17:58 -0300
Subject: [PATCH 12/12] Removes unnecesary test.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 test/geometry/utility/geometry_test.cc | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/test/geometry/utility/geometry_test.cc b/test/geometry/utility/geometry_test.cc
index c37f994..0c16d10 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<double>::infinity()};
   const LineString3d kOnlyZ{{0., 0., 0.}, {0., 0., 100.}, {0., 0., 0.}};