ICP算法公式推导和PCL源码解析

ICP算法可以使用SVD或者非线性优化的方法进行求解。因为PCL源码中对ICP的求解就是使用SVD，所以这里对其进行一些探究。

1.公式推导

• 1.首先对第$i$个点构建误差项：
  $$e_i=p_i-\left(\mathbf{R}{\mathbf{p}}_i^{\prime }+\mathbf{t}\right)$$

• 2.构建最小二乘问题：求得使得目标函数最小的未知量$R,t$
  $$\underset{\mathbf{R},t}{\min }J=\frac{1}{2}\sum _{i=1}^n{\Vert \left({\mathbf{p}}_i-\left(\mathbf{R}{\mathbf{p}}_i^{\prime }+\mathbf{t}\right)\right)\Vert }_2^2$$
  接下来定义一些需要用的变量：

  首先定义两组点云的质心：

$$\mathbf{p}=\frac{1}{n}\sum _{i=1}^n\left({\mathbf{p}}_i\right),{\mathbf{p}}^{\prime }=\frac{1}{n}\sum _{i=1}^n\left({\mathbf{p}}_i^{\prime }\right)$$

• 4.在误差函数中添加质心，进行配平
  12∑i=1n∥pi−(Rpi′+t)∥2=12∑i=1n∥pi−Rpi′−t−p+Rp′+p−Rp′∥2=12∑i=1n∥(pi−p−R(pi′−p′))+(p−Rp′−t)∥2=12∑i=1n(∥pi−p−R(pi′−p′)∥2+∥p−Rp′−t∥2+2(pi−p−R(pi′−p′))T(p−Rp′−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} 21​i=1∑n​∥pi​−(Rp