ICP算法可以使用SVD或者非线性优化的方法进行求解。因为PCL源码中对ICP的求解就是使用SVD，所以这里对其进行一些探究。
1.首先对第i i i e i = p i − ( R p i ′ + t )
e_{i}=p_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)
e i = p i − ( R p i ′ + t )
2.构建最小二乘问题：求得使得目标函数最小的未知量R , t R,t R , t min R , t J = 1 2 ∑ i = 1 n ∥ ( p i − ( R p i ′ + t ) ) ∥ 2 2
\min _{\boldsymbol{R}, t} J=\frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)\right)\right\|_{2}^{2}
R , t min J = 2 1 i = 1 ∑ n ∥ ( p i − ( R p i ′ + t ) ) ∥ 2 2
首先定义两组点云的质心：
p = 1 n ∑ i = 1 n ( p i ) , p ′ = 1 n ∑ i = 1 n ( p i ′ )
\boldsymbol{p}=\frac{1}{n} \sum_{i=1}^{n}\left(\boldsymbol{p}_{i}\right), \quad \boldsymbol{p}^{\prime}=\frac{1}{n} \sum_{i=1}^{n}\left(\boldsymbol{p}_{i}^{\prime}\right)
p = n 1 i = 1 ∑ n ( p i ) , p ′ = n 1 i = 1 ∑ n ( p i ′ )
4.在误差函数中添加质心，进行配平1 2 ∑ i = 1 n ∥ p i − ( R p i ′ + t ) ∥ 2 = 1 2 ∑ i = 1 n ∥ p i − R p i ′ − t − p + R p ′ + p − R p ′ ∥ 2 = 1 2 ∑ i = 1 n ∥ ( p i − p − R ( p i ′ − p ′ ) ) + ( p − R p ′ − t ) ∥ 2 = 1 2 ∑ i = 1 n ( ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2 + 2 ( p i − p − R ( p i ′ − p ′ ) ) T ( p − R p ′ − t ) )
\begin{aligned}\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{p}_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)\right\|^{2} &=\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{p}_{i}-\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}-\boldsymbol{t}-\boldsymbol{p}+\boldsymbol{R} \boldsymbol{p}^{\prime}+\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}\right\|^{2} \\&=\frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right)+\left(\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right)\right\|^{2} \\&=\frac{1}{2} \sum_{i=1}^{n}\left(\left\|\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right\|^{2}+\left\|\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right\|^{2}+\right.\\&\left.2\left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right)^{T}\left(\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right)\right)\end{aligned}
2 1 i = 1 ∑ n ∥ p i − ( R p i ′ + t ) ∥ 2 = 2 1 i = 1 ∑ n ∥ p i − R p i ′ − t − p + R p ′ + p − R p ′ ∥ 2 = 2 1 i = 1 ∑ n ∥ ( p i − p − R ( p i ′ − p ′ ) ) + ( p − R p ′ − t ) ∥ 2 = 2 1 i = 1 ∑ n ( ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2 + 2 ( p i − p − R ( p i ′ − p ′ ) ) T ( p − R p ′ − t ) ) ( p i − p − R ( p i ′ − p ′ ) ) \left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right) ( p i − p − R ( p i ′ − p ′ ) ) min R , t J = 1 2 ∑ i = 1 n ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2
\min _{\boldsymbol{R}, \boldsymbol{t}} J=\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right\|^{2}+\left\|\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right\|^{2}
R , t min J = 2 1 i = 1 ∑ n ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2
5.仔细观察左右两项，我们发现左边只和旋转矩阵 R R R 也 有 也有 也 有 R R R t t t
5.1 计算两组点的质心位置 p , p ′ p, p′ p , p ′ q i = p i − p , q i ′ = p i ′ − p ′
\boldsymbol{q}_{i}=\boldsymbol{p}_{i}-\boldsymbol{p}, \quad \boldsymbol{q}_{i}^{\prime}=\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}
q i = p i − p , q i ′ = p i ′ − p ′
5.2 根据以下优化问题计算旋转矩阵：R ∗ = arg min R 1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2
\boldsymbol{R}^{*}=\arg \min _{\boldsymbol{R}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{q}_{i}-\boldsymbol{R} \boldsymbol{q}_{i}^{\prime}\right\|^{2}
R ∗ = arg R min 2 1 i = 1 ∑ n ∥ q i − R q i ′ ∥ 2
5.3 根据第二步的 R R R t ∗ = p − R p ′
\boldsymbol{t}^{*}=\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}
t ∗ = p − R p ′
6.根据以上推导，我们重点需要求出旋转量R R R t t t
1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2 = 1 2 ∑ i = 1 n q i T q i + q i ′ T R T R q i ′ − 2 q i T R q i ′
\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{q}_{i}-\boldsymbol{R} \boldsymbol{q}_{i}^{\prime}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{n} \boldsymbol{q}_{i}^{T} \boldsymbol{q}_{i}+\boldsymbol{q}_{i}^{\prime T} \boldsymbol{R}^{T} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}-2 \boldsymbol{q}_{i}^{T} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}
2 1 i = 1 ∑ n ∥ q i − R q i ′ ∥ 2 = 2 1 i = 1 ∑ n q i T q i + q i ′ T R T R q i ′ − 2 q i T R q i ′
注意到第一项和 R R R R T R = I R^TR = I R T R = I R R R ∑ i = 1 n − q i T R q i ′ = ∑ i = 1 n − tr ( R q i ′ q i T ) = − tr ( R ∑ i = 1 n q i ′ q i T )
\sum_{i=1}^{n}-\boldsymbol{q}_{i}^{T} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}=\sum_{i=1}^{n}-\operatorname{tr}\left(\boldsymbol{R} \boldsymbol{q}_{i}^{\prime} \boldsymbol{q}_{i}^{T}\right)=-\operatorname{tr}\left(\boldsymbol{R} \sum_{i=1}^{n} \boldsymbol{q}_{i}^{\prime} \boldsymbol{q}_{i}^{T}\right)
i = 1 ∑ n − q i T R q i ′ = i = 1 ∑ n − t r ( R q i ′ q i T ) = − t r ( R i = 1 ∑ n q i ′ q i T ) 3 ∗ 1 ∗ 1 ∗ 3 3*1 * 1*3 3 ∗ 1 ∗ 1 ∗ 3
7.对这个优化函数的求解，则使用SVD来完成。定义矩阵：W = ∑ i = 1 n q i q i ′ T
\boldsymbol{W}=\sum_{i=1}^{n} \boldsymbol{q}_{i} \boldsymbol{q}_{i}^{\prime T}
W = i = 1 ∑ n q i q i ′ T W W W W W W W = U Σ V T
\boldsymbol{W}=\boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^{T}
W = U Σ V T Σ Σ Σ U U U V V V W W W R R R R = U V T
\boldsymbol{R}=\boldsymbol{U} \boldsymbol{V}^{T}
R = U V T t t t
align 函数开始，调用关系如上图所示。
