Fix sth xyz transform

Signed-off-by: hoangtungdinh <[email protected]>

qc_opendrive/base/utils.py

@@ -1517,22 +1517,20 @@ def get_point_xyz_from_road(
     road: etree._ElementTree, s: float, t: float, h: float
 ) -> Union[None, models.Point3D]:
     yaw = get_heading_from_road_reference_line(road, s)
-    # A reference line doesn't have pitch.
-    pitch = 0.0
     roll = get_roll_from_road_reference_line(road, s)
 
-    if yaw is None or pitch is None or roll is None:
+    if yaw is None or roll is None:
         return None
 
-    rotation = transforms3d.euler.euler2mat(yaw, pitch, roll, "rzyx")
+    rotation = transforms3d.euler.euler2mat(yaw, 0.0, roll, "rzyx")
     d_point = rotation.dot(np.array([0.0, t, h]))
 
-    ref_line_point = get_point_xy_from_road_reference_line(road, s)
+    ref_line_point = get_point_xyz_from_road_reference_line(road, s)
     if ref_line_point is None:
         return None
 
     return models.Point3D(
         x=ref_line_point.x + d_point[0],
         y=ref_line_point.y + d_point[1],
-        z=d_point[2],
+        z=ref_line_point.z + d_point[2],
     )