More performant ObjectQuery's implementation

[francocipollone]

Context

maliput::object::SimpleObjectQuery complies with the ObjectQuery API however its underlying implementation may not be as performant as desired.

The bottleneck happens when querying maliput by using methods like maliput::api::RoadGeometry::FindRoadPositions(or ToRoadPosition). The implementations of these methods are full responsibility of the backend, and so, maliput_object's performance is directly affected by the maliput backend that is being used.

When using maliput_malidrive backend, how the SimpleObjectQuery implementation performs, is directly affected by the number of lanes of the RoadNetwork as the implementation uses brute force when finding the lanes via FindRoadPosition.

Some time measurements.

Number of lanes in the TShapeRoad: 12
maliput_query --xodr_file_path=TShapeRoad.xodr --omit_nondrivable_lanes -- FindOverlappingLanesIn intersected 5 5 5 15 15 0 0 0 0 
>> Elapsed Query Time: 0.00292117 s

Number of lanes in the Town01: 202
maliput_query --xodr_file_path=Town01.xodr --omit_nondrivable_lanes -- FindOverlappingLanesIn intersected 5 5 5 15 15 0 0 0 0 
>> Elapsed Query Time: 0.0733985 s

Number of lanes in the Town07: 754
maliput_query --xodr_file_path=Town07.xodr --omit_nondrivable_lanes -- FindOverlappingLanesIn intersected 5 5 5 15 15 0 0 0 0 
>> Elapsed Query Time: 0.291728 s

Proposal

We could detach from the maliput backend implementation's performance by proposing a re-organization of the RoadNetwork space into a convenient space-partitioning data structure (like kdtree).

Therefore we could create a layer on top of maliput, to reorganize the space and provide FindRoadPositions methods like that can be used later on by this new ObjectQuery implementation.

graph TD;
    maliput_object-->maliput_kdtree;
    maliput_kdtree-->maliput;
    maliput-->maliput_malidrive;
    maliput-->maliput_multilane;
    maliput-->maliput_dragway;
    maliput-->maliput_****;

Loading

Suggested Steps for designing

• Define the requirements/constraints for the maliput_kdtree layer/package.
  • Should it be part of maliput package? (I guess answer of this will be directly connected to whether which kdtree implementation is going to be used (third-library or homemade)
  • Which methods are expected to provide? Thinking about how is it going to be consumed by the ObjectQuery's implementation. (plus other we consider interesting to have)
• Define the requirements/constraints for the kdtree implementation:
  • Which are the queries we should provide to fulfill maliput_kdtree necessities. (closest neighbor, etc)
  • Third library or homemade?
    Our space will be static (once the roadnetwork is created there aren't expected modifications.)

Note: This issue's description is expected to be iterated if needed

[francocipollone]

CC: @agalbachicar @tfoote I expect this issue to trigger the creation of implementation-related issues in this and other repos

[agalbachicar]

One slightly different way to approach this would be to first define which queries are required from maliput_object to a maliput::api::RoadGeometry and then define next steps.

From reviewing the code and reading this issue I suspect we just need to do:

• Inertial Frame coordinate to Lane Frame coordinate transform.
  • This is currently covered by maliput::api::RoadGeometry::FindRoadPositions(), maliput::api::RoadGeometry::ToRoadPosition() and maliput::api::Lane::ToLanePosition()
• Intersection between volume / surface defined in the Inertial Frame and Lanes.
  • This is not covered by any method in the maliput::api namespace. It is solved with a custom implementation for bounding boxes in maliput_object.

Am I missing any other query?

[agalbachicar]

I would suggest considering always the possibility of implementing the fast approach once and for all backends. Most probably, in maliput::geometry_base.

[francocipollone]

Inertial Frame coordinate to Lane Frame coordinate transform.
This is currently covered by maliput::api::RoadGeometry::FindRoadPositions(), maliput::api::RoadGeometry::ToRoadPosition() and maliput::api::Lane::ToLanePosition()

In my mind those are the queries I was expecting yes.

Intersection between volume / surface defined in the Inertial Frame and Lanes

I haven't thought about it. Is it something that RoadGeometry should provide? Shouldn't it be part of maliput_object?

I would suggest considering always the possibility of implementing the fast approach once and for all backends. Most probably, in maliput::geometry_base.

How that approach would be? This new RoadGeometry implementation would already reorganize the space into a kdtree, in a way that all the RoadGeometry queries would be under the hood using this?
That's interesting for avoiding having an extra layer on top of maliput. Maliput would automatically boost performance.

[agalbachicar]

Is it something that RoadGeometry should provide? Shouldn't it be part of maliput_object?

Consider the following overloads in the RoadGeometry:

std::vector<RoadPositionResult> FindRoadPositions(const InertialPosition& inertial_position, double radius) const;

std::vector<RoadPositionResult> FindRoadPositions(const BoundingRegion& bounding_region) const;

Jumping from one to another seems reasonable, actually, the former query can be converted into the second one as soon as we define a sphere as a BoundingRegion implementation.

[agalbachicar]

Shouldn't it be part of maliput_object?

The only drawback that I see in not having a collection as a KD-tree or R-tree oustide the RoadGeometry is that it becomes really hard to leverage the power of the structure to compute intersections between objects themselves. We could register those objects in the same collection but posterior filtering may degrade the performance. It is also true that if you have an object in space it is very likely to be on the road surface and not flying.

[agalbachicar]

That's interesting for avoiding having an extra layer on top of maliput. Maliput would automatically boost performance.

Exactly. We could either offer 2 different queries to convert to and from Lane space and Inertial space depending on the algorithm to use.

Aside from that, we might use this to implement another type of backend that relies on labeled point clouds. Any of the current backends might be exportable to a file that contains information about the connectivity graph, and then all the points per lane. We could use any interpolation method to derive points in between and keep using the kd-tree struct as backend.

[agalbachicar]

@stonier @liangfok what are your thoughts about this comment ?

[francocipollone]

Shouldn't it be part of maliput_object?

The only drawback that I see in not having a collection as a KD-tree or R-tree oustide the RoadGeometry is that it becomes really hard to leverage the power of the structure to compute intersections between objects themselves. We could register those objects in the same collection but posterior filtering may degrade the performance. It is also true that if you have an object in space it is very likely to be on the road surface and not flying.

Probably more related to implementation however I agree about probable degradation in performance in that case, nevertheless, one thing that came up to my mind now is that even though at the moment the objects are expected to be static, if we add the ability to change their position, then the tree (for the kdtree impl) will need recomputation. Again, more related to implementation but I wanted to raise that.

Exactly. We could either offer 2 different queries to convert to and from Lane space and Inertial space depending on the algorithm to use.

This approach sounds like the most beneficial from a user perspective. Like it.

[francocipollone]

I would suggest considering always the possibility of implementing the fast approach once and for all backends. Most probably, in maliput::geometry_base.

Proof of concept

I've been working on a proof of concepts for this approach, I created two branches, one in maliput and the other one in maliput_malidrive.

Disclaimer: This was implemented for testing purposes and the final approach will certainly be different to this implementation.

• maliput --> francocipollone/kdtree_poc
  
• maliput_malidrive --> francocipollone/kdtree_poc
  

Considerations

The RoadGeometry implementation in maliput::geometry_base allows you to enable the Spacial Reorganization (via kdtree) from the backend when building the RoadGeometry. In consequence, the ToRoadPosition method will use the kdtree space for getting the Lane that is closer to the Inertial position passed to the ToRoadPosition.

The kdtree was filled by uniformly sampling the RoadNetwork every 10 cm, in consecuence the RoadGeometry build time takes a bit longer when using enabling this spacial reorganization.

I used maliput_integration's maliput_query app for checking the improvement in time when calling the RoadGeometry::ToRoadPosition method.

Trying this out

Xodr file map being used: Town07
Number of lanes: 754
Query being analyzed: RoadGeometry::ToRoadPosition

Brute force algorithm (Current behavior)

franco@fe0fd227783d:~/maliput_ws_focal$ maliput_query --xodr_file_path=Town07.xodr --omit_nondrivable_lanes -- ToRoadPosition 15 15 0 
[INFO] Loading road network using malidrive backend implementation...
[INFO] RoadNetwork loaded successfully in 1.5028 seconds.
ToRoadPosition(inertial_position: (x = 15, y = 15, z = 0))
              : Result: nearest_pos:(x = 15.061, y = 8.73938, z = 0.0267639) with distance: 6.26098
                RoadPosition: (lane: 26_0_-2, lane_pos: (s = 16.1251, r = -1.75, h = 0))
Elapsed Query Time: 0.0387208 s

Usign kdtree (proof of concept)

franco@842e66ec5c4a:~/maliput_ws_focal$ maliput_query --xodr_file_path=Town07.xodr --omit_nondrivable_lanes -- ToRoadPosition 15 15 0 
[INFO] Loading road network using malidrive backend implementation...
[INFO] RoadNetwork loaded successfully in 5.20544 seconds.
ToRoadPosition(inertial_position: (x = 15, y = 15, z = 0))
              : Result: nearest_pos:(x = 15.061, y = 8.73938, z = 0.0267639) with distance: 6.26098
                RoadPosition: (lane: 26_0_-2, lane_pos: (s = 16.1251, r = -1.75, h = 0))
Elapsed Query Time: 0.000170596 s

Summary of tested maps

map	N Lanes	Brute Force - RN Load Time[s]	Kdtree - RN Load Time[s]	Load Time Change	Brute Force - ToRoadPosition Query time[s]	Kdtree - ToRoadPosition Query time[s]	Query time Change
Town07	724	1.5028	5.20544	+246%	0.0387208	0.000170596	-99.5594%
TShapeRoad	12	0.0541779	0.149824	+176.5%	0.000419402	0.000053993	-87.1%

Conclusions

From the results:

• Map load time is extended about 2/3 times due to the uniform RoadNetwork sampling (Entire surface using 10cm step).
• Query time is massively improved --> About 200 times better

This was just a proof of concept and there is a lot of room for improvements.