diff --git a/doc/opendrive_link.md b/doc/opendrive_link.md index 486ad4708eaae3f6a1ae9efaa923c121cfe9cdb1..f43c3157abd6ba56ee60c3c5cf69526d73104e9d 100644 --- a/doc/opendrive_link.md +++ b/doc/opendrive_link.md @@ -5,7 +5,7 @@ OpendriveLink This class describes the connection between two [COpendriveRoadNode](opendrive_road_node.md) road nodes. This connection is defined in a single direction which coincides with the traffic direction. -The next image shows (as solid arrows) the set of COpendriveLink's defined for the example road presented in [here](), with the COpendriveRoadNode's objects (as red dots). +The next image shows (as solid black arrows) the set of COpendriveLink's defined for the example road presented in [here](), with the COpendriveRoadNode's objects (as red dots). <img src="images/links.png" alt="COpendriveLink objects created for the example road" width="607" height="544"> @@ -34,9 +34,26 @@ The main features of this class are: * Provides road mark information between its two road nodes. For lane links this will always be OD_MARK_NONE, but for non-lane links it will correspond to the actual road mark. * Provides the reference to the parent lane [COpendriveLane](opendrive_lane.md) only for lane links * Provides the reference to the parent road segment [COpendriveRoadSegment](opendrive_road_segment.md). -* It is possible to find the closest pose (minimum distance) on the link geometry to a given poition. +* It is possible to find the closest pose (minimum distance) on the link geometry to a given position. * It is possible to generate the actual trajectory (sub-trajectory) defined by the internal geometry (x and y coordinates and optionally the heading). * It is possible to get the pose and curvature at any point on the link trajectory. * In case of unexpected errors, this class throws an instance of CException. All other errors are reported to the user. -For further details on the implementation of these features check the doxygen API documentation. \ No newline at end of file +For further details on the implementation of these features check the doxygen API documentation. + +## Closest poses and link distances + +At this point it is important to pay some attention to how the closest poses and link distances are computed. + +In this case, the origin of the reference system is placed at the position of the starting road node. The positive x axis (in red) is pointing toward the ending road node and the positive y axis (in green) is always pointing to the left (the positive z axis would be up). + +The next image shows an example of how the closest poses are computed, as well as the reference systems, for two road segment (a straight segment on the left and a curve segment on the right). In the right image, the links (represented as solid black arrows) don't follow the center of the lane as they actually do. + +<img src="images/ref_system_links.png" alt="Closest poses and link distances computation for links" width="946" height="452"> + +When finding the closest pose belonging to a link, the desired pose of interest (yellow points in the previous image) is projected (closest distance) to the geometry that describes the links between road nodes (light red points in the previous image labeled L_\<origin_node\>_\<target_node\>). If the imaginary line between the position of interest and its projection into the link is not orthogonal to the link geometry, it is considered that the point of interest does not belong to the link, and therefore, the closest point does not exist. + +In case the closest pose exist, the **link distance** is always computed from the position of the starting road node, following the path defined by the link geometry as shown in the previous image. Do not confuse this distance with the closest euclidean distance between the point of interest and the closest point belonging to the link. + +When a lane has a single geometry, the closest poses and distances computations for lanes and links are equivalent. When a lane has multiple geometries, the actual closest pose is the same, but the link distance will be smaller than the lane distance, in general, because the first one is computed from the nearest node, and the second one from the start of the lane. See the documentation of the [COpendriveLane](opendrive_lane.md) class for more details. + diff --git a/include/opendrive_link.h b/include/opendrive_link.h index 675be45474866b9f315b31e789b6ee7a7559717e..bca711ab7cf501546230eba3f71e9c4a72921648 100644 --- a/include/opendrive_link.h +++ b/include/opendrive_link.h @@ -127,9 +127,10 @@ class COpendriveLink * with the link object. Otherwise the data on this parameters is not * valid. * - * \return the distance between the projected pose and the link start position if - * given pose coincide with the link object. Otherwise the maximum double - * value (std::numeric_limits<double>::max()) will be returned. + * \return the link distance between the projected pose and the link start position, + * following the link geometry, if given pose coincide with the link object. + * Otherwise the maximum double value (std::numeric_limits<double>::max()) + * will be returned. */ double find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose) const; /**