Precomputes linestring's segments intervals #51

francocipollone · 2023-02-02T20:56:49Z

🎉 New feature

Related to #50 #47

Summary

LineString's constructor conforms internally a map with <Interval, Segment> where the interval contains the minimum and maximum value of the interval for each one of the segments.
- So getting the correspondent segment for a given parameter p of the linestring is quite quick.
Other improvements:
- For-loops for the GetClosestPoint methods were replaced by for_each with parallel execution
- Minor improvements also to avoid copying and relying on references.

Speed up

Benchmark name	cpu time[us]	cpu time[us]	speed up	map
	main	francocipollone/segments_interval
MaliputOsmQueryTime/GetLane/0	0.025	0.022	1.14	SingleLane.osm
MaliputOsmQueryTime/GetLane/1	0.023	0.023	1.00	ArcLane.osm
MaliputOsmQueryTime/GetLane/2	0.024	0.025	0.96	ArcLaneDense.osm
MaliputOsmQueryTime/GetLane/3	0.024	0.022	1.09	Circuit.osm
MaliputOsmQueryTime/Length/0	0.003	0.003	1.00	SingleLane.osm
MaliputOsmQueryTime/Length/1	0.003	0.003	1.00	ArcLane.osm
MaliputOsmQueryTime/Length/2	0.003	0.003	1.00	ArcLaneDense.osm
MaliputOsmQueryTime/Length/3	0.003	0.003	1.00	Circuit.osm
MaliputOsmQueryTime/ToLanePosition/0	5.22	3.15	1.66	SingleLane.osm
MaliputOsmQueryTime/ToLanePosition/1	36	17.8	2.02	ArcLane.osm
MaliputOsmQueryTime/ToLanePosition/2	1447	568	2.55	ArcLaneDense.osm
MaliputOsmQueryTime/ToLanePosition/3	107	35.8	2.99	Circuit.osm
MaliputOsmQueryTime/ToInertialPosition/0	1.68	1.09	1.54	SingleLane.osm
MaliputOsmQueryTime/ToInertialPosition/1	10.2	4.05	2.52	ArcLane.osm
MaliputOsmQueryTime/ToInertialPosition/2	318	117	2.72	ArcLaneDense.osm
MaliputOsmQueryTime/ToInertialPosition/3	20.9	8.09	2.58	Circuit.osm
MaliputOsmQueryTime/GetOrientation/0	1.52	0.935	1.63	SingleLane.osm
MaliputOsmQueryTime/GetOrientation/1	8.81	4.07	2.16	ArcLane.osm
MaliputOsmQueryTime/GetOrientation/2	341	126	2.71	ArcLaneDense.osm
MaliputOsmQueryTime/GetOrientation/3	20.3	7.68	2.64	Circuit.osm
MaliputOsmQueryTime/ToRoadPosition/0	12.5	8.64	1.45	SingleLane.osm
MaliputOsmQueryTime/ToRoadPosition/1	94.1	42	2.24	ArcLane.osm
MaliputOsmQueryTime/ToRoadPosition/2	3532	1354	2.61	ArcLaneDense.osm
MaliputOsmQueryTime/ToRoadPosition/3	1836	665	2.76	Circuit.osm
MaliputOsmQueryTime/FindRoadPositions/0	10.1	7.53	1.34	SingleLane.osm
MaliputOsmQueryTime/FindRoadPositions/1	82	34.9	2.35	ArcLane.osm
MaliputOsmQueryTime/FindRoadPositions/2	2897	1224	2.37	ArcLaneDense.osm
MaliputOsmQueryTime/FindRoadPositions/3	1753	577	3.04	Circuit.osm

Test it

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
Consider updating Python bindings (if it affects the public API)

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

include/maliput_sparse/geometry/line_string.h

francocipollone · 2023-02-02T21:00:28Z

@agalbachicar PTAL, still missing some minor docstrings. In the PR's description there is stated the speed ups in query times.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone · 2023-02-03T13:40:42Z

Locally it is building correctly while in GhAction CI it is failing cause using <execution> header and a failed dependency:

  In file included from /usr/include/c++/9/pstl/parallel_backend.h:14,
                   from /usr/include/c++/9/pstl/algorithm_impl.h:25,
                   from /usr/include/c++/9/pstl/glue_execution_defs.h:52,
                   from /usr/include/c++/9/execution:32,
                   from /__w/maliput_sparse/maliput_sparse/ros_ws/src/q0ggbncasvb/maliput_sparse/src/geometry/utility/geometry.cc:65:
  /usr/include/c++/9/pstl/parallel_backend_tbb.h:19:10: fatal error: tbb/blocked_range.h: No such file or directory
     19 | #include <tbb/blocked_range.h>
        |          ^~~~~~~~~~~~~~~~~~~~~

In GCC 9 we need to link against tbb, in GCC 10 it isn't required. (Not sure why it is working locally)

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

agalbachicar

Great improvement! PTAL to some minor comments.

agalbachicar · 2023-02-06T09:35:50Z

include/maliput_sparse/geometry/line_string.h

+  /// 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.


Why in general it is the start index + 1 ? Can we add that to the docstring.

I think this comment edit was not effective. I wanted to say that we should explicitly mention that start_index + 1 is generally used within the LineString constructor.

agalbachicar · 2023-02-06T09:36:17Z

include/maliput_sparse/geometry/line_string.h

+  /// - 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.


Suggested change

/// Defines an interval in the P value of the parametrized LineString.

/// Defines an interval in the @f$ p @f$ value of the parametrized LineString.

agalbachicar · 2023-02-06T09:36:47Z

include/maliput_sparse/geometry/line_string.h

+    /// 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;


Probably should delete.

agalbachicar · 2023-02-06T09:38:04Z

include/maliput_sparse/geometry/line_string.h

+      /// @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.


Suggested change

// Less than operator.

/// Less than operator.

agalbachicar · 2023-02-06T09:50:53Z

src/geometry/utility/geometry.cc

  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;


Can we merge this into the following?

return remaining_distance < kEpsilon ? start : start + d_segment.normalized() * remaining_distance;

Indeed
Done.

agalbachicar · 2023-02-06T09:54:13Z

src/geometry/utility/geometry.cc

-  return {line_string.end() - 1, line_string.end() - 2, line_string.length()};
+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});


Does the compiler understand the right type conversion if we did?

const auto segment = line_string.segments().at({p});

Yeah it does.
Done

francocipollone · 2023-02-06T13:59:04Z

src/geometry/utility/geometry.cc

+  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};
    }


Sorry, this is not aligned with parallel execution as there might be race conditions, so I will remove the parallel execution here and use a regular for_each statement.

Done at 9f81200

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

agalbachicar

LGTM

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone · 2023-02-07T14:37:44Z

@agalbachicar , please take a look at 8d2d32b

I found this case when trying to create the meshes for the arc_lane_dense map(The last point of the map is the same at the right before). Even though it reflects a possible wrong creation of the map adding that could solve the issue

agalbachicar

Is it possible to create a unit test for that?

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone · 2023-02-08T21:43:30Z

Is it possible to create a unit test for that?

Check f48371a
Not only did I create the unit test but also provided a logic to remove the duplicated points. There is no point on keep duplicated coordinates.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

agalbachicar

LGTM

francocipollone added 4 commits February 2, 2023 10:22

Implements segments interval.

1652b4c

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

Adds some improvements.

20dabfb

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

Removes use of to 2d conversion in the Get2DTangentAtP method.

9b0250d

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

Use parallel for_each for the get closest point methods.

86e3702

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone commented Feb 2, 2023

View reviewed changes

include/maliput_sparse/geometry/line_string.h Outdated Show resolved Hide resolved

francocipollone requested a review from agalbachicar February 2, 2023 20:59

Adds docstrings.

8867492

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone added 2 commits February 3, 2023 10:45

Fixes TBB dependency.

2bc6668

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

Adds tbb to the package xml.

1d2c300

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone marked this pull request as ready for review February 3, 2023 14:32

francocipollone mentioned this pull request Feb 3, 2023

Stores linestring's points in a kdtree. #52

Merged

6 tasks

agalbachicar requested changes Feb 6, 2023

View reviewed changes

francocipollone commented Feb 6, 2023

View reviewed changes

francocipollone added 2 commits February 6, 2023 11:05

Removes parallel execution due to race conditions.

9f81200

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

Addresses comments.

4b03a2b

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone requested a review from agalbachicar February 6, 2023 14:43

agalbachicar previously approved these changes Feb 7, 2023

View reviewed changes

Skips segment creation when it is zero length.

8d2d32b

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone dismissed agalbachicar’s stale review via 8d2d32b February 7, 2023 14:35

francocipollone requested a review from agalbachicar February 7, 2023 14:37

agalbachicar requested changes Feb 7, 2023

View reviewed changes

Deals with consecutive points.

f48371a

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

Removes unnecesary test.

c8a2c8d

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

francocipollone requested a review from agalbachicar February 9, 2023 12:54

agalbachicar approved these changes Feb 9, 2023

View reviewed changes

francocipollone merged commit 1e5e203 into main Feb 9, 2023

francocipollone deleted the francocipollone/segments_interval branch February 9, 2023 17:24

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Precomputes linestring's segments intervals #51

Precomputes linestring's segments intervals #51

francocipollone commented Feb 2, 2023 •

edited

Loading

francocipollone commented Feb 2, 2023

francocipollone commented Feb 3, 2023 •

edited

Loading

agalbachicar left a comment

agalbachicar Feb 6, 2023

agalbachicar Feb 6, 2023

francocipollone Feb 6, 2023

agalbachicar Feb 6, 2023

francocipollone Feb 6, 2023

agalbachicar Feb 6, 2023

francocipollone Feb 6, 2023

agalbachicar Feb 6, 2023

francocipollone Feb 6, 2023

agalbachicar Feb 6, 2023

francocipollone Feb 6, 2023

agalbachicar Feb 6, 2023

francocipollone Feb 6, 2023

francocipollone Feb 6, 2023

francocipollone Feb 6, 2023

agalbachicar left a comment

francocipollone commented Feb 7, 2023

agalbachicar left a comment

francocipollone commented Feb 8, 2023

agalbachicar left a comment

	/// Defines an interval in the P value of the parametrized LineString.
	/// Defines an interval in the @f$ p @f$ value of the parametrized LineString.

Precomputes linestring's segments intervals #51

Precomputes linestring's segments intervals #51

Conversation

francocipollone commented Feb 2, 2023 • edited Loading

🎉 New feature

Summary

Speed up

Test it

Checklist

francocipollone commented Feb 2, 2023

francocipollone commented Feb 3, 2023 • edited Loading

agalbachicar left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

agalbachicar left a comment

Choose a reason for hiding this comment

francocipollone commented Feb 7, 2023

agalbachicar left a comment

Choose a reason for hiding this comment

francocipollone commented Feb 8, 2023

agalbachicar left a comment

Choose a reason for hiding this comment

francocipollone commented Feb 2, 2023 •

edited

Loading

francocipollone commented Feb 3, 2023 •

edited

Loading