I am given a covariance matrix of the 7-dimensional random vector $A$. The goal is to transform it to the $6 \times 6$ covariance matrix of a 6-dimensional random vector $B$, which is in a way equivalent to $A$.
$B = f(A)$ The function $f$ that maps $A$ to $B$ is known and to be described in this paragraph. Let's state the problem explicitly, so both $A$ and $B$ are $\text{SE3}$ transformations, but $A$ uses 'quaternion + translation' parameterization, while vector $B$ is in a 'axis angle + translation' representation.
$$ A = (q_x,\ q_y,\ q_z,\ q_w,\ t_x,\ t_y,\ t_z)^T\\
B = (r_x,\ r_y,\ r_z,\ t_x,\ t_y,\ t_z)^T\\
B = f(A )= \left( \begin{array}{cc}
\phi = & 2 \arccos(q_w)\\
r_x = & q_x\ \frac {\phi} {\sin(\phi/2)}\\
r_y = & q_y\ \frac {\phi} {\sin(\phi/2)}\\
r_z = & q_z\ \frac {\phi} {\sin(\phi/2)}\\
\end{array} \right)
$$
We will go one step further here write backward transform:
$$
A = f^{-1}(B) = \left(\begin{array}{cc}
\phi = & \|r \|_2 \\
q_x = & r_x \frac {sin(\phi / 2)} {\phi} \\
q_y = & r_y \frac {sin(\phi / 2)} {\phi} \\
q_z = & r_z \frac {sin(\phi / 2)} {\phi} \\
q_w = & \cos(\phi / 2)\\
\end{array}\right)
$$
But our goal is to convert not mere vectors but their covariances, so the original task may be formulated as following:
$$
\mbox{Cov6} = \frac {\partial B}{\partial A}(A_0) \cdot \mbox{Cov7} \cdot \frac {\partial B}{\partial A}(A_0)^T \\
\mbox{Cov7} = \frac {\partial A}{\partial B}(B_0) \cdot \mbox{Cov6} \cdot \frac {\partial A}{\partial B}(B_0)^T
$$
I have written out jacobians:
$$
\frac {\partial B}{\partial A}(A_0) = \left(\begin{array}{cc}
\frac {\partial r}{\partial q}(q_0) & 0_{3\times3} \\
0_{3\times4} & I_{3\times3} \\
\end{array}\right) \\
\frac {\partial A}{\partial B}(B_0) = \left(\begin{array}{cc}
\frac {\partial q}{\partial r}(r_0) & 0_{4\times3} \\
0_{3\times3} & I_{3\times3} \\
\end{array}\right)
$$
To define jacobians fully one thing that left to do is to write out five entries of above matrices; We will do axis angle from quaternion first:
$$
\frac {\partial r_k}{\partial q_k}(q_0) = \frac {\phi} {\sin(\phi/2)}
\hspace{2cm}
\frac {\partial r_k}{\partial q_l}(q_0) = 0 \\
\frac {\partial r_k}{\partial q_w}(q_0) =
\frac {2 q_k} {\sqrt{1-w^2}}\Big[ \frac 1 {\sin(\phi/2)} - \frac{\phi\ cos(\phi/2)} {\sin^2(\phi/2)}\Big]
$$
Now, write out quaternion from axis angle:
$$
\frac {\partial q_k}{\partial r_l}(r_0) = r_k\ r_l\ \big[ \frac 1 2 \phi^{-2} \cos(\phi/2) - \phi^{-3}\ \sin(\phi/2) \big] \\
\frac {\partial q_k}{\partial r_k}(r_0) = r_k^2\ \big[ \frac 1 2 \phi^{-2} \cos(\phi/2) - \phi^{-3}\ \sin(\phi/2) \big] + \frac {\sin(\phi/2)} {\phi} \\
\frac {\partial q_w}{\partial r_k}(r_0) = -\frac 1 2 \sin(\phi/2) \frac {r_k} {\phi} \\
$$
That finishes my derivations and brings me to the question:
Simple scripted implementation of the above gives significant discrepancy when I convert covariances forth and back. Are the jacobians wrong? Are my $f$ or $f^{-1}$ wrong?
Huge thanks for the help, guys!
