Skip to content

Commit

Permalink
Fix the transformation with the right concept of elevation (#75)
Browse files Browse the repository at this point in the history 
Signed-off-by: hoangtungdinh <[email protected]>
  • Loading branch information
@hoangtungdinh
hoangtungdinh authored Aug 26, 2024
1 parent 5244160 commit 71aa2ac
Show file tree
Hide file tree
Showing 3 changed files with 158 additions and 30 deletions.
28 changes: 20 additions & 8 deletions qc_opendrive/base/utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1269,14 +1269,25 @@ def calculate_elevation_value(elevation: models.OffsetPoly3, s: float) -> float:
return f_elevation(s - elevation.s_offset)


def get_point_xyz_from_road_reference_line(
def get_point_xy_from_road_reference_line(
road: etree._ElementTree, s: float
) -> Union[None, models.Point3D]:
) -> Union[None, models.Point2D]:
geometry = get_corresponding_road_geometry_by_s(road, s)
if geometry is None:
return None

point_2d = get_point_xy_from_geometry(geometry, s)
if point_2d is None:
return None

return models.Point2D(x=point_2d.x, y=point_2d.y)


def get_point_xyz_from_road_reference_line(
road: etree._ElementTree, s: float
) -> Union[None, models.Point3D]:
point_2d = get_point_xy_from_road_reference_line(road, s)

if point_2d is None:
return None

Expand Down Expand Up @@ -1529,21 +1540,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)
pitch = get_pitch_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:
return None

ref_line_point = get_point_xyz_from_road_reference_line(road, s)
if ref_line_point is None:
return None

rotation = transforms3d.euler.euler2mat(yaw, pitch, roll, "rzyx")
d_point = rotation.dot(np.array([0.0, t, h]))

ref_line_point = get_point_xy_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=ref_line_point.z + d_point[2],
z=d_point[2],
)
98 changes: 98 additions & 0 deletions tests/data/utils/Ex_Line-Spiral-Arc_superelevation.xodr
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
<?xml version="1.0" standalone="yes"?>
<OpenDRIVE>
<header revMajor="1" revMinor="8" name="" version="1.00" date="Mon Oct 28 16:41:46 2019" north="0.0000000000000000e+00" south="0.0000000000000000e+00" east="0.0000000000000000e+00" west="0.0000000000000000e+00">
</header>
<road rule="RHT" length="2.3000000934150270e+02" id="1" junction="-1">
<link>
</link>
<type s="0.0000000000000000e+00" type="rural" country="DE"/>
<planView>
<geometry s="0.0000000000000000e+00" x="-5.6539792387543251e+01" y="-3.4394463667820069e+01" hdg="3.3186980419760154e-01" length="1.0000000000000000e+02">
<line/>
</geometry>
<geometry s="1.0000000000000000e+02" x="3.8003686923043311e+01" y="-1.8133261823256248e+00" hdg="3.3186980419884304e-01" length="2.9999999999999996e+01">
<spiral curvStart="0.0000000000000000e+00" curvEnd="1.3333327910466574e-02"/>
</geometry>
<geometry s="1.3000000000000000e+02" x="6.5603727689096445e+01" y="9.8074617455403796e+00" hdg="5.3186972285460032e-01" length="2.9999991463294364e+01">
<arc curvature="1.3333327910466574e-02"/>
</geometry>
<geometry s="1.5999999146329435e+02" x="8.7773023553010319e+01" y="2.9721920045249909e+01" hdg="9.3186944634590163e-01" length="2.0000000000000000e+01">
<spiral curvStart="1.3333327910466574e-02" curvEnd="6.6666666666666671e-03"/>
</geometry>
<geometry s="1.7999999146329435e+02" x="9.7828942354905308e+01" y="4.6971187858525226e+01" hdg="1.1318693921172343e+00" length="5.0000017878208354e+01">
<arc curvature="6.6666666666666671e-03"/>
</geometry>
</planView>
<elevationProfile>
<elevation s="0.0000000000000000e+00" a="0.0000000000000000e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
</elevationProfile>
<lateralProfile>
<superelevation s="0.0" a="0.785398" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
</lateralProfile>
<lanes>
<laneSection s="0.0000000000000000e+00">
<left>
<lane id="3" type="none" level="false">
<link>
</link>
<width sOffset="0.0000000000000000e+00" a="6.0000000000000000e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
</lane>
<lane id="2" type="border" level="false">
<link>
</link>
<width sOffset="0.0000000000000000e+00" a="1.6799999999999999e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
</lane>
<lane id="1" type="driving" level="false">
<link>
</link>
<width sOffset="0.0000000000000000e+00" a="3.5699999999999998e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
<roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="1.2000000000000000e-01" laneChange="none" height="1.9999999552965164e-02">
<type name="solid" width="1.2000000000000000e-01">
<line length="0.0000000000000000e+00" space="0.0000000000000000e+00" tOffset="0.0000000000000000e+00" sOffset="0.0000000000000000e+00" rule="no passing" width="1.2000000000000000e-01"/>
</type>
</roadMark>
</lane>
</left>
<center>
<lane id="0">
<link>
</link>
<roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="1.2000000000000000e-01" laneChange="both" height="1.9999999552965164e-02">
<type name="broken" width="1.2000000000000000e-01">
<line length="4.0000000000000000e+00" space="8.0000000000000000e+00" tOffset="0.0000000000000000e+00" sOffset="0.0000000000000000e+00" rule="caution" width="1.2000000000000000e-01"/>
</type>
</roadMark>
</lane>
</center>
<right>
<lane id="-1" type="driving" level="false">
<link>
</link>
<width sOffset="0.0000000000000000e+00" a="3.5699999999999998e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
<roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="1.2000000000000000e-01" laneChange="none" height="1.9999999552965164e-02">
<type name="solid" width="1.2000000000000000e-01">
<line length="0.0000000000000000e+00" space="0.0000000000000000e+00" tOffset="0.0000000000000000e+00" sOffset="0.0000000000000000e+00" rule="no passing" width="1.2000000000000000e-01"/>
</type>
</roadMark>
</lane>
<lane id="-2" type="border" level="false">
<link>
</link>
<width sOffset="0.0000000000000000e+00" a="1.6799999999999999e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
</lane>
<lane id="-3" type="none" level="false">
<link>
</link>
<width sOffset="0.0000000000000000e+00" a="6.0000000000000000e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
</lane>
</right>
</laneSection>
</lanes>
<objects>
</objects>
<signals>
</signals>
<surface>
</surface>
</road>
</OpenDRIVE>
62 changes: 40 additions & 22 deletions tests/test_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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, 5),
("simple_line_elevation.xodr", 5, 10, 0, 5, 10, 5),
("simple_line_elevation.xodr", 5, -10, 0, 5, -10, 5),
("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_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, 20),
("simple_line_heading_and_elevation.xodr", 20, 5, 0, -5, 20, 20),
("simple_line_heading_and_elevation.xodr", 20, -5, 0, 5, 20, 0),
("simple_line_heading_and_elevation.xodr", 20, 5, 0, -5, 20, 0),
(
"Ex_Line-Spiral-Arc_elevation.xodr",
150,
10,
0,
74.12961916583495,
29.09285535716386,
150.0,
0,
),
(
"Ex_Line-Spiral-Arc_elevation.xodr",
Expand All @@ -154,25 +154,25 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
0,
88.45632996588242,
15.137735950587523,
150.0,
0,
),
(
"simple_line_elevation.xodr",
0,
0,
10,
-7.071067811865475,
0,
7.0710678118654755,
0,
10,
),
(
"simple_line_elevation.xodr",
0,
0,
-10,
7.071067811865475,
0,
-7.0710678118654755,
0,
-10,
),
(
"simple_line_superelevation.xodr",
Expand Down Expand Up @@ -233,36 +233,54 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
50,
5,
0,
-3.535534483397321,
47.500000408510964,
52.499999591506345,
-3.5355344833850797,
50,
3.5355333282354717,
),
(
"simple_line_heading_and_elevation_and_superelevation.xodr",
50,
-5,
0,
3.53553448388698,
52.499999591489036,
47.500000408493655,
50,
-3.5355333282354717,
),
(
"Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr",
150,
-5,
0,
85.56999321017989,
21.43919391700692,
147.50000040849366,
83.82560356938673,
19.64835535961951,
-3.5355333282354717,
),
(
"Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr",
150,
5,
0,
77.01595592153748,
22.79139739074446,
152.49999959150634,
78.76034556233064,
24.58223594813187,
3.5355333282354717,
),
(
"Ex_Line-Spiral-Arc_superelevation.xodr",
150,
-5,
0,
83.82560356938673,
19.64835535961951,
-3.5355333282354717,
),
(
"Ex_Line-Spiral-Arc_superelevation.xodr",
150,
5,
0,
78.76034556233064,
24.58223594813187,
3.5355333282354717,
),
],
)
Expand Down

0 comments on commit 71aa2ac

Please sign in to comment.