Point cloud registration

In this post, I mainly address the least square optimization problem in point cloud registration.

$\mathbf{T^*} = \mathop{\arg\min}_{\mathbf{T^*}}\sum_{(p,q)\in\mathcal{K}} || \mathbf{p} - \mathbf{T}\mathbf{q} ||^2$

where $\mathbf{p}, \mathbf{q}$ are the correspondence point pair in two point cloud pieces. $\mathbf{T}$ is rigid transformation matrix.

Generally, there are two approaches to solve this problem (These two methods have been widely used in various non-linear optimization).

I briefly summarize some important points here. Please refer to here for the detail of close-form derivation, and here for iterative solution.

Close-form solution

The optimal translation is

$\mathbf{t^*} = \mathbf{\bar q} - R\mathbf{\bar p}$

where $\mathbf{\bar q}, \mathbf{\bar p}$ are the mean of correspondence points. $R$ is rotation matrix.

The optimal rotation is

$R = VU^T$

where $V, U$ is the SVD two component results of a matrix, which is composed by normalized correspondence points (see derivation for the detail).

Note that the SVD here is not applied for solving least square (the least square here is different from conventional format, check my previous post in linear algebra section).

Besides, the solution $R = VU^T$ may not be a true rotation matrix, because the derivation process is to solve the optimal orthodox matrix. Hence, $R$ may be a reflect matrix (the determinant of $R$ is -1). In this case, rotation matrix should be modified as below, so that we can gurantee the det( $R$ )=1 (the reasons and details in above derivation link).

$R = V \begin{pmatrix} 1 & & & & & \\ & 1 & & & & \\ & & ... & & & \\ & & & & 1 & \\ & & & & & -1 \\ \end{pmatrix} U^T$

Iterative solution (Gauss Newton method)

There is another solution which is a more flexible and general format.

We represent the rigid transformation matrix into a vector $\xi = (\theta_x, \theta_y, \theta_z, x, y, z)$ that collates rotation and translation components.

Suppose we have $n$ pairs correspondence points. We define the error function

$f(\xi) = \begin{pmatrix} p_1 - Tq_1 \\ p_2 - Tq_2 \\ ... \\ p_n - Tq_n \\ \end{pmatrix}_{3n\times 1}$

Note that we discard the last element in $p-Tq$ after calculation, and $\xi$ is included in $T$ . The original objective is $\min ||f(\xi)||^2$

Given a tiny disturbance $\Delta \xi$ , and apply Taylor expansion.

$f(\xi + \Delta \xi) \approx f(\xi) + J(\xi) \Delta \xi$

Where $J$ is the Jacobian matrix first order is called Jacobian, and second order is called Hessian .

Our target now is to find a best iteration direction $\Delta \xi$ , which make the minimum $\lVert f(\xi + \Delta \xi)\rVert^2$ . In other word, we have

$\Delta \xi^* = \mathop{\arg\min_{\Delta \xi}} \frac{1}{2} \lVert f(\xi)+J\Delta \xi \rVert^2$

calculate derivation w.r.t. $\Delta \xi$ , and make the derivation equals to 0. We have

$J(\xi)^T J(\xi) \Delta \xi = -J(\xi)^T f(\xi)$

since $\xi$ is a initial guess, we can calculate $\Delta \xi$ (i.e. this equation tells us how to update $\xi$ ).

Note that $J^T J$ is Hessian matrix $H$ too. Pose graph optimization also uses this method to optimize.

$H(\xi) \Delta \xi = -J(\xi)^T f(\xi)$

Note that we cannot directly apply add to update $\xi$ because of the rotation component. One alternative way is to update $T$ as below (because $\Delta \xi$ is tiny, we have $\sin\theta \approx \theta$ , $\cos\theta \approx 0$ ).

$T_k \approx \begin{pmatrix} 1 & -\theta_z & \theta_y & x \\ \theta_z & 1 & -\theta_x & y \\ -\theta_y & \theta_x & 1 & z \\ 0 & 0 & 0 & 1 \\ \end{pmatrix} \times T_{k-1}$

Iterative solution (Levenberg-Marquardt method)

LM is similar with Newton method, but it controls a maximum bound for update step. Therefore, it is usually more robust than Newton method.

Please refer to here for the detailed introduction.

An extension, Lagrange Multipler Method

Lagrange multiple method has been widely used in various optimization with constraints problems.

This method has very intuitive geometry (the tangent lines at optimal point must be parallel) explanation about why it works. Check HERE for the details.