Open louisdehaybe opened 2 years ago
Thank you and good catch! However the function outputs correctly. No error there.
The discrepancy between the algorithm in the book and code is due to a small trick used in the code. MatrixLog6()
outputs $[{\cal S}]\theta$, the top right $3\times3$ submatrix of which is $v\theta = \theta G^{-1}(\theta)p = (\theta G^{-1}(\theta))p$. Multiplying $\theta$ and $G^{-1}$ together at first reduces some unnecessary computation.
Hello,
In the book, page 106, they provide the analytical expression for G^-1, that is being use to calculate the log_SE3 . In particular, the experssion begins by 1/theta * Identity + ....
However, in both matlab and python versions of the code, the function MatrixLog6 begins by eye(3) + ... , so there is not the 1/theta in the code.
Is there an error, or do I miss something ?
Thanks !