Stores linestring's points in a kdtree.

include/maliput_sparse/geometry/line_string.h

@@ -37,6 +37,7 @@
 
 #include <maliput/common/logger.h>
 #include <maliput/common/maliput_throw.h>
+#include <maliput/math/kd_tree.h>
 #include <maliput/math/vector.h>
 
 namespace maliput_sparse {
@@ -122,6 +123,43 @@ class LineString final {
 
   using Segments = std::map<typename Segment::Interval, Segment>;
 
+  /// Extends the CoordinateT class with information about the index of the coordinate in the LineString and the
+  /// @f$ p @f$ value up-to the coordinate in the parametrized LineString.
+  /// Convenient to use with the KDTree.
+  class Point : public CoordinateT {
+   public:
+    /// Default constructor.
+    Point() = default;
+
+    /// Creates a Point.
+    /// @param coordinate The coordinate of the point.
+    /// @param idx The index of the coordinate in the LineString.
+    /// @param p The @f$ p @f$ value up-to the coordinate in the parametrized LineString.
+    /// @throw maliput::common::assertion_error When `p` is negative.
+    Point(const CoordinateT& coordinate, std::size_t idx, double p) : CoordinateT(coordinate), idx_(idx), p_(p) {
+      MALIPUT_THROW_UNLESS(p >= 0.);
+    }
+
+    /// Creates a point
+    /// @param coordinate The coordinate of the point.
+    explicit Point(const CoordinateT& coordinate) : CoordinateT(coordinate) {}
+
+    /// @return If provided via constructor, the index of the coordinate in the LineString, std::nullopt otherwise.
+    std::optional<std::size_t> idx() const { return idx_; }
+    /// @return If provided via constructor, the @f$ p @f$ value up-to the coordinate in the parametrized LineString,
+    /// std::nullopt otherwise.
+    std::optional<double> p() const { return p_; }
+
+    /// @return The underlying coordinate of the point.
+    const CoordinateT* coordinate() const { return this; }
+
+   private:
+    std::optional<std::size_t> idx_{};
+    std::optional<double> p_{};
+  };
+
+  using KDTree = maliput::math::KDTree<Point, CoordinateT::kDimension>;
+
   /// Constructs a LineString from a std::vector.
   ///
   /// This function calls LineString(coordinates.begin, coordinates.end)
@@ -165,14 +203,23 @@ class LineString final {
       coordinates_.erase(coordinates_.begin() + *it);
     }
     MALIPUT_THROW_UNLESS(coordinates_.size() > 1);
-    // Fill up the segments collection
+    // Fill up the segments collection and the points collection.
     double p = 0;
+    points_.reserve(coordinates_.size());
     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};
+      // Add the segment.
       segments_.emplace(interval, Segment{idx, idx + 1, interval});
+      // Add the point.
+      points_.push_back(Point(coordinates_[idx], idx, p));
       p += segment_length;
     }
+    // Add the last point.
+    points_.push_back(Point(coordinates_[coordinates_.size() - 1], coordinates_.size() - 1, p));
+    // Build the KDTree.
+    kd_tree_ = std::make_shared<KDTree>(points_.begin(), points_.end());
+    // Set the length.
     length_ = p;
   }
 
@@ -196,6 +243,12 @@ class LineString final {
   /// @return A vector of segments.
   const Segments& segments() const { return segments_; }
 
+  /// Return the points of this LineString.
+  const std::vector<Point>& points() const { return points_; }
+
+  /// Get KD Tree of the LineString.
+  const KDTree* kd_tree() const { return kd_tree_.get(); }
+
   /// @returns begin iterator of the underlying collection.
   iterator begin() { return coordinates_.begin(); }
   /// @returns begin const iterator of the underlying collection.
@@ -215,8 +268,10 @@ class LineString final {
 
  private:
   std::vector<CoordinateT> coordinates_{};
+  std::vector<Point> points_{};
   Segments segments_{};
   double length_{};
+  std::shared_ptr<KDTree> kd_tree_;
 };
 
 // Convenient aliases.

include/maliput_sparse/geometry/utility/geometry.h

@@ -122,13 +122,15 @@ maliput::math::Vector3 GetTangentAtP(const LineString3d& line_string, double p,
 
 /// Gets the closest point in the @p segment to the given @p xyz point.
 /// @tparam CoordinateT The coordinate type of the @p segment .
-/// @param segment Segment to be computed the closest point from.
+/// @param start_segment_point Start point of the segment.
+/// @param end_segment_point End point of the segment.
 /// @param coordinate Point to be computed the closest point to.
 /// @param tolerance tolerance.
 /// @return A ClosestPointResult struct containing the closest point, the distance between the closest point and @p xyz
 /// and the p coordinate in the segment matching the closest point.
 template <typename CoordinateT = maliput::math::Vector3>
-ClosestPointResult<CoordinateT> GetClosestPointToSegment(const std::pair<CoordinateT, CoordinateT>& segment,
+ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
+                                                         const CoordinateT& end_segment_point,
                                                          const CoordinateT& coordinate, double tolerance);
 
 /// Gets the closest point in the @p line_string to the given @p xyz point.

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_itr = line_string.segments().find({p});
-  return {segment_itr->second.idx_start, segment_itr->second.idx_end, segment_itr->second.p_interval.min};
+  const auto segment = line_string.segments().at({p});
+  return {segment.idx_start, segment.idx_end, segment.p_interval.min};
 }
 
 double Get2DHeadingAtP(const LineString3d& line_string, double p, double tolerance) {
@@ -328,17 +328,21 @@ Vector3 GetTangentAtP(const LineString3d& line_string, double p, double toleranc
 }
 
 template <typename CoordinateT>
-ClosestPointResult<CoordinateT> GetClosestPointToSegment(const std::pair<CoordinateT, CoordinateT>& segment,
+ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
+                                                         const CoordinateT& end_segment_point,
                                                          const CoordinateT& coordinate, double tolerance) {
-  const CoordinateT d_segment{segment.second - segment.first};
+  if (start_segment_point == end_segment_point) {
+    return {0., start_segment_point, (start_segment_point - coordinate).norm()};
+  }
+  const CoordinateT d_segment{end_segment_point - start_segment_point};
   const CoordinateT d_segment_normalized{d_segment.normalized()};
-  const CoordinateT d_coordinate_to_first{coordinate - segment.first};
+  const CoordinateT d_coordinate_to_first{coordinate - start_segment_point};
 
   const double unsaturated_p = d_coordinate_to_first.dot(d_segment_normalized);
   const double p = std::clamp(unsaturated_p, 0., d_segment.norm());
 
   // point at p
-  const CoordinateT point = p * d_segment_normalized + segment.first;
+  const CoordinateT point = p * d_segment_normalized + start_segment_point;
   const double distance = (coordinate - point).norm();
   return {p, point, distance};
 }
@@ -349,17 +353,47 @@ ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const mali
   ClosestPointResult3d segment_closest_point_result;
   segment_closest_point_result.distance = std::numeric_limits<double>::max();
 
-  const auto& segments = line_string.segments();
-  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);
-    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};
-    }
-  });
-
+  const std::vector<LineString3d::Point>& line_string_points = line_string.points();
+  const LineString3d::KDTree* kd_tree = line_string.kd_tree();
+  MALIPUT_THROW_UNLESS(kd_tree != nullptr);
+  const LineString3d::Point& nearest_point = kd_tree->nearest_point(LineString3d::Point{xyz});
+  // Nearest point is the closest point to the line string.
+  // 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);
+  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_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);
+
+    // 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{
+                                        nearest_point.p().value(), line_string_points.at(nearest_idx + 1).p().value()}};
+  }
   return {segment_closest_point_result.p + closest_segment->p_interval.min, segment_closest_point_result.point,
           segment_closest_point_result.distance};
 }
@@ -377,8 +411,7 @@ ClosestPointResult3d GetClosestPointUsing2dProjection(const LineString3d& line_s
   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)};
-    const auto current_closest_point_res = GetClosestPointToSegment(segment_2d, xy, tolerance);
+    const auto current_closest_point_res = GetClosestPointToSegment(To2D(start), To2D(end), 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};
@@ -417,8 +450,10 @@ double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, 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 ClosestPointResult3d GetClosestPointToSegment(const maliput::math::Vector3&, const maliput::math::Vector3&,
+                                                       const maliput::math::Vector3&, double);
+template ClosestPointResult2d GetClosestPointToSegment(const maliput::math::Vector2&, const maliput::math::Vector2&,
+                                                       const maliput::math::Vector2&, double);
 
 template class ClosestPointResult<maliput::math::Vector3>;
 template class ClosestPointResult<maliput::math::Vector2>;

test/geometry/line_string_test.cc

@@ -127,6 +127,41 @@ TEST_F(SegmentTest, Values) {
   EXPECT_EQ(interval_2.max, dut_2.p_interval.max);
 }
 
+class PointTest : public ::testing::Test {};
+
+TEST_F(PointTest, BadConstructors) {
+  const maliput::math::Vector3 coordinate{1., 2., 3.};
+  EXPECT_THROW((LineString3d::Point{coordinate, 0, -1.}), maliput::common::assertion_error);
+}
+
+TEST_F(PointTest, Constructors) {
+  {
+    const LineString3d::Point dut{};
+    ASSERT_NE(nullptr, dut.coordinate());
+    EXPECT_EQ(maliput::math::Vector3{}, *dut.coordinate());
+    EXPECT_EQ(std::nullopt, dut.p());
+    EXPECT_EQ(std::nullopt, dut.idx());
+  }
+  {
+    const maliput::math::Vector3 coordinate{1., 2., 3.};
+    const LineString3d::Point dut(coordinate);
+    ASSERT_NE(nullptr, dut.coordinate());
+    EXPECT_EQ(coordinate, *dut.coordinate());
+    EXPECT_EQ(std::nullopt, dut.p());
+    EXPECT_EQ(std::nullopt, dut.idx());
+  }
+  {
+    const maliput::math::Vector3 coordinate{1., 2., 3.};
+    const std::size_t idx = 213;
+    const double p = 0.123;
+    const LineString3d::Point dut(coordinate, idx, p);
+    ASSERT_NE(nullptr, dut.coordinate());
+    EXPECT_EQ(coordinate, *dut.coordinate());
+    EXPECT_EQ(p, dut.p());
+    EXPECT_EQ(idx, dut.idx());
+  }
+}
+
 class LineString3dTest : public ::testing::Test {
  public:
   const Vector3 p1{Vector3::UnitX()};
@@ -188,6 +223,45 @@ TEST_F(LineString3dTest, Segments) {
   EXPECT_NEAR((p1 - p2).norm() + (p2 - p3).norm(), segment_p2_p3.p_interval.max, kTolerance);
 }
 
+TEST_F(LineString3dTest, Points) {
+  const LineString3d dut(std::vector<Vector3>{p1, p2, p3});
+  const std::vector<LineString3d::Point>& points = dut.points();
+
+  ASSERT_EQ(static_cast<size_t>(3), points.size());
+
+  std::size_t idx{0};
+  ASSERT_NE(nullptr, points.at(idx).coordinate());
+  EXPECT_EQ(p1, *(points.at(idx).coordinate()));
+  ASSERT_NE(std::nullopt, points.at(idx).p());
+  EXPECT_EQ(0., points.at(idx).p().value());
+  ASSERT_NE(std::nullopt, points.at(idx).idx());
+  EXPECT_EQ(idx, points.at(0).idx().value());
+
+  idx = 1;
+  ASSERT_NE(nullptr, points.at(idx).coordinate());
+  EXPECT_EQ(p2, *(points.at(idx).coordinate()));
+  ASSERT_NE(std::nullopt, points.at(idx).p());
+  EXPECT_EQ((p2 - p1).norm(), points.at(idx).p().value());
+  ASSERT_NE(std::nullopt, points.at(idx).idx());
+  EXPECT_EQ(idx, points.at(idx).idx().value());
+
+  idx = 2;
+  ASSERT_NE(nullptr, points.at(idx).coordinate());
+  EXPECT_EQ(p3, *(points.at(idx).coordinate()));
+  ASSERT_NE(std::nullopt, points.at(idx).p());
+  EXPECT_EQ((p2 - p1).norm() + (p3 - p2).norm(), points.at(idx).p().value());
+  ASSERT_NE(std::nullopt, points.at(idx).idx());
+  EXPECT_EQ(idx, points.at(idx).idx().value());
+}
+
+TEST_F(LineString3dTest, KDTree) {
+  const LineString3d dut(std::vector<Vector3>{p1, p2, p3});
+  const auto kd_tree = dut.kd_tree();
+  ASSERT_NE(nullptr, kd_tree);
+  const auto res = kd_tree->nearest_point(LineString3d::Point{p2});
+  EXPECT_EQ(p2, *res.coordinate());
+}
+
 TEST_F(LineString3dTest, LengthInjectedDistanceFunction) {
   const LineString<Vector3, SquaredDistanceFunction<Vector3>> dut(std::vector<Vector3>{p1, p2, p3});
   EXPECT_NEAR(4., dut.length(), 1e-14);

test/geometry/utility/geometry_test.cc

@@ -439,7 +439,8 @@ class GetClosestPointToSegmentTest : public ::testing::TestWithParam<GetClosestP
 TEST_P(GetClosestPointToSegmentTest, Test) {
   ASSERT_EQ(case_.eval_points.size(), case_.expected_closest.size()) << ">>>>> Test case is ill-formed.";
   for (std::size_t i = 0; i < case_.eval_points.size(); ++i) {
-    const auto dut = GetClosestPointToSegment(case_.segment, case_.eval_points[i], kTolerance);
+    const auto dut =
+        GetClosestPointToSegment(case_.segment.first, case_.segment.second, case_.eval_points[i], kTolerance);
     EXPECT_NEAR(case_.expected_closest[i].p, dut.p, kTolerance);
     EXPECT_TRUE(maliput::math::test::CompareVectors(case_.expected_closest[i].point, dut.point, kTolerance));
     EXPECT_NEAR(case_.expected_closest[i].distance, dut.distance, kTolerance);