迭代最近点 ICP 详细推导（C++实现）

Alan Lan

已于 2022-03-30 17:10:39 修改

阅读量1.2w

点赞数 16

分类专栏： PCL 文章标签： c++ 线性代数算法

于 2020-07-20 19:39:46 首次发布

本文链接：https://blog.csdn.net/A_L_A_N/article/details/81329728

版权

一、简介

迭代最近点算法（Iterated Closest Points, ICP），顾名思义，就是采用迭代优化的思想以空间距离作为匹配点的选择依据，通过不断调整点云的位姿使得匹配点之间距离累计最小。假设有两组点云，其中一个目标点云A另一个为参考点云B，ICP算法的目的是为了算出一个最优的旋转矩阵R和平移向量t使得变换后的点云A能与B达到最精确的匹配。ICP算法因为其思想简单，精度高等特点成为了局部配准的主流算法。

ICP需要反复执行以下两步直至收敛：第一步，计算A点集与B点集之间的匹配点对；第二步，根据前一步得到的匹配点对计算出A点集与B点集之间的转换矩阵，并对A点集进行相应的转换。在取得了点集A与B的匹配点基础上，点集之间的转换矩阵求解也分为两种方式：利用线性代数的求解（主要是SVD），以及利用非线性优化方式的求解（类似于Bundle Adjustment）。其中奇异值分解法（SVD）因为准确、稳定以及高效等特点被广泛运用，本文主要介绍利用SVD进行转换矩阵的求解。

最近点对查找：对应点的计算是整个配准过程中耗费时间最长的步骤，查找最近点，利用 kd tree提高查找速度。kd tree 法建立点的拓扑关系是基于二叉树的坐标轴分割，构造 kd tree 的过程就是按照二叉树法则生成。首先按 X 轴寻找分割线，即计算所有点的x值的平均值，以最接近这个平均值的点的x值将空间分成两部分，然后在分成的子空间中按 Y 轴寻找分割线，将其各分成两部分，分割好的子空间在按X轴分割……依此类推，最后直到分割的区域内只有一个点。这样的分割过程就对应于一个二叉树，二叉树的分节点就对应一条分割线，而二叉树的每个叶子节点就对应一个点，这样点的拓扑关系就建立了。

ICP方法的配准步骤如下：假设给两个三维点集 X1 和 X2，

第一步，计算X2中的每一个点在X1 点集中的对应近点；

第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；

第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；

第四步，如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。

二、详细推导

问题描述：

假设存在两个点云集合 $\{p\}$ 和 $\{p^{'}\}$ ，求：一个欧式变换 R, t 使得

求解问题：

假设误差项为：

$e_{i}=p_{i}-(Rp_{i}^{'}+t)$

那么问题转化为优化问题：

$\min_{R,t}J=\frac{1}{2}\sum_{i=1}^{n}\left \| (p_{i}-(Rp_{i}{'}+t)) \right \|^{2}$

定义质心为：

$p=\frac{1}{n}\sum_{i=1}^{n}(p_{i}),p^{'}=\frac{1}{n}\sum_{i=1}^{n}(p_{i}^{'})$

那么有：

$\frac{1}{2}\sum_{i=1}^{n}\left\| p_{i}-(Rp_{i}{'}+t) \right \|^{2} \\=\frac{1}{2}\sum_{i=1}^{n}\left \|p_{i}-Rp_{i}^{'}-t-p+Rp^{'}+p-Rp^{'}\right \|^{2} \\=\frac{1}{2}\sum_{i=1}^{n}\left(\left \|p_{i}-p-R(p_{i}^{'}-p^{'})\right \|^{2}+\left \| p-Rp^{'}-t \right \|^{2}+2(p_{i}-p-R(p_{i}^{'}-p^{'}))^{T}(p-Rp^{'}-t)\right)$

因为

所以问题转化为：

$\min_{R,t}J=\frac{1}{2}\sum_{i=1}^{n}\left (\left \|p_{i}-p-R(p_{i}^{'}-p^{'})\right \|^{2}+\left \| p-Rp^{'}-t \right \|^{2} \right )$

左右两项都大于零，左边只和旋转矩阵R相关，而右边既有R也有t，但只和质心相关。可以先求出R，再令第二项为零就能得到t。

1、计算两组质心位置 $p,p^{'}$ ，然后计算每个点的去质心坐标：
$q_{i}=p_{i}-p,q_{i}^{'}=p_{i}^{'}-p^{'}$
2、根据以下优化问题计算旋转矩阵：
$R^{*}=\arg \min_{R}\frac{1}{2}\sum_{i=1}^{n} \left \| q_{i}-Rq_{i}^{'} \right \|^{2}$
3、根据第2步的R计算t：
$t^{*}=p-Rp^{'}$