Provides a default ToRoadPosition/FindRoadPosition implementations using kdtree data structure

[francocipollone]

Part of #516

Summary

Provide a base implementation of the RoadGeometry::ToRoadPosition and RoadGeometry::FindRoadPosition methods
that uses a kdtree under the hood for improving query times.

Pendings

• Implement it for the FindRoadPosition method.

[francocipollone]

For finding roads within a given radius we will need the kdtree to provide range a search queries. So I'm gonna continue working on that and then go back to this PR.

[Voldivh]

For some preliminary results on this implementation, take a look to #516 .

[agalbachicar]

PTAL and let me know.

[Voldivh]

After some discussion regarding the design process of this feature, we agreed to create a template for maliput::geometry_base::RoadGeometry in order to give the users/builders of the road geometry a way to select which strategy to use (atm BruteForce and KDTree would be the only available). However, this modification implies several changes in additional parts of the code that does not need, nor care about the strategy to use.
So far I have needed to modify all the forward declarations of the class (Ex. geometry_base/junction.h and geometry_base/branch_point.h) and the mock up class of road geometry and it's use on all the tests (check this branch.
Due to this extensive changes needed to implement this feature using templates, I'm wondering if it is actually the best option to go. Some ideas that were in the air during discussion that would still solve the problem can be:

• Create a method like RoadGeometry::SelectStrategy() that can be called after building the road geometry (before the init function we need to call at the last moment of the builder).
• Use an enum to identify the strategies and give the constructor an additional argument that would be use on the constructor definition.

I would like your input on this in order to select the appropriate solution for this particular issue @agalbachicar @francocipollone . I would like to explicitly say that this modification to our design would not affect the fact that the strategies are children classes from a base parent class (just like we discussed)

[agalbachicar]

Unsolicited review. PTAL

[agalbachicar]

@Voldivh I started to review the code before I had the chance to notice about your comment. Sorry about that. I might be missing something but I don't understand why you need to templetize also the Passkey in the Branchpoint and in the Junction classes. That makes no sense at all at a glance because you are only templetizing the constructor, not the entire RoadGeometry.

That said, we should keep in mind that this code will not be executed directly by backends, and we are trying to remove code duplication in the backends by offering in higher levels a common and performant solution. So it is OK to have more and potentially not that trivial code here in favor of not having to replicate cumbersome code in every backend implementation.

[francocipollone]

That makes no sense at all at a glance because you are only templetizing the constructor, not the entire RoadGeometry.

@agalbachicar You can't have a templatized parameter in the constructor that is not deductible by argument. And in this case we are not passing any argument so templatizing only the constructor isn't an option.

[francocipollone]

That makes no sense at all at a glance because you are only templetizing the constructor, not the entire RoadGeometry.

@agalbachicar You can't have a templatized parameter in the constructor that is not deductible by argument. And in this case we are not passing any argument so templatizing only the constructor isn't an option.

As a workaround, we could use a Helper argument like:

template <typename T>
struct Helper {
    using type = T;
};
// To be used:
RoadGeometry(Helper<BruteForceStragegy>{});

But it's a bit cumbersome.

[agalbachicar]

Oh, ok. Alternatively we could have 2 constructors:

• One that receives a std::unique_ptr
• One that calls the other with std::unique_ptr

That will remove the template argument which is an inconvenient decoration at this point.

[agalbachicar]

Sounds good to me. Let's proceed with the other comments now! Thanks @Voldivh

[francocipollone]

Added some suggestions on docstrings

[agalbachicar]

Checkpoint. Please wait until I finish with my review. I did not have the chance to full review the PR again.

[agalbachicar]

Consider trying with std::transform once more now that you'll store raw pointers: #517 (comment)

[agalbachicar]

Missing?

[agalbachicar]

Solving in favor for deferring the change to use pointers instead of ids.

[francocipollone]

It isn't possible as interestingly std::set only offers iterators that are constants: 😞

See https://en.cppreference.com/w/cpp/container/set/begin

Because both iterator and const_iterator are constant iterators (and may in fact be the same type), it is not possible to mutate the elements of the container through an iterator returned by any of these member functions.

[agalbachicar]

I have completed the review.

Let's move forward with the comments and then make sure that downstream the backends using geometry_base (malidrive - multilane) opt in for BruteForceStrategy and share the same results as before. That should provide some guarantees when tests pass that things remain the same at the integration level.
I will also suggest to add test coverage in a follow up PR. I can suggest a few tips to make things easier.

[agalbachicar]

Missing comment maybe?

[agalbachicar]

Thanks for addressing most of hte commetns .There are still some, ptal @Voldivh, great job !

[francocipollone]

I am tempted to initialize BruteForceStrategy by default and let the user initialize other by demand.
(Note that otherwise if InitailizeStrategy isn't called then the DoToRoadPosition and DoFindRoadPosition will throw because strategy_ being nullptr).

It wouldn't impact current backends and it doesn't impose a method call when you simply want to use the standard one.
Wdyt? @agalbachicar

[agalbachicar]

It's OK to keep the status quo but I think the default implementation must be the KDTree based one.

[francocipollone]

The thing is that the KDTree strategy should be initialize after the RoadGeometry is populated. So, we need to user to trigger the initialization while with the brute force one we can just define it as default value of strategy_.

Also the user should indicate the sampling step desired when using the kdtree strategy.

[agalbachicar]

Sounds good then.

[francocipollone]

Done.

[agalbachicar]

LGTM

[francocipollone]

Next steps:

• Add tests! At the moment in order to verify correct functioning I've tried Removes ToRoadPosition/FindRoadPosition implementations using a brute force algorithm maliput_malidrive#226 and run the tests locally and they all passed.
• Use it in the maliput backends.