Stores linestring's points in a kdtree. #52
Conversation
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
|
This is a proof of concept. Some docstrings should be added and a bit of code polishing could be also necessary. @agalbachicar |
|
CI is failing until maliput/maliput#541 is merged. |
| Segments segments_{}; | ||
| double length_{}; | ||
| std::shared_ptr<KDTreeData> kdtree_; |
There was a problem hiding this comment.
Here we have a shared_ptr but we are calling make_unique.
Some questions:
- Why shared_ptr?
- Why KDTreeData? I would just call it KDTree, the class namespacing will apply.
There was a problem hiding this comment.
I went for using shared_ptr because:
- It allows us to use the default copy constructor and avoid having multiple kdtree's in memory. (if you copy the references for the kdtree, will go the same space in memory)
- In LaneGeometry's constructor we are copying the linestring. If we use unique_ptr we should move it.
We could refactor this to use unique_ptr, probably the lane geometry class now will need to receive unique_ptr and the class won't have the possibility to be copied.
There was a problem hiding this comment.
Why KDTreeData? I would just call it KDTree, the class namespacing will apply.
Indeed
There was a problem hiding this comment.
I am not fond of shared_ptr and I think it will play against us any potential parallelization we would like to use.
Also, I would prefer to pass raw pointers and consider a clone method so as to control copies as well. We don't want to copy points everywhere but because of that we also don't want to extend the life of the points where we don't want it to go.
Let's make a case for shared_ptr in which we don't really know where the kdtree will be used and how, and I'll be more than happy to add it.
There was a problem hiding this comment.
Unique ptr
If going for a unique_ptr here I wouldn't add a copy constructor that regenerates the kdtree because that would be way non-performance for the class.
So I purpose switching to a unique ptr but let me tackle this in a follow-up PR because it affects so many classes here and in maliput_osm. LineString3d is spread across this package and at the parser level in maliput_osm:
- LaneGeometry will be the class that will hold the ownership of the linestrings
- As LineString has a non-sharable member, all LineString instances will be a
std::unique_ptrinstance, and all classes holding a LineString will also need to be a unique_ptr.
(Note that heap-allocated memory isn't as fast as stack memory)
Shared ptr
Let's make a case for shared_ptr in which we don't really know where the kdtree will be used and how, and I'll be more than happy to add it.
maliput_sparse isn't a final and contained backend, but a tool where other backends, relying on other formats, will build their castle above.
With this in mind, it is a bit hard to think ways in which LineString class will be used. HOWEVER, the case that quickly comes to my mind is the case where you want to use the same LineString to be used by two adjacent lanes.
maliput_sparse::geometry::LineString3d left_lane_0 = FromMyFormatToMaliputSparse(weird_format_left_line_string);
maliput_sparse::geometry::LineString3d right_lane_1 = left_lane_0; // wuala, kdtree isn't built again.@agalbachicar ptal
There was a problem hiding this comment.
Sorry for not coming back here before.
So I purpose switching to a unique ptr but let me tackle this in a follow-up PR because it affects so many classes here and in maliput_osm
I am OK with deferring to a follow up PR the conversion to a unique_ptr.
With this in mind, it is a bit hard to think ways in which LineString class will be used. HOWEVER, the case that quickly comes to my mind is the case where you want to use the same LineString to be used by two adjacent lanes.
Then, it makes sense to have a view-type of the LineString that allows you to access it but not necessarily own the memory. It could be arranged somewhere else. For that to happen, we can change the builder to construct LineStrings at the Segment level, and then organize how we want them to be used. That means, we could derive LaneGeometries from the set of LineStrings or explicitly organize the pair of LineStrings at the moment of construction the LaneGeometry. This does not require a shared_ptr.
src/geometry/utility/geometry.cc
Outdated
| // 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); | ||
| if (nearest_point.idx().value() == 0) { |
There was a problem hiding this comment.
Nit: we should try to refactor this if-else-elseif possible. That should help to refactor this code and speed it up a bit more.
There was a problem hiding this comment.
Refactor on what sense?
We can do something with a switch case like
const IdxType idx_type = nearest_point.idx().value() == 0
? IdxType::kStart
: (nearest_point.idx().value() == line_string.size() - 1 ? IdxType::kEnd : IdxType::Middle);
switch case:
IdxType::kStart:
//
IdxType::kEnd:
//
default:
// However I am not sure if that is an improvement.
Do you want to avoid the two if conditions before the else? To avoid two comparison most of the time?
There was a problem hiding this comment.
In each clause we are setting closest_segment and segment_closest_point_result. The first two cases have a very similar way of obtaining the closest_segment and pretty much the same for segment_closest_point_result.
The last two, require more computation but do the same.
I wonder if we could obtain the closest_segment by passing the index always and save the special cases at the beginning and the end for a general one. That would simplify the four cases into two.
There was a problem hiding this comment.
I did some minor adjustments at cf00b78
However there is a trade-off: More simple code vs more performant, and I think that with this last addition, I making the execution run some more comparisons than before.
Maybe I am missing some clear refactor
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
…/poc_line_string_kd_tree Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
|
To also improve ToLanePosition method(therefore ToRoadPosition and FindRoadPosition by consecuence), I am thinking to also store a 2d kdtree to be used by the GetClosestPointUsing2dProjection. This is probably a bit cumbersome, however, using GetClosestPoint (with kdtree3) vs GetClosestPointUsing2dProjection for the LanePosition calculation, the different is massive in performance. I will think a bit about the possibilities. EDITED: I think I could take advantage of the kdtree 3d as it is for improving this query. I will tackle this in a future PR |
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
src/geometry/utility/geometry.cc
Outdated
| // 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); | ||
| if (nearest_point.idx().value() == 0) { |
There was a problem hiding this comment.
In each clause we are setting closest_segment and segment_closest_point_result. The first two cases have a very similar way of obtaining the closest_segment and pretty much the same for segment_closest_point_result.
The last two, require more computation but do the same.
I wonder if we could obtain the closest_segment by passing the index always and save the special cases at the beginning and the end for a general one. That would simplify the four cases into two.
francocipollone
changed the title
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
🎉 New feature
Related to #50 #47
Goes on top of #51
Summary
Speed up
Calculating speed up on top of #51
Note: ToInertialPosition doesn't show an speed up because under the hood it relies on GetClosestPointUsing2dProjection which doesn't use the kdtree to obtain the closest point.
And what about the memory footprint?
Test it
Checklist