3
$\begingroup$

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?

2 Answers 2

3

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?

  • 0
    Unfortunately, it's checked out from the University library for the next few weeks.2012-10-23
1

Consider using a library like Ceres solver (https://code.google.com/p/ceres-solver/) which has automatic differentiation support.