search

artivis / manif

A small C++11 header-only library for Lie theory.
https://artivis.github.io/manif
MIT License
1.46k stars 239 forks source link

Small angle approximation for SO3 right and left Jacobians #247

Closed xianlopez closed 2 years ago

xianlopez commented 2 years ago

Hi. First of all thanks for the great paper and this C++ library.

I was trying to implement the right Jacobian of SO3 myself and realized that equation 143 is not well defined for theta = (0, 0, 0). Looking at the implementation in this library I see that you do an approximation when the vector is small. However, that's not mentioned in the paper (or I couldn't find it). Could you please explain how you arrive to that result? Looking at equations 143 to 146, it's not clear to me how you solve the indeterminate and get to that approximation.

joansola commented 2 years ago

To find small-angle approximations, substitute sin(theta) and cos(theta) by their Taylor expansions, cancel the terms with those in the denominator, and truncate the remaining terms at the desired length.

For example the following term appearing in the right Jacobian:

(1 - cos (a) ) / a^2 ~ (1 - (1 - a^2 / 2 + a^4 / 24)) / a^2 = (a^2 / 2 - a^4 / 24) / a^2 = 1/2 - a^2 / 24 ~ 1/2

You proceed akin with the other term (a - sin(a) ) / a^3

xianlopez commented 2 years ago

Aha, that makes sense, thanks for the quick answer!

The term (a - sin(a)) / a^3 gives me 1/6, but I guess that's ignored because it multiplies a quadratic term, [theta]_x^2, right?

joansola commented 2 years ago

Yes, you would get (I might have put the signs wrong)

Jl(a) = I + [a]x / 2 + [a]x^2 / 6 + ...

and you may decide to truncate before the quadratic term -- this is up to you.