build ParamPoly3 geometry from point list

[mljack]

Hi, do we have plans to support building ParamPoly3 geometry from a point list? That would be very useful to buid opendrive maps from real world data.

[mander76]

Not really no, it would be a very nice addition. But what I want to do and have time for is not the same :(

[johschmitz]

We've been discussing this a bit in the standardization meetings and I think it would be good if OpenDRIVE could support Cubic splines and/or Bezier splines and then some established libraries for spline interpolation could be used to solve this problem. Let's see if this can be added at some point.

[StephenHU-sh]

I'm really looking forward to it being implemented, because it's very useful.

[johschmitz]

I already thought about some ways how to implement this. Currently my plan is to use the Bezier spline curve of Blender as a basis for the calculations. What I need as a general basis for this feature though is the ability to support multiple geometries per road. That is also something needed to properly create junction connecting roads.

[johschmitz]

@mander76 I am trying to export parampoly3 roads now but I am facing some issues. I also found a TODO in the relevant scenariogeneration lib code; https://github.com/pyoscx/scenariogeneration/blob/ba864c5694e3da0c776f1b9a387f8783dea30cd0/scenariogeneration/xodr/geometry.py#L813 Here is a screenshot of what happens and the file attached:

example.zip

Maybe my definition of the function is different or wrong. If I figure it out I am going to submit a pull request. Otherwise maybe you have an idea what is going on.

Edit: Could also be that this issue has nothing to do with scenariogeneration lib and is just between my tool and esmini, not sure at the moment.

[johschmitz]

Hm okay I think the coordinate systems are slightly different and setting the coefficients au and av to zero solves the problem. I think you then do the math to calculate the proper x and y values on your side.

    <geometry s="50.0" x="50.0" y="0.0" hdg="0.0" length="38.60865479713262">
        <paramPoly3 aU="0" bU="36.05551528930664" cU="11.334892272949219" dU="-17.39040756225586" aV="0" bV="0.0" cV="24.54518222808838" dV="-4.545182228088379" pRange="normalized"/>
    </geometry>

[johschmitz]

There seems to be another problem though See example: example.zip

@mander76 I am unsure how to provide the heading for individual geometry sections, maybe there is a bug in the calculation.

Edit: Sorry, I now feel that I have somewhat hijacked this issue since my problem is slightly out of scope with regards to the original issue. I do have an idea now though how to solve the original issue, so if you want to discuss/brainstorm about this a bit let me know.

[mander76]

Hmm, I have colleagues that uses this quite often and have no problem.. but why do you need to set the heading? it will create a continuous road without the heading entry?

[johschmitz]

@mander76 Are you sure they use it with multiple geometries in a single road or is it multiple roads with a single parampoly3 geometry? Both should work.

Edit: Of course there might also be a mistake on my end, quite possible.

[mander76]

yes I helped them a couple of weeks ago with it.

I will have to check if some "trick" is used though... let me get back on this...

[eknabevcc]

I suggest the following minor adjustments in the provided OpenDRIVE file:

1. For each new paramPoly3 segment, set aU and bU to zero (we want the segment/local coordsystem to start at the specified x,y position)
2. Adjust start x value in the third and last segment from 70 to 50 (sum of 20 + 30 m from the two first straight segments)

As in attached file: bdsc_export-mod.zip

Then it looks correct, both in esmini odrviewer and https://odrviewer.io/

[mander76]

Yes, @eknabevcc is right, the package assumes that all geometires have a x=0, y=0 and heading=0 when it puts them together. I double checked with my colleague and he confirmed that when he used multiple geometires, he had to rotate them to heading 0..

This should probably be fixed, but not sure how to do it to be honest, then it's a specific thing only for PolyParam3 type of geometries.

[johschmitz]

@mander76 I don't get it, so am I not able to build arbitrary parampoly3 geometries? Can't there be an option to supply x and y for each geometry segment? So instead of

elif geometry_section['curve_type'] == 'parampoly3':
    geometry = xodr.ParamPoly3(
                    au=0,
                    bu=geometry_section['coefficients_u']['b'],
                    cu=geometry_section['coefficients_u']['c'],
                    du=geometry_section['coefficients_u']['d'],
                    av=0,
                    bv=geometry_section['coefficients_v']['b'],
                    cv=geometry_section['coefficients_v']['c'],
                    dv=geometry_section['coefficients_v']['d'],
                    prange="normalized",
                    length=geometry_section['length'])

I would do

elif geometry_section['curve_type'] == 'parampoly3':
    geometry = xodr.ParamPoly3(
                    x=geometry_section['point_start'].x,
                    y=geometry_section['point_start'].y,
                    hdg=geometry_section['heading_start'],
                    au=0,
                    bu=geometry_section['coefficients_u']['b'],
                    cu=geometry_section['coefficients_u']['c'],
                    du=geometry_section['coefficients_u']['d'],
                    av=0,
                    bv=geometry_section['coefficients_v']['b'],
                    cv=geometry_section['coefficients_v']['c'],
                    dv=geometry_section['coefficients_v']['d'],
                    prange="normalized",
                    length=geometry_section['length'])

And you would recognize that and not recalculate the values or something like that?

Anyway I also do

planview.set_start_point(obj['geometry'][0]['point_start'][0],
                        obj['geometry'][0]['point_start'][1],obj['geometry'][0]['heading_start'])

So this should also be possible for each geometry segment I think. Without such a feature I have a problem...

[johschmitz]

I am just wondering why I do not have this problem for the other geometries.. do you always put them together via calculating their start and end heading direction? If so why can't you do the same for the parampoly3 geometry? Maybe this is a bug?

Still it should be possible to define all the values for all segments manually. I just found there was a method add_fixed_geometry(self, geom, x_start, y_start, h_start, s=None), maybe that is what I am looking for.

[johschmitz]

Alright, add_fixed_geometry() solved my problem but I think the bug should also be fixed because it works for all other geometries with just add_geometry(). My guess is something like you do not calculate the heading correctly but only in the case of parampoly3 geometries or so. Maybe this equation is slightly wrong for some cases: https://github.com/pyoscx/scenariogeneration/blob/ba864c5694e3da0c776f1b9a387f8783dea30cd0/scenariogeneration/xodr/geometry.py#L887-L890 Edit: I am not sure at which point of the program this is evaluated so I was not able to quickly debug this. Let me know if you can not find it, then I can try to spend more time.

[mander76]

I'll try to have time to sit with it soon, have quite a lot right now.. But it's something there that is messing things up.. I have to setup a bunch of examples to make sure it works properly..

[mander76]

Hmm, I was looking into this both standard and had a discussion with @eknabevcc . Interestingly in the standard we found this: **

To simplify representation, the local coordinate system should be aligned with the s/t-coordinate system at the start point (@x,@y) and start orientation @hdg:

u points in local s-direction, meaning along the reference line at the start point

v points in local t-direction, meaning in lateral deviation from the reference line at the start point

the parameters @aU, @aV and @bV shall be zero, @bU shall be > 0

**

So technically three inputs to the constructor of polyparam3 could be removed and just set to zero to be compliant with the standard. And in both your (@johschmitz) exampels this is not true.

There are two options to handle this in the package as I see it:

1. just throw error when one of these values are non-zero (and bU < 0)
2. rotate and translate all parameters so that aU, aV and bV are zero, and write out a warning that the parameters has been recalculated to fit the standard.

[mander76]

rotation_example.zip Made an example how the rotation could be done.

Await your @johschmitz feedback before I incorporate it into the package

[johschmitz]

I think what you are citing there is only relevant for the first geometry segment of a road if I understand it correctly. You can play with the latest release of my Blender addon to build a road with multiple parampoly3 geometrie segments for comparison (hold Shift to ads additional segments, maybe also use Ctrl for snapping and Alt to modify the start heading and shift+mousewheel for the end heading).

I can also upload an example later myself and look at your example to provide additional feedback.

[johschmitz]

Here is the example example.zip

[johschmitz]

Alright, I also looked at your example, it is okay I guess but I think you should definitely extend it also to the case that you have one road and two or three parampoly3 geometries/geometry segments inside that single road. Then you will see that it is not so easy just with add_geometry().

Regarding the magic you describe

There are two options to handle this in the package as I see it:

1. just throw error when one of these values are non-zero (and bU < 0)
2. rotate and translate all parameters so that aU, aV and bV are zero, and write out a warning that the parameters has been recalculated to fit the standard.

At which point would you modify the user input and for which API? You mean when the user calls add_geometry() and does not provide any start point and heading? I think in the case that the user provides all information I would not modify his input data and rather just throw a warning to let him figure out his mistake. But to be honest I am not sure if that is good because it is not really necessary to set @bV to zero, it just makes it harder to solve the equations I think because you need to add additional constraints. Not sure why they ask for it in the standard. I am not even sure right now what is the proper way to do this. If you know please tell me. I will also try to read up on this. Maybe you can throw a warning when the a values are non zero, so far I did not see a reason to have them non-zero. I would say when the user only provides each segment but not the necessary x, y and headings to put them together in a continuous way, then you can think about rotating them and connecting them properly like it happens with the other geometries.

But maybe eventually this will also lead to a helper function like requested by @mljack which uses something like https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.CubicSpline.html or so in order to build a C2/G2 continous parametric cubic spline through a list of given points and then adds them as geometries to a road.

[johschmitz]

Okay, I actually do see one use-case for non zero a values. If you intentionally want to build a "broken" non-continous OpenDRIVE file for some tests, it should be possible to do so.

[bv]

Alright, I also looked at your example, it is okay I guess but I think you should definitely extend it also to the case that you have one road and two or three parampoly3 geometries/geometry segments inside that single road. Then you will see that it is not so easy just with add_geometry().

Regarding the magic you describe

There are two options to handle this in the package as I see it:

1. just throw error when one of these values are non-zero (and bU < 0)
2. rotate and translate all parameters so that aU, aV and bV are zero, and write out a warning that the parameters has been recalculated to fit the standard.

At which point would you modify the user input and for which API? You mean when the user calls add_geometry() and does not provide any start point and heading? I think in the case that the user provides all information I would not modify his input data and rather just throw a warning to let him figure out his mistake. But to be honest I am not sure if that is good because it is not really necessary to set @bV to zero, it just makes it harder to solve the equations I think because you need to add additional constraints. Not sure why they ask for it in the standard. I am not even sure right now what is the proper way to do this. If you know please tell me. I will also try to read up on this. Maybe you can throw a warning when the a values are non zero, so far I did not see a reason to have them non-zero. I would say when the user only provides each segment but not the necessary x, y and headings to put them together in a continuous way, then you can think about rotating them and connecting them properly like it happens with the other geometries.

But maybe eventually this will also lead to a helper function like requested by @mljack which uses something like https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.CubicSpline.html or so in order to build a C2/G2 continous parametric cubic spline through a list of given points and then adds them as geometries to a road.

hello guys, I believe you don't even realise, that you are tagging me by mentioning @bV

[mander76]

Alright, I also looked at your example, it is okay I guess but I think you should definitely extend it also to the case that you have one road and two or three parampoly3 geometries/geometry segments inside that single road. Then you will see that it is not so easy just with add_geometry(). Regarding the magic you describe

There are two options to handle this in the package as I see it:

1. just throw error when one of these values are non-zero (and bU < 0)
2. rotate and translate all parameters so that aU, aV and bV are zero, and write out a warning that the parameters has been recalculated to fit the standard.

At which point would you modify the user input and for which API? You mean when the user calls add_geometry() and does not provide any start point and heading? I think in the case that the user provides all information I would not modify his input data and rather just throw a warning to let him figure out his mistake. But to be honest I am not sure if that is good because it is not really necessary to set @bV to zero, it just makes it harder to solve the equations I think because you need to add additional constraints. Not sure why they ask for it in the standard. I am not even sure right now what is the proper way to do this. If you know please tell me. I will also try to read up on this. Maybe you can throw a warning when the a values are non zero, so far I did not see a reason to have them non-zero. I would say when the user only provides each segment but not the necessary x, y and headings to put them together in a continuous way, then you can think about rotating them and connecting them properly like it happens with the other geometries. But maybe eventually this will also lead to a helper function like requested by @mljack which uses something like https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.CubicSpline.html or so in order to build a C2/G2 continous parametric cubic spline through a list of given points and then adds them as geometries to a road.

hello guys, I believe you don't even realise, that you are tagging me by mentioning @bV

Haha sry :P

[mander76]

@johschmitz I don't agree that is has to be the first geometry, not according to the text atleast. Since each geometry has the same entries, and the road/planview has no geometric information at all.

My guess why they want "bv" = 0 is that "bv" is that the initial heading of a road should be determined by the hdg input, having both "bv" and "hdg" will make two separate inputs for the inital heading of the road.

I would say when the user only provides each segment but not the necessary x, y and headings to put them together in a continuous way, then you can think about rotating them and connecting them properly like it happens with the other geometries.

That is more or less what I'm proposing, add_gemoetry() is made to create a "correct, continuous" (almost regretting the heading input to that function). Option 2 is basically a rotation so that "bv" = 0. and putting "av" and "au" is just a simple translation.

it is not really necessary to set "bV" to zero" This can be said for "aV" and "aU" aswell, since these can be adjusted with the inital x and y aswell.

So technically there is an option 3 to my list, and that is to adjust the inital heading and x,y to make the road correct.

Okay, I actually do see one use-case for non zero a values. If you intentionally want to build a "broken" non-continous OpenDRIVE file for some tests, it should be possible to do so.

This is always possible to create whatever you want using add_fixed_geometry

[johschmitz]

@bv: I did realise that I @ mentioned you and changed it but it was already to late.. Sorry for that but the user name is kind of unfortunate in this case, hehe.

@mander76 my interpretation now is that the road should be smooth and the v axis is perpendicular to the road and hence the b component needs to be 0 in order to achieve the smoothness. And you are right, it needs to be the case for every geometry segments since each one comes with its own heading. Is it true though that bu and bv are equivalent to a rotation or do they not also change the shape of the curve? I need to think about this one, it is not obvious to me.

[johschmitz]

Here is an example plot: https://www.wolframalpha.com/input?i=ParametricPlot%5B%7B1*t%2B1*t%5E2%2B0.5*t%5E3%2C+0*t%2B0.2*t%5E2%2B0.3*t%5E3%7D%2C+%7Bt%2C+0%2C+1%7D%5D With that it is easy to play with the parameters. I think reparameterization for a certain rotation might be possible (not sure to be honest) but I don't think it is super straightforward.

[johschmitz]

Okay I went through the equations and I think it is actually not that hard to rotate and reparameterize.

Here is an example for a 90 degree rotation (non 90 degrees rotation are difficult to view in the Wolfram Alpha plot due to changing aspect ratios).

Before https://www.wolframalpha.com/input?i=ParametricPlot%5B%7B1*t+%2B+1*t%5E2+%2B+0.5*t%5E3%2C+0*t+%2B+0.2*t%5E2+%2B+0.3*t%5E3%7D%2C+%7Bt%2C+0%2C+1%7D%5D

After rotation https://www.wolframalpha.com/input?i=ParametricPlot%5B%7B%281*cos%2890%29%29*t%2B%281*cos%2890%29-0.2*sin%2890%29%29*t%5E2%2B%280.5*cos%2890%29-0.3*sin%2890%29%29*t%5E3%2C+%281*sin%2890%29%29*t%2B%281*sin%2890%29%2B0.2*cos%2890%29%29*t%5E2%2B%280.5*sin%2890%29%2B0.3*cos%2890%29%29*t%5E3%7D%2C+%7Bt%2C+0%2C+1%7D%5D

This assumes that (x,y)=(ucos(θ)−vsin(θ),usin(θ)+vcos(θ)) and θ=0 degrees for the first plot and θ=90 degrees for the second plot.

I don't find it a good idea though. I would return to my initial idea that bV=0 and bU>0 should only be necessary for the first geometry segment and then we can use the same heading for all segments and then it is much simpler to calculate a spline through a number of points resulting in multiple segements without having to reparameterize each one of them indiviually. Not sure what the advantage of the reparameterization for each segment is. It is a lot of additional calculations and it probably just increases some numerical errors. Also, actually one of the ideas for OpenDRIVE 1.8 was to clarify the equations necessary for building a long spline through a series of points using parampoly3 but there was no time for this. I think this discussion will come up with OpenDRIVE 1.9 again.

[mander76]

yes, the equations are not that hard to deal with. It would be nice though if the standard would give clear equations on this.

I had another idea and that is to add the translation/rotation as an option to the PolyParam3 class, "recalculate_ab_coefficients", which will then just recalculate "bV" = 0, or maybe provide a helper_function that does this..

Many options but not sure what way to go :-/

[johschmitz]

My suggestion would be to first implement what OP asked for and then think about what to do because only then do we fully understand the problem.

Here is a Python script to calculate the coefficients (and plot the curve):

import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline

# Given set of points
x = np.array([1, 2, 3, 4, 5, 0])
y = np.array([1, 3, 2, 4, 3, 0])
t = np.array([0, 1, 2, 3, 4, 5])

# Calculate the parameters for the cubic spline

spl_x = CubicSpline(t, x, bc_type='natural')
spl_y = CubicSpline(t, y, bc_type='natural')

# Plotting the resulting curve
t_plot = np.linspace(0, len(x)-1, 100)
x_plot = spl_x(t_plot)
y_plot = spl_y(t_plot)

plt.figure()
plt.plot(x, y, 'o', label='Control Points')
plt.plot(x_plot, y_plot, label='Cubic Spline')
plt.legend()
plt.xlabel('x')
plt.ylabel('y')
plt.title('Cubic Spline Interpolation')
plt.grid(True)

# Manually calculate the spline segments
segments = len(x) - 1
du = spl_x.c[0,:]
cu = spl_x.c[1,:]
bu = spl_x.c[2,:]
au = spl_x.c[3,:]
dv = spl_y.c[0,:]
cv = spl_y.c[1,:]
bv = spl_y.c[2,:]
av = spl_y.c[3,:]

# Plotting the manually calculated spline segments
for i in range(segments):
    t_manual = np.linspace(0, 1, 100)
    x_manual = np.zeros_like(t_manual)
    y_manual = np.zeros_like(t_manual)
    print(f"Segment {i+1}:")
    print(f"  au = {au[i]:.4f}, bu = {bu[i]:.4f}, cu = {cu[i]:.4f}, du = {du[i]:.4f}")
    print(f"  av = {av[i]:.4f}, bv = {bv[i]:.4f}, cv = {cv[i]:.4f}, dv = {dv[i]:.4f}")
    x_manual = au[i] + bu[i] * t_manual + cu[i] * t_manual**2 + du[i] * t_manual**3
    y_manual = av[i] + bv[i] * t_manual + cv[i] * t_manual**2 + dv[i] * t_manual**3

    plt.plot(x_manual, y_manual, '--', label=f'Cubic Spline Segment {i+1} - Manual calculation')

plt.legend()
plt.show()

Basically we just need to use the CubicSpline() method from Scipy two times. @mander76 What do you think how hard would it be to implement a helper which integrates this? Something like add_parampoly3_spline_geometries()or so which internally calls a bunch of add_geometry() or add_fixed_geometry() with parampoly3.

Edit: updated the script.

[ryzary]

Hi @johschmitz . I am also interested in the idea of building ParamPoly3 geometry from a list of points. I saw your latest comment on this thread. Could you explain what is t here? Is it the same as p as in the OpenDrive 1.6 spec ?

[johschmitz]

That's correct, sometimes it is called t and sometimes p or something else but it is the same parameter.