I am writing a program to find the transformation between two sets of 3D points extracted from a moving stereo camera. I am using an 'out of the box' Levenberg-Marquardt implementation to find this transformation by minimizing the reprojection error of the 3D points in frame B onto 2D points in frame A:
\begin{equation} \min_{\Delta}e^2 \end{equation}
where
$e$ is the reprojection error vector: $ \begin{pmatrix} e_1 \\ \vdots \\ e_n \\ \end{pmatrix} = \begin{pmatrix} \|x_1^a-P_1\Delta x_1^b\| \\ \vdots \\ \|x_n^a-P_n\Delta x_n^b\| \\ \end{pmatrix}$
$x_i^a$ is the $i^{th}$ 2D point in frame A (extended to $\begin{pmatrix}x_i^a & 1 & 0\end{pmatrix}$)
- $x_i^b$ is the $i^{th}$ 3D point in frame B (extended to $\begin{pmatrix}x_i^a & 1\end{pmatrix}$)
- $P_i$ is the projection matrix: $P_i = \begin{pmatrix} \frac{f}{{x_i^b}_z} & 0 & \frac{c_x}{{x_i^b}_z} & 0 \\ 0 & \frac{f}{{x_i^b}_z} & \frac{c_x}{{x_i^b}_z} & 0 \\ 0 & 0 & \frac{f}{{x_i^b}_z} & 0 \\ 0 & 0 & 0 & 0 \end{pmatrix}$
- $\Delta$ is the 6D homogenous rigid transformation matrix that transforms 3D points in frame B into their matching 3D points in frame A
I am parameterizing $\Delta$ by a 6D vector $\begin{pmatrix}w_x w_y w_z t_x t_y t_z\end{pmatrix}$:
- $w=\begin{pmatrix}w_x w_y w_z\end{pmatrix}$ is a $so3$ rotation vector (angle/axis)
- $t=\begin{pmatrix}t_x t_y t_z\end{pmatrix}$ is a 3D translation vector
When actually computing the error, I use Rodrigues' formula to calculate a $3x3$ rotation matrix from $w$: $R = e^\hat{w}$ so that $\Delta = \begin{pmatrix}R & t\\ \mathbf{0} &1\end{pmatrix}$.
The error vector I provide to the LM algorithm will be in the form of $\begin{pmatrix}e_1 \\e_2\\\vdots\\e_n\end{pmatrix}$, so I need to compute the Jacobian of the form:
\begin{equation} J = \begin{pmatrix} \frac{\partial e_1}{w_x} & \frac{\partial e_1}{w_y} & \frac{\partial e_1}{w_z} & \frac{\partial e_1}{t_x} & \frac{\partial e_1}{t_y} & \frac{\partial e_1}{t_z} \\ \\ ...\\ \\ \frac{\partial e_n}{w_x} & \frac{\partial e_n}{w_y} & \frac{\partial e_n}{w_z} & \frac{\partial e_n}{t_x} & \frac{\partial e_n}{t_y} & \frac{\partial e_n}{t_z} \\ \end{pmatrix} \end{equation}
When I expand my error function out and take the partial derivatives by brute force, it quickly turns into a big mess. However, I've seen that there is some trick to computing the Jacobian of Rodrigues here. How can I go about using this to find the Jacobian of my full error function vector?