Fix sth xyz transform (#77)

Signed-off-by: hoangtungdinh <[email protected]>
asam-ev · Aug 27, 2024 · e3bc56a · e3bc56a
1 parent d3854fc
commit e3bc56a
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 18 deletions.
diff --git a/qc_opendrive/base/utils.py b/qc_opendrive/base/utils.py
@@ -1517,22 +1517,22 @@ 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(
+    point_xyz = 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],
     )
+
+    return point_xyz
diff --git a/tests/test_utils.py b/tests/test_utils.py
@@ -131,21 +131,21 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
             0.0,
         ),
         ("simple_line_elevation.xodr", 0, 0, 0, 0, 0, 0),
-        ("simple_line_elevation.xodr", 5, 0, 0, 5, 0, 0),
-        ("simple_line_elevation.xodr", 5, 10, 0, 5, 10, 0),
-        ("simple_line_elevation.xodr", 5, -10, 0, 5, -10, 0),
+        ("simple_line_elevation.xodr", 5, 0, 0, 5, 0, 5),
+        ("simple_line_elevation.xodr", 5, 10, 0, 5, 10, 5),
+        ("simple_line_elevation.xodr", 5, -10, 0, 5, -10, 5),
         ("simple_line_heading_and_elevation.xodr", 0, 5, 0, -5, 0, 0),
         ("simple_line_heading_and_elevation.xodr", 0, -5, 0, 5, 0, 0),
-        ("simple_line_heading_and_elevation.xodr", 20, -5, 0, 5, 20, 0),
-        ("simple_line_heading_and_elevation.xodr", 20, 5, 0, -5, 20, 0),
+        ("simple_line_heading_and_elevation.xodr", 20, -5, 0, 5, 20, 20),
+        ("simple_line_heading_and_elevation.xodr", 20, 5, 0, -5, 20, 20),
         (
             "Ex_Line-Spiral-Arc_elevation.xodr",
             150,
             10,
             0,
             74.12961916583495,
             29.09285535716386,
-            0,
+            150,
         ),
         (
             "Ex_Line-Spiral-Arc_elevation.xodr",
@@ -154,7 +154,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
             0,
             88.45632996588242,
             15.137735950587523,
-            0,
+            150,
         ),
         (
             "simple_line_elevation.xodr",
@@ -235,7 +235,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
             0,
             -3.5355344833850797,
             50,
-            3.5355333282354717,
+            53.5355333282354717,
         ),
         (
             "simple_line_heading_and_elevation_and_superelevation.xodr",
@@ -244,7 +244,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
             0,
             3.53553448388698,
             50,
-            -3.5355333282354717,
+            50 - 3.5355333282354717,
         ),
         (
             "Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr",
@@ -253,7 +253,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
             0,
             83.82560356938673,
             19.64835535961951,
-            -3.5355333282354717,
+            150 - 3.5355333282354717,
         ),
         (
             "Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr",
@@ -262,7 +262,16 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
             0,
             78.76034556233064,
             24.58223594813187,
-            3.5355333282354717,
+            153.5355333282354717,
+        ),
+        (
+            "Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr",
+            150,
+            5,
+            10,
+            83.82560191408653,
+            19.648356971986246,
+            160.6066022954953,
         ),
         (
             "Ex_Line-Spiral-Arc_superelevation.xodr",
@@ -282,6 +291,15 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
             24.58223594813187,
             3.5355333282354717,
         ),
+        (
+            "Ex_Line-Spiral-Arc_superelevation.xodr",
+            150,
+            5,
+            10,
+            83.82560191408653,
+            19.648356971986246,
+            10.60660229549529,
+        ),
     ],
 )
 def test_get_point_xyz_from_road(file_name, s, t, h, x, y, z) -> None: