Iterative Closest Point

Iterative closest point (ICP) algorithm is to align two point clouds iteratively:

1. For each point $\mathbf{x}^{(i)}$ in the query point cloud, find its corresponding point $\mathbf{y}^{(\cal{C}_i)}$ in the reference point cloud using the current estimation of rotation matrix $\mathbf{R}$ and translation $\mathbf{t}$: \begin{equation}\cal{C}_i = \arg\min_{j} \left|\left|\mathbf{y}^{(j)}-\mathbf{R}\mathbf{x}^{(i)}-\mathbf{t}\right|\right|_2^2\,.\tag{1}\end{equation}
2. Update the rotation matrix $\mathbf{R}$ and translation $\mathbf{t}$ by optimizing \begin{equation}\min_{\mathbf{R},\mathbf{t}} \sum_{i=1}^n\left|\left|\mathbf{y}^{(\cal{C}_i)}-\mathbf{R}\mathbf{x}^{(i)}-\mathbf{t}\right|\right|_2^2\,.\tag{2}\end{equation} 

To solve (2), we first take derivative on $\mathbf{t}$ and obtain \begin{equation}\mathbf{t} = \frac{1}{n}\sum_{i=1}^n\mathbf{y}^{(\cal{C}_i)}-\mathbf{R}\,\frac{1}{n}\sum_{i=1}^n\mathbf{x}^{(i)}\,.\tag{3}\end{equation} As a result, we can remove $\mathbf{t}$ from (2) and optimize the rotation matrix $\mathbf{R}$ with two centered point clouds: \begin{equation}\min_{\mathbf{R}} \sum_{i=1}^n\left|\left|\mathbf{y}^{(\cal{C}_i)}-\mathbf{R}\mathbf{x}^{(i)}\right|\right|_2^2\,.\tag{4}\end{equation} Define the covariance matrix between two point clouds \begin{equation}\mathbf{W}\equiv\sum_{i=1}^n \mathbf{x}^{(i)}\,\mathbf{y}^{T{(\cal{C}_i)}}\,,\tag{5}\end{equation} the optimization (4) is equivalent to the problem \begin{equation}\max_{\mathbf{R}}\,\text{Tr}\left(\mathbf{R}\mathbf{W}\right)\,.\tag{6}\end{equation}

The final result of the rotation matrix is $\mathbf{R}=\mathbf{V}\mathbf{U}^T$ by SVD $\mathbf{W}=\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T$. A short proof is shown in the ICP paper: we can prove that $\langle \mathbf{A},\, \mathbf{B}\rangle\equiv\text{Tr}\left(\mathbf{A}\boldsymbol{\Sigma}\mathbf{B}^T\right)$ satisfies the definition of inner product. As a result, by Schwarz inequality, $\text{Tr}\left(\mathbf{R}\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T\right)$ reaches the maximum when $\mathbf{R}\mathbf{U}=\mathbf{V}$, i.e. $\mathbf{R}=\mathbf{V}\mathbf{U}^T$.

Another proof is by Lagrangian multiplier: \begin{equation}L\left(\mathbf{R},\boldsymbol{\Lambda}, \lambda\right)\equiv -\text{Tr}\left(\mathbf{R}\mathbf{W}\right)+\text{Tr}\left(\boldsymbol{\Lambda}\left(\mathbf{R}\mathbf{R}^T-\mathbf{I}\right)\right)+\lambda\left(\det{\mathbf{R}}-1\right)\,.\end{equation}

Here involves the derivative of a determinant: Note that $\det{\mathbf{A}}=\prod_i \lambda_i=\exp{\left(\sum_i \log \lambda_i\right)}=\exp{\left(\text{Tr}\left(\log\mathbf{A}\right)\right)}$, we have $\log\left(\det{\mathbf{A}}\right)=\text{Tr}\left(\log\mathbf{A}\right)$, and thus \begin{equation}\frac{\partial L}{\partial \mathbf{R}}=-\mathbf{W}^T + 2\boldsymbol{\Lambda}\mathbf{R}+\lambda \det{\mathbf{R}}\,\mathbf{R}^{-T}=-\mathbf{W}^T+\left(2\boldsymbol{\Lambda}+\lambda\right)\mathbf{R}\,,\end{equation} which gives $\left(2\boldsymbol{\Lambda}+\lambda\right)\mathbf{R}=\mathbf{W}^T$. Multiplying it with $\mathbf{R}^T\left(2\boldsymbol{\Lambda}+\lambda\right)=\mathbf{W}$ leads to $2\boldsymbol{\Lambda}+\lambda=\left(\mathbf{W}^T\mathbf{W}\right)^{1/2}$. As a result, $\mathbf{R}=\left(\mathbf{W}^T\mathbf{W}\right)^{-1/2}\mathbf{W}^T=\mathbf{V}\mathbf{U}^T$.