prashanthr05
commented
5 years ago Similarly, the linear part of the exp() method is missing a multiplication with the Jacobian of SO(3).
This should be, $$ T = \begin{bmatrix} I3 + \frac{(1 - \text{cos}(||\omega||))}{||\omega||^{2}} \omega \omega^{T} + \frac{\text{sin}(||\omega||)}{||\omega ||} \omega {\times} & (I3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} \omega {\times} + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} \omega^{2} {\times}) v\\ 0 {3\times 1} & 1 \end{bmatrix} $$
where the rotation part is obtained using Rodrigues formula (which is computed correctly using the AxisAngle computations of Eigen.)
Here, the log of SE(3) says the linear part is unchanged https://github.com/robotology/idyntree/blob/7733e56f858cfb319261f118871d37b8cbc5ca0f/src/core/src/Transform.cpp#L534
which does not seem to be coherent with the definition of SE(3) logarithm described in Appendix section B of the paper Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems.
Looks like we are missing a multiplication of the linear part with a left/right Jacobian inverse of $SO(3)$.
where, the left Jacobian of $SO(3)$ is defined as,
$$ J{l{SO(3)}} = \sum{n = 0}^{\infty} \frac{1}{(n+1)!} [\omega\times]^n = (I3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} [\omega {\times}] + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} [\omega _{\times}]^{2})$$ where $\omega$ is the
AngularMotionVector3.When simplified further, $$ J{l{SO(3)}} = \frac{\text{sin}(||\omega||)}{||\omega||}I3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||} [\phi {\times}] + \bigg(1 - \frac{\text{sin}(||\omega||)}{||\omega||}\bigg) \phi\phi^T$$ where $\phi = \frac{\omega}{||\omega||}$
The inverse left Jacobian is given by, $$ J^{-1} {l {SO(3)}} = \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) I 3 + \bigg( 1 - \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) \bigg) \phi \phi^T - \frac{||\omega||}{2} [\phi {\times}]$$
cc @traversaro