Take the 2-minute tour ×
Mathematics Stack Exchange is a question and answer site for people studying math at any level and professionals in related fields. It's 100% free, no registration required.

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?

share|improve this question
add comment

2 Answers

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?

share|improve this answer
    
Unfortunately, it's checked out from the University library for the next few weeks. –  Boatzart Oct 23 '12 at 17:08
add comment

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

share|improve this answer
add comment

Your Answer

 
discard

By posting your answer, you agree to the privacy policy and terms of service.

Not the answer you're looking for? Browse other questions tagged or ask your own question.