Added Jacobians for Rot3::xyz and related conversions to euler angles

[Eskilade]

Hi,

This PR adds the remaining Jacobians for conversions between Rot3 and euler angles (first PR here ). Some of the expressions below are a bit hairy, so please feel free to suggest a different way of doing it :)

[jlblancoc]

Awesome!

[jlblancoc]

Perhaps it would be convenient to add unit tests covering ypr near the extreme +-pi and +-pi/2 values?

[dellaert]

Great! I'm OK to approve as is, but I think the computation is quite heavy and could probably be improved quite a bit. Still, we did not have this before, and it might not make a huge difference in an application.

[dellaert]

nice !

[dellaert]

So, this is of course perfectly fine. However, if you care about performance, this is (a) 81 mults, and (b) we could use of the fact that mH is super-sparse. In particular, SO3.cpp says

    mH << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
        R * P3.block<3, 3>(6, 0);

Maybe add as a TODO. In fact I suspect the 3*3 matrix of derivatives might yield not too complicated symbolic expressions, and we do a lot of compute here (including multiplying a lot of things with zero). Might be worth some mathematica exploration.

[Eskilade]

Perhaps it would be convenient to add unit tests covering ypr near the extreme +-pi and +-pi/2 values?

Ok, I'll add more tests. However, the derivative breaks down for pitch values +- pi / 2. How do you prefer to handle that? Do you use nans?

[jlblancoc]

However, the derivative breaks down for pitch values +- pi / 2. How do you prefer to handle that? Do you use nans?

That was my point, bringing out that discussion :-)
Let's see what @dellaert thinks, but I feel leaned to say that the unit test must pass for values near the edge, and raise an exception if exactly (within an epsilon) of the edges if the derivative (not even the numerical one) is really not well-defined there.

[dellaert]

However, the derivative breaks down for pitch values +- pi / 2. How do you prefer to handle that? Do you use nans?

That was my point, bringing out that discussion :-)
Let's see what @dellaert thinks, but I feel leaned to say that the unit test must pass for values near the edge, and raise an exception if exactly (within an epsilon) of the edges if the derivative (not even the numerical one) is really not well-defined there.

I think an exception is a good idea. There is a singularity, for sure.

[Eskilade]

Hmm, I'm a bit unsure about which epsilon value we should use... If I set roll and yaw to zero, and if
a) is the largest entry in the difference between the numerical derivative and the calculated derivative in absolute value
b) the entry in the calculated derivative corresponding to (a)

then I get :

• a) ~3e-08 b) ~10 for pitch = pi/2 - 0.1
• a) ~3e-05 b) ~100 for pitch = pi/2 - 0.01
• a) ~0.03 b) ~1e3 for pitch = pi/2 - 1e-3
• a) ~30 b) ~1e4 for pitch = pi/2 - 1e-4
• a) ~2e4 b) ~1e5 for pitch = pi/2 - 1e-5

What do you think? I hope the numbers are understandable.

Edit : I should add that I'm talking about the derivative of Rot3::RQ

[jlblancoc]

• a) ~3e-05 b) ~100 for pitch = pi/2 - 0.01

Thanks for the great summary and the time collecting the results!
I think "pi/2 - 0.01" would be safe enough. Don't set the unit test threshold too tight (e.g. if your error is ~3e-05 , mark the limit like 1 order of magnitude larger than that) since different CPU architectures may give slightly different numerical results for the same code and make the unit test to fail.
Thanks!

[dellaert]

@Eskilade let me know when ready to merge....

[Eskilade]

Yupp, ok for my part ! Btw, does this merit a mention in contributors or something similar ? :)

[dellaert]

Of course, feel free to add your name, and then I'll merge :-)

[Eskilade]

Where do I add it ? Like in which file :)

[varunagrawal]

You can add your name here: https://github.com/borglab/gtsam.org/blob/master/about.md

[Eskilade]

Alright, but since that's a different repo, then you can go ahead and merge this PR :)

[lanyouzibetty]

@Eskilade Thanks for your awesome work. Can you tell me how to derive the jacobians for conversions between Rot3 and euler angles? I have not deduced it, thanks!

[Eskilade]

@lanyouzibetty, have a look here : https://github.com/Eskilade/orient/blob/master/documentation/conversion_formulas.pdf :)

[lanyouzibetty]

@Eskilade Thanks very much !