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

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

1.公式推导

• 1.首先对第$i$i个点构建误差项：
  $e_{i}=p_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)$ei​=pi​−(Rpi′​+t)

• 2.构建最小二乘问题：求得使得目标函数最小的未知量$R,t$R,t
  $\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,tmin​J=21​i=1∑n​∥(pi​−(Rpi′​+t))∥22​
  接下来定义一些需要用的变量：

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

$\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=n1​i=1∑n​(pi​),p′=n1​i=1∑n​(pi′​)

• 4.在误差函数中添加质心，进行配平
  $\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​−(Rpi′​+t)∥2​=21​i=1∑n​∥pi​−Rpi′​−t−p+Rp′+p−Rp′∥2=21​i=1∑n​∥(pi​−p−R(pi′​−p′))+(p−Rp′−t)∥2=21​i=1∑n​(∥pi​−p−R(pi′​−p′)∥2+∥p−Rp′−t∥2+2(pi​−p−R(pi′​−p′))T(p−Rp′−t))​
  注意到交叉项部分中,$\left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right)$(pi​−p−R(pi′​−p′))在求和之后是为0的，因此目标优化函数可以进行化简，变成：
  $\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,tmin​J=21​i=1∑n​∥pi​−p−R(pi′​−p′)∥2+∥p−Rp′−t∥2

• 5.仔细观察左右两项，我们发现左边只和旋转矩阵 $R$R 相关，而右边既有 $R $也有$也有 $t，但 只和质心相关。只要我们获得了 $R$R，令第二项为零就能得到 $t$t。于是，ICP 可以分为以下 三个步骤求解：

  • 5.1 计算两组点的质心位置 $p, p′$p,p′，然后计算每个点的去质心坐标：
    $\boldsymbol{q}_{i}=\boldsymbol{p}_{i}-\boldsymbol{p}, \quad \boldsymbol{q}_{i}^{\prime}=\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}$qi​=pi​−p,qi′​=pi′​−p′

  • 5.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∗=argRmin​21​i=1∑n​∥qi​−Rqi′​∥2

  • 5.3 根据第二步的 $R$R，计算$ t$：
    $\boldsymbol{t}^{*}=\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}$t∗=p−Rp′

• 6.根据以上推导，我们重点需要求出旋转量$R$R，求得之后代入自然可以获得$t$t。对公式（6）进行展开：

$\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}$21​i=1∑n​∥qi​−Rqi′​∥2=21​i=1∑n​qiT​qi​+qi′T​RTRqi′​−2qiT​Rqi′​

​ 注意到第一项和 $R$R 无关，第二项由于 $R^TR = I$RTR=I，亦与 $R$R 无关。因此，实际上优化 目标函数变为：
$\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​−qiT​Rqi′​=i=1∑n​−tr(Rqi′​qiT​)=−tr(Ri=1∑n​qi′​qiT​)
​ 其实就是$3*1 * 1*3$3∗1∗1∗3的矩阵，自然只有对角线上的数据表示两个点之间的有效数据。

• 7.对这个优化函数的求解，则使用SVD来完成。定义矩阵：
  $\boldsymbol{W}=\sum_{i=1}^{n} \boldsymbol{q}_{i} \boldsymbol{q}_{i}^{\prime T}$W=i=1∑n​qi​qi′T​
  $W$W 是一个 3 × 3 的矩阵，对 $W$W 进行 SVD 分解，得：
  $\boldsymbol{W}=\boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^{T}$W=UΣVT
  其中，$Σ$Σ 为奇异值组成的对角矩阵，对角线元素从大到小排列，而 $U$U 和 $V$V 为正交矩 阵。当 $W$W 满秩时，$R$R 为：
  $\boldsymbol{R}=\boldsymbol{U} \boldsymbol{V}^{T}$R=UVT
  求得结果后，根据公式（7）即可获得$t$t的结果。

2.PCL源码阅读

整个源码从align函数开始，调用关系如上图所示。
需要注意的是，PCL中根据算法的分类，对所有的代码进行归类。比如pcl属于Registration类，那么所有的匹配算法都统一放在Registration文件夹下。
其中h文件是定义好的函数接口，一些简单的函数直接在里面通过内联函数的形式进行了完善。
impl（implement）文件夹下的hpp文件则是对h文件中定义的复杂文件的实现。
src文件夹则是一个空的.cpp文件，只包含了对应的头文件。个人理解主要是为了编译生成可调用的库文件。