3D inverse transformation jacobian

243 Views Asked by At

I am working on robotics and I have to deal with transformations. In my case, $\pmb{t} = \left[x \; y \; z \; \psi \; \theta \; \phi \right]^T$ where $x$, $y$ and $z$ is the translation and $\psi$, $\theta$ and $\phi$ are the roll, pitch and yaw angles. I my case I apply the rotations as $\pmb{R}=\mathrm{\bf{Rot}}_z(\phi)\mathrm{\bf{Rot}}_y(\theta)\mathrm{\bf{Rot}}_x(\psi)$. In Neira 1993 and Smith 1990 the inverse of the transformation is defined:

$\pmb{t}' = \ominus \pmb{t} = \begin{bmatrix} x' \\ y' \\ z' \\ \psi' \\ \theta' \\ \phi' \end{bmatrix} = \begin{bmatrix} -\pmb{R}^T\begin{bmatrix} x \\ y \\ z \end{bmatrix} \\ \mathrm{atan2}(o_z,a_z) \\ \mathrm{atan2}(-n_z,n_x\cos(\phi)+n_y\sin(\phi)) \\ \mathrm{atan2}(n_y,n_x) \end{bmatrix}$

where:

$\pmb{R} = \begin{bmatrix} n_x & o_x & a_x \\ n_y & o_y & a_y \\ n_z & o_z & a_z \end{bmatrix}$

In the same works, the jacobians of this operation is described:

$ \pmb{J}_\ominus = \begin{bmatrix} -\pmb{R}^T & \pmb{N} \\ \pmb{0}_{3\times3} & \pmb{Q} \end{bmatrix}$

with:

$\pmb{N} = \begin{bmatrix} 0 & -n_z x \cos(\phi)-n_zy\sin(\phi)+z\cos(\theta) & n_y x-n_x y\\z' & -o_z x \cos(\phi) - o_z y \sin(\phi) + z \sin(\theta) \sin(\psi) & o_y x - o_x y \\ -y' & a_z x \cos(\phi) - a_z y \sin(\phi) + z \sin(\theta) \cos(\psi) & a_y x - a_x y \end{bmatrix}$ $\pmb{Q} = \begin{bmatrix} -n_x/(1-a_x^2) & -o_x \cos(\psi)/(1-a_x^2) & a_z a_x / (1 - a_x^2) \\ o_x/(1-a_x^2)^{1/2} & -a_z \cos(\phi) / (1-a_x^2)^{1/2} & a_y/(1-a_x^2)^{1/2} \\ n_x a_x /(1-a_x^2) & -a_y \cos(\phi) /(1-a_x^2) & -a_z /(1-a_x^2) \end{bmatrix}$

I have implemented this function and its Jacobian (code is available here) as well as the direct compounding operation ($\pmb{t}_3 = \pmb{t}_1\oplus\pmb{t}_2$). Then, I have also implemented a way to compute the derivatives of each function using Ceres-Solver. Then, I compare the Jacobian that I implemented from Neira 1993 and Smith 1990 with the one computed using Ceres-Solver. The Jacobian of the transformation from the analytic and the automatic differentiation is very similar (numerical error differences). However, the inverse composition there is a difference between the analytic Jacobian from Neira 1993 with the one computed using Ceres-Solver. Moreover, these two jacobians are different only 50% (approximately) of the times and always in the same elements of the Jacobian, row 2 of $\pmb{Q}$. You can test the code (it is developed with Ubuntu, I do not know if it will compile in windows).

Does anybody have an idea of where can I get the Jacobian of the inverse of the transformation (a part from Neira 1993 and Smith 1990) or point me what is wrong with the current Jacobian?