MoveAlongS messes up pitch and roll?

[brifsttar]

Hey Emil,

I'm encountering an issue with MoveAlongS's handling of pitch and roll. My test case is in Unreal, but after having squashed a few bugs on my side of things, I think this one is from RoadManager. Or at least, I'm having a hard time understanding a few things.

So my test case is the following:

• I'm working with an xodr that has elevation on roads
• I have an actor located on a road going slightly down, so I align the actor (more or less) with it, leading to a -1° pitch angle
• I call MoveAlongS on that actor to spawn another actor 10cm behind it
• The new actor seems to have invalid pitch and roll
  • Pitch is at +1.3°, which is a 2+° offset from the reference point
  • A very slight roll angle appears, which is not really problematic but seems weird

Below are two images that show the reference point, then the one created from MoveAlongS.

I'm focusing on the pitch error for the moment. On this road, the ~2° offset seems constant: if I set my reference point at a 45° pitch, the spawned actor will be at ~47° pitch. I have a feeling that this offset is related to the road angle, but I haven't run more test on it (e.g., create an unrealistic road with a 45° pitch angle).

On the code side of things, MoveAlongS calls SetLanePos, which then calls EvaluateZHPR and then onto R0R12EulerAngles, and it seems that's where the p_ and r_ get messed up (or at least that's what my conditional breakpoints are telling me).

However, I suck at math and I'm having a hard time figuring out whether there is an issue with this code, or if I should look elsewhere. I'm using the default PosMode, which is relative for pitch and roll. I tried with absolute, but then as expected those coordinate don't update on the MoveAlongS call (which is an issue for longer MoveAlongS distances).

If you want to look into it, here is the xodr file (as txt so GitHub allows it). Below are the coordinate for my reference point:

x: -628.92144531250005
y: -1815.2664062500000
z: -0.22668130874633791
h: -0.29530104428854087
p: -0.019211324305475123
r: -5.0596909065038617e-14

Could you help me figure out why the pitch is offset during this MoveAlongS? Let me know if I can be of any help.

Thank you as always for your work :)

Bertrand

[brifsttar]

For information, there seem to be an easy workaround:

double pr = p.GetPRelative();
double rr = p.GetRRelative();
ret = p.MoveAlongS(...);
p.SetPitchRelative(pr);
p.SetRollRelative(rr);

A quick test of this code seem to generate correct pitch and roll.

[brifsttar]

I'm still bad at math, but I tried to follow this, and it seem they compute the pitch angle differently than you are. So I replaced the following:

https://github.com/esmini/esmini/blob/da1db1aa15c8e01dcdf7ceebe915fe26bb75f3dc/EnvironmentSimulator/Modules/CommonMini/CommonMini.cpp#L919

...with...

p = GetAngleInInterval2PI(asin(R2[2][0]));

...and it kind of seem to be working? I removed the minus just because I noticed that the result seemed oddly to have the wrong sign, so... I figured maybe it has something to do with LHC/RHC (or maybe I'm plain wrong).

But once again, I'm very bad at all this and I absolutely do not feel 1% confident enough to throw this in a PR. So if you have time to look at it, that would be greatly appreciated. In the meantime, my workaround does the trick :-)

Bertrand

[eknabevcc]

Thanks for investigation effort! I've started looking into it, but was interrupted by other tasks. What version of esmini are you using? For my further investigation.

[brifsttar]

Thank you for looking into it!

I upgraded to 2.33.2 (da1db1aa15c8e01) to check if the latest release had this issue (it does), which is where I'm currently running all my tests. Before that I identified the issue in 2.31.9 (371e593c133).

[eknabevcc]

I think I found at least one potentially related bug. How do you set the position in first place? WorldPosition, LanePosition...?

[brifsttar]

Good question, here's the current code I'm using:

	FVector p = CoordTranslate::UeToOdr::Location(T.GetLocation());
	FVector r = CoordTranslate::UeToOdr::Rotation(T.Rotator().Euler());
	_TrackPos.SetX(p.X);
	_TrackPos.SetY(p.Y);
	_TrackPos.XYZ2TrackPos(p.X, p.Y, p.Z, false, -1, false, _HintRoad);
	_TrackPos.SetZ(p.Z);
	_TrackPos.SetHeading(r.X);
	_TrackPos.SetPitch(r.Y);
	_TrackPos.SetRoll(r.Z);
	//_TrackPos.EvaluateOrientation();

The last line is commented because during my initial tests, I had noticed that it seemed to cause the issue, and I wasn't really sure why I needed it.

At first, I wasn't even using MoveAlongS: a simple Inertial -> Track -> Inertial conversion seemed to be messing up pitch and roll. Commenting the line seemed to fix the issue without having side effects, so I moved on to the MoveAlongS issue, which it now seems to be caused by the same function (which got renamed EvaluateZHPR in the meantime since I merged master).

[eknabevcc]

Thanks for detailed info, very helpful. I'll continue investigation as soon I find time.

[brifsttar]

Thank you for your help!

Take all the time you need, there's no hurry with this bug. In the meantime I've been using the "maybe-fix" I mentioned in https://github.com/esmini/esmini/issues/496#issuecomment-1821252938, and everything seems to be running fine on my side (I haven't noticed any side effect, which I'm a bit worried about since I don't fully understand the change I made in the first place). So this issue is definitely not blocking on my end.

[eknabevcc]

We did some recent updates wrt positioning. When you find time, please try the same operations with esmini v2.34.0 or later.

It would be interesting to find out whether it works better now. (I did some tests, including your road and positions, but the context is a bit different)

[brifsttar]

Hey Emil, thanks for getting back to me on this.

I just merged master (which is v2.35) and the issue is still there (though I didn't have much time for testing, so maybe I missed something). Below is a screenshot with the hand-placed point on the right, and the MoveAlongS-created point on the left. When the initial point is perfectly aligned with the road, it appears that MoveAlongS simply flips its pitch angle.

If the initial point has a non-aligned pitch angle (let's call that P), the MoveAlongS point will have a its pitch angle set to P - 2 * (road pitch angle)

However, I don't see any issue with the roll anymore, though I didn't really focus on that in my earlier testing so I can't say for sure if there was a change or not.

I won't have time to run more tests for this year, but since this issue is rather minor and non-blocking, I think it can definitely wait.

Happy holidays to you and your team!

[eknabevcc]

Once again, thanks for good input. Let's try make some progress here. I did some further testing with your coordinate input. These are my current conclusions:

• Initially your object has a negative pitch, while the road pitch is positive (downhill) in the driving direction.
• MoveAlongS will, by default, align pitch and roll to the road. So the pitch will be adjusted in the MoveAlongS call.

So, to my understanding the swapped sign is actually an adjustment to align the pitch to the road.

The alignment of orientation and elevation (z) can be controlled. There are some, quite recent, info in User guide: https://esmini.github.io/#_positioning. But it's a bit tricky to digest, so I'll try give examples:

Example 1: Initialize position with a non-aligned pitch

Initial position given in OpenSCENARIO file:

<WorldPosition  x="-628.92144531250005" y="-1815.2664062500000" h="-0.29530104428854087" p="-0.2"/>

Omitted z and roll will be aligned to road.

Following code:

printPos(obj);
obj->pos_.SetInertiaPosMode(-628.92144531250005,
	-1815.2664062500000,
	0.0,
	-0.29530104428854087,
	0.0,
	0.0,
	roadmanager::Position::PosMode::Z_REL | roadmanager::Position::PosMode::H_ABS |
		roadmanager::Position::PosMode::P_REL | roadmanager::Position::PosMode::R_REL);
printPos(obj);
obj->pos_.MoveAlongS(-1.0);
printPos(obj);

gives this output:

x -628.92145 y -1815.26641 z -0.25465 h 5.98788 p 6.08319 r 0.00000   (p 6.08319 == -0.2)
x -628.92145 y -1815.26641 z -0.25465 h 5.98789 p 0.02172 r 0.00038
x -629.87282 y -1814.95838 z -0.23294 h 5.98706 p 0.02184 r 0.00038

and looks like this:

Example 2: Initialize omitting pitch

<WorldPosition x="-628.92144531250005" y="-1815.2664062500000" h="-0.29530104428854087"/>

The same code as in example 1 gives this result:

x -628.92145 y -1815.26641 z -0.25465 h 5.98789 p 0.02172 r 0.00038
x -628.92145 y -1815.26641 z -0.25465 h 5.98789 p 0.02172 r 0.00038
x -629.87282 y -1814.95838 z -0.23294 h 5.98706 p 0.02184 r 0.00038

and looks like this:

About PosMode

To add some pitch from the application there are two options:

1. Relative pitch. This is useful when application is not aware of road elevations but still want to indicate pitch caused by acceleration and deceleration.
2. Absolute pitch. This is useful when application is powered by advanced vehicle model and evelation contact points for each tyre.

Changing the code above for absolute pitch, example:

obj->pos_.SetInertiaPosMode(-628.92144531250005,
	-1815.2664062500000,
	0.0,
	-0.29530104428854087,
	0.1,
	0.0,
	roadmanager::Position::PosMode::Z_REL | roadmanager::Position::PosMode::H_ABS |
		roadmanager::Position::PosMode::P_ABS | roadmanager::Position::PosMode::R_REL);

[brifsttar]

Hi Emil,

Thanks a lot for this thorough answer. I had seen all the "relative" code pop up while merging recently, but I didn't really understood what this was all about. It's now much clearer.

One difference between my use case and your example is that I use absolute values for all coordinates, since they're all user input (i.e., the gizmo). But I think RoadManager can handle that since SetMode has different properties for SET and UPDATE, which is pretty awesome :)

I've ran a lot more test and I think I understand the issue now. The issue is that the relative align mode is relative to the road direction, and not the lane direction. So when the point is initially aligned with the road direction, it works fine. But when the point is aligned opposite to the road direction, here's what I think happens:

• In our case, road has a 1.25° pitch angle (in its direction)
• So on the opposite side of the road, the point has a -1.25° pitch angle to align with the road
• When using P_REL, RoadManager considers that the relative pitch angle of that point is (road pitch - point pitch) = (1.25 - (-1.25)) = 2.5°
• After moving the point, MoveAlongS sets the point pitch to the sum of the relative road pitch (2.5°) and the road pitch (1.25°), which is how we get a point that has a pitch angle 3x higher than expected.

The code below illustrates that.

void Move(double X, double Y, double Z, double H, double P, double R) {
	roadmanager::Position p;
	p.SetInertiaPosMode(X, Y, Z, H, P, R,
		roadmanager::Position::PosMode::Z_ABS |
		roadmanager::Position::PosMode::H_ABS |
		roadmanager::Position::PosMode::P_ABS |
		roadmanager::Position::PosMode::R_ABS
	);
	p.SetMode(
		roadmanager::Position::PosModeType::UPDATE,
		roadmanager::Position::PosMode::Z_REL |
		roadmanager::Position::PosMode::H_REL |
		roadmanager::Position::PosMode::P_REL |
		roadmanager::Position::PosMode::R_REL
	);
	p.MoveAlongS(1);
	// print(p.GetP()) in Unreal
	UE_LOG(LogTemp, Warning, TEXT("%f"), p.GetP());
}

// Two points at the same location, but opposite orientation
// First location is aligned with the road direction
// Returns 6.261363 = correct
Move(-628.70222656249996, -1815.0725000000000, -0.25786924362182617, 2.8290140156239638, -0.021701903508873483, 0);
// Second location is aligned with the lane direction
// Returns 0.064980 = incorrect, expected 0.021885
Move(-628.70222656249996, -1815.0725000000000, -0.25786924362182617, -0.29703405029675922, 0.021699287129277857, 0);

Maybe you already had that figured out and that's what you explained, but my brain was slow to compute all of that.

I think a (theoretical) solution would be to have a ..._REL_DRIVING_DIRECTION PosMode? I'm not sure, because all this is still quite new to me. But if that would work, it sounds quite painful to actually implement.

In the meantime:

• I have no idea why my workaround mentioned earlier (https://github.com/esmini/esmini/issues/496#issuecomment-1821252938) works, since it has nothing to do with relative coordinates
• I'm not sure what would be a correct workaround for the issue, ideally that wouldn't require change to the RoadManager codebase. Maybe by calling GetPRelativeDrivingDirection (which doesn't exist, but could be implemented) before MoveAlongS, then applying it via SetPRelativeDrivingDirectionafterwards (doesn't exist either, but could also be implemented)?
• I found two more bugs in my code while investigating this, which I'm not sure if it's good or bad :P

Thanks again for all your help.

All of my tests were done on v2.36.1.

[eknabevcc]

Thanks for the analysis. I will read and digest carefully :) and then get back to you

[eknabevcc]

Ok, another attempt to fix this tricky issue.

Maybe it was a bit over ambitious introducing the fully flexible orientation modes where relative and absolute values could be mixed. But I don't give up quite yet :)

For the case when orientation is specified in terms of absolute values but then should be aligned with road surface (relative mode) - like in your case, now the relative components are calculated by means of matrix equation: Rroad * Rrel = Rabs => Rrel = Rroad_inv * Rabs. From the resulting Rrel rotation matrix, the individual Euler components (heading, pitch, roll) are then extracted. Hopefully this will give the correct relative components for further road alignment, e.g. MoveAlongS(...).

Also a test case similar to your case has been added.

The commit (9a48f598a1b9d0010ce457385adffb02e08ba059) has been merged to dev and will be included in next release.

[brifsttar]

It works!

I didn't run really thorough tests, but both the snippet I shared above and the test case I had in Unreal are now behaving as expected. I will need to run more tests since I had to change some code on my end (I switched to using SetInertiaPosMode as it seems to be the preferred way of setting inertial coordinates).

Thank you so much Emil for taking the time to solve this issue and for all your very valuable explanations. I'm very grateful for your work, as I'm sure the whole OpenDRIVE/OpenSCENARIO also is.

[eknabevcc]

Ah, preliminary good news at least! :) Seems like we're on the right track here. Let's keep it open for a while in case you/we find further flaws. Thank you for valuable and clear reports 👍

[brifsttar]

More great news: the change has been merged into our 3 currently in-development projets, and we haven't noticed any side effect.

To end this on a light note, here's the actual downstream bug that was caused by this issue.

https://github.com/esmini/esmini/assets/53508707/41e3f783-bfb5-466d-8e49-3c16d32a7e11

In this scenario all traffic is spawned dynamically around ego, and I use MoveAlongS to compute the spawn coordinate relative to ego's current position. The pitch issue made it so that the spawn position's pitch was pointing slightly up on a road going slightly down. It was only a few degrees offset, but since I physically "push" cars at a speed of ~150km/h on spawn, it ended up jumping into the air, landing way down the road outside its target lane (road was also slightly curved). After that, the regulation loop was fighting against the vehicle physics to stay in its lane, leading to this weird state.

Needless to say I scratched my head quite a bit when I saw this car come up on that otherwise long and very uneventful scenario :D

Thanks again for all your work!

[eknabevcc]

Thanks for the feedback and info! It's nice to get some insight :) 👍

I totally understand the head-scratching, I mean the wobbling being a result of a pitch error in roadmanager position is not obvious I must admit :)

BTW: Thanks for posting the video clip. I think it all looks great! The scenery and light conditions and everything. Also, even though the fighting and wobbling is not intended I must say the vehicle dynamics looks really good 👍