[Math] Jacobian of Reprojection Error

lie-groupsoptimizationrigid transformationrotations

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 $3×3$ 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?

Best Answer

Minimizing reprojection error using a sparse Levenberghe Marquardt algorithm is discussed in detail in an appendix to Hartley and Zisserman. Have you tried looking there?

Related Question