Point-to-Plane ICP

To compute the distance between corresponding points, use the point-to-plane distance: The minimization problem is:

\[ \text{min}_{R,t} \sum_i | (R \cdot q_i + t - p_i) \cdot n_i |^2 \]

where \(p_i, q_i\) are corresponding closest points and \(n_i\) normal at \(p_i\). The point \(p_i\) is in the target or fixed point cloud. The point \(q_i\) is in the source or moving point cloud.

Under the assumption of small rotations, we can approximate the rotation matrix \(R(\alpha, \beta, \gamma)\), that is the product of three rotations around the \(x\)-, \(y\)- and \(z\)-axis, by the first order Taylor expansion around the identity matrix \(I\):

\[ R(\alpha, \beta, \gamma) = R_z(\gamma) R_y(\beta) R_x(\alpha) \approx I + \left( \begin{array}{c c c} 0 & - \gamma & \beta\\ \gamma & 0 & -\alpha\\ -\beta & \alpha & 0 \end{array}\right) \]

where we first rotate \(\alpha\) around the \(x\)-axis, then \(\beta\) around the y-axis and \(\gamma\) around the z-axis. We note that with this approximation, the search for the optimal rotation and translation reduces to a quadratic minimization problem in the three rotation angles and the three coordinates of the translation: \((\alpha, \beta, \gamma, t_x, t_y, t_z)\).

A general unconstrained quadratic minimization problem has the form, see Convex Optimization, Boyd and Vandenberghe: \[ \text{minimize}\, \frac{1}{2} x^T P x + q^T x + r \]

The solution to this problem can be obtained by solving the linear system \[ P x + q = 0 \]

We, therefore, reduce the point-to-plane minimization to an unconstrained quadratic minimization problem, for which the solution can be computed by solving a linear system. We first note that \[ |q \cdot n|^2 = q^T\cdot n \cdot n^T \cdot q \]

Note that it is the dot product between vectors on the left and, on the right, the matrix product. We introduce some notation. \[ \Theta = \begin{pmatrix} \alpha \\ \beta \\ \gamma \\ t_x \\ t_y \\ t_z \end{pmatrix}, \quad W_i = \begin{pmatrix} 0 & q_{i,z} & -q_{i,y} & 1 & 0 & 0\\ -q_{i,z} & 0 & q_{i,x} & 0 & 1 & 0\\ q_{i,y} & -q_{i,x} & 0 & 0 & 0 & 1\\ \end{pmatrix} \]

Therefore, we rewrite the distance as \[ R q_i + t - p_i \approx W_i \, \Theta + (q_i - p_i) \]

Also note that the cross product can be given as a matrix multiplication as follows. First introduce the \(3\times 3\) matrix \[ \Omega = \begin{pmatrix} 0& q_z& -q_y\\ -q_z&0&q_x\\ q_y&-q_x& 0\end{pmatrix} \]

and obtain \[ \Omega^T \, n = ( n^T \Omega )^T = q \times n \]

it follows: \[ W_i^T n_i = \begin{pmatrix}c_{i,x} \\ c_{i,y} \\ c_{i,z} \\ n_{i,x} \\ n_{i,y} \\ n_{i,z}\end{pmatrix} = C_i \]

where \(c_i = q_i \times n_i\).

Putting all this together we obtain \[ \begin{align} |(R q_i + t - p)\cdot n_i|^2 \approx&\, \Theta^T W_i^T n_i n_i^T W_i \Theta + 2 (q_i - p_i)^T n_i n_i^T W_i \Theta + (q_i-p_i)^T n_i n_i^T (q_i - p_i) \\ \approx&\, \Theta^T C_i \, C_i^T \Theta + 2 (q_i - p_i)^T n_i C_i^T \Theta + (q_i-p_i)^T n_i n_i^T (q_i - p_i) \end{align} \]

The point-to-plane minimization problem can be formulated as follows: \[ \min_{R,t} \sum_i | (R \cdot q_i + t - p_i) \cdot n_i |^2 \longrightarrow \text{min}_{(\alpha, \beta, \gamma, t_x, t_y, t_z)} F(\alpha, \beta, \gamma, t_x, t_y, t_z) \]

where \[ \begin{align} F(\alpha, \beta, \gamma, t_x, t_y, t_z) = &\; \Theta^T \left(\sum_i C_i C_i^T \right) \Theta + 2 \left(\sum_i (q_i - p_i)^T n_i C_i^T\right) \Theta \\ &\; + \sum_i (q_i-p_i)^T n_i n_i^T (q_i - p_i) \end{align} \]

In matrix notation we have \[ \begin{align} F(\alpha, \beta, \gamma, t_x, t_y, t_z) &= \Theta^T K \Theta + 2 b^T \Theta + c\\ \end{align} \]

with \[ \begin{align} K &= \sum_i C_i \, C_i^T \\ b &= \sum_i \left( (q_i - p_i)^T n_i \, C_i^T \right)^T\\ \end{align} \]

or in a much more complex and cumbersome notation without matrix multiplication

\[ \begin{align} K &= \sum_i \begin{pmatrix} {c_{i,x}}^2 & c_{i,x}\,c_{i,y} & c_{i,x}\,c_{i,z} & c_{i,x}\,n_{i,x} & c_{i,x}\,n_{i,y} & c_{i,x}\,n_{i,z}\\ c_{i,x}\,c_{i,y} & {c_{i,y}}^2 & c_{i,y}\,c_{i,z} & c_{i,y}\,n_{i,x} & c_{i,y}\,n_{i,y} & c_{i,y}\,n_{i,z}\\ c_{i,x}\,c_{i,z} & c_{i,y}\,c_{i,z} & {c_{i,z}}^2 & c_{i,z}\,n_{i,x} & c_{i,z}\,n_{i,y} & c_{i,z}\,n_{i,z}\\ c_{i,x}\,n_{i,x} & c_{i,y}\,n_{i,x} & c_{i,z}\,n_{i,x} & {n_{i,x}}^2 & n_{i,x}\,n_{i,y} & n_{i,x}\,n_{i,z}\\ c_{i,x}\,n_{i,y} & c_{i,y}\,n_{i,y} & c_{i,z}\,n_{i,y} & n_{i,x}\,n_{i,y} & {n_{i,y}}^2 & n_{i,y}\,n_{i,z}\\ c_{i,x}\,n_{i,z} & c_{i,y}\,n_{i,z} & c_{i,z}\,n_{i,z} & n_{i,x}\,n_{i,z} & n_{i,y}\,n_{i,z} & {n_{i,z}}^2 \end{pmatrix}\\[2ex] b &= \sum_i \begin{pmatrix} c_{i,x} (q_i - p_i)\cdot n_i\\ c_{i,y} (q_i - p_i)\cdot n_i\\ c_{i,z} (q_i - p_i)\cdot n_i\\ n_{i,x} (q_i - p_i)\cdot n_i\\ n_{i,y} (q_i - p_i)\cdot n_i\\ n_{i,z} (q_i - p_i)\cdot n_i \end{pmatrix}\\[2ex] c &= \sum_i |(q_i-p_i)\cdot n_i|^2 \end{align} \]

The minimization problem takes the form \[ \nabla F = 0 \, \Leftrightarrow \, K \Theta = -b \]

Remark

To transform the \(q_i\) points of the moving mesh using the de-linearized rotation, i.e. at the iteration \(k+1\) the points in the moving mesh are given by \[ q^{(k+1)}_i = R q^{(k)}_i + t \] where the rotation matrix is \[ R(\alpha, \beta,\gamma) = R_z(\gamma) R_y(\beta) R_x(\alpha) \]

Final remarks

Rotation matrix using Euler angles and the Tait-Bryan representation, see Wikipedia: Euler angles: \[ \begin{align} R & = R_z(\theta_3)R_y(\theta_2)R_x(\theta_1)\\[10pt] & = \begin{pmatrix} \cos\theta_3 & -\sin\theta_3 & 0\\ \sin\theta_3 & \cos\theta_3 & 0\\ 0 & 0 & 1\\ \end{pmatrix} \begin{pmatrix} \cos\theta_2 & 0 & \sin\theta_2\\ 0 & 1 & 0\\ -\sin\theta_2 & 0 & \cos\theta_2 \\ \end{pmatrix} \begin{pmatrix} 1 & 0 & 0\\ 0 & \cos\theta_1 & -\sin\theta_1 \\ 0 & \sin\theta_1 & \cos\theta_1\\ \end{pmatrix}\\[10pt] & = \begin{pmatrix} \cos\theta_3\cos\theta_2 & \cos\theta_3 \sin\theta_2\sin\theta_1 - \sin\theta_3\cos\theta_1 & \cos\theta_3 \sin\theta_2\cos\theta_1 + \sin\theta_3\sin\theta_1\\ \sin\theta_3\cos\theta_2 & \sin\theta_3 \sin\theta_2\sin\theta_1 + \cos\theta_3\cos\theta_1 & \sin\theta_3 \sin\theta_2\cos\theta_1 - \cos\theta_3\sin\theta_1\\ -\sin\theta_2 & \cos\theta_2 \sin\theta_1 & \cos\theta_2 \cos\theta_1\\ \end{pmatrix} \end{align} \]

The small angle approximation is obtained as follows Replace in the matrix above the following terms \[ \cos \theta_i \approx 1,\quad \sin\theta_i \approx \theta_i,\quad \theta_i \cdot \theta_j \approx 0, \quad \theta_i \cdot \theta_j \cdot \theta_k \approx 0 \] The linearized rotation matrix (discard all term quadratic in the angles): \[ R(\theta_1, \theta_2, \theta_3) = I + \begin{pmatrix} 0 & - \theta_3 & \theta_2\\ \theta_3 & 0 & -\theta_1\\ -\theta_2 & \theta_1 & 0 \end{pmatrix} \]