Fixes bug at ToRoadPosition query. #174
Conversation
| const maliput::api::RBounds lane_boundaries = lane_bounds(s); | ||
| const double r = maliput::math::saturate(unconstrained_prh[1], lane_boundaries.min(), lane_boundaries.max()); |
There was a problem hiding this comment.
Please, look at the following https://github.com/ToyotaResearchInstitute/maliput/blob/main/maliput/include/maliput/api/road_geometry.h#L85-L93
Quoted here:
/// The map from RoadGeometry to the `Inertial`-frame is not onto (as a bounded
/// RoadGeometry cannot completely cover the unbounded Cartesian universe).
/// If @p inertial_position does represent a point contained within the volume
/// of the RoadGeometry, then result distance is guaranteed to be less
/// than or equal to `linear_tolerance()`.
///
/// The map from RoadGeometry to `Inertial`-frame is not necessarily one-to-one.
/// Different `(s,r,h)` coordinates from different Lanes, potentially from
/// different Segments, may map to the same `(x,y,z)` `Inertial`-frame location.I think using segment_bounds() is correct because when doing ToLanePosition() you can map the entire Segment volume.
If an INERTIAL Frame point falls within the Segment volume it is expected that all RoadPositions (one per RoadPositions in the Segment) have distance close to zero. RoadGeometry::ToRoadGeometry should return the one that minimizes (closer to zero) the r-coordinate.
By looking at #173 , the mapping to lane 1_0_1 yields r = -1.97309e-10 and that's the minimum. The algorithm should return that instead of any other lane in the Segment.
There was a problem hiding this comment.
Let me know what you think about 26f8b7c.
It matches API definition and complies with both issues:
- CheckInvariants() outcome for some RoadGeometries #155 (comment).
- ToRoadPosition wrong behavior #173
( I had missed pressing the "comment" button)
This reverts commit 6df03e4.
francocipollone
changed the title
There was a problem hiding this comment.
LGTM
Let's add a ticket in multilane to account for this scenario as well.
Can we also create a test case out of issue #173 sample query that originated all this so we make sure it works in the future?
+1 |
francocipollone
deleted the
francocipollone/fixes_to_lane_position_bug
branch
|
Related to #173