icp算法的实现_点云ICP注册

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者：GeometryHub 链接：https://zhuanlan.zhihu.com/p/86042646 本文转载自知乎，作者已授权，未经许可请勿二次转载。

背景

两个点云要注册在一块，一般分两个步骤：先做一个大致的对齐，也就是所谓的初始注册，一般可以通过一些可靠的点对来计算得到(如图3所示)；然后在初始注册的基础上进行精细注册，提升注册的精度(如图4所示)。精细注册的方法，一般采用ICP算法，也就是最近点迭代的方法。

ICP算法总览

下面先总的介绍一下ICP算法，之后再详细介绍里面的一些重要步骤。

算法输入是两片有部分重叠的点云a和b，并且已经初始注册好了，输出是ICP注册的刚体变换T：

1. 对b进行点采样，得到采样点集s

2. 在a中寻找采样点集s的最近对应点，得到点对集合c

3. 对c中的点云进行加权处理，并删掉一些不好的点对

4. 应用目标能量来优化点对距离，得到刚体变换Ti。Ti对采样点集进行刚体变换

5. 迭代步骤2-4，直到目标能量优化停止。T=Tn * ...... T1 * T0

点采样

由于计算速度的要求，一般是需要对点云b进行采样。然后用采样点去找对应进行优化。除了计算上的要求，如果用全点云进行匹配的话，精度也不会更加的好。因为更好的采样方法可以避免陷入能量局部极小的情况。常见的采样方法有均匀采样和几何采样。

• 均匀采样：采样点分布均匀，采样速度快，适合几何特征比较多的点云。因为这样的点云，均匀采样总能采样到几何特征。如果几何特征少的话，如下左图所示，有可能就采样不到几何特征。

• 几何采样：采样点会在几何特征明显的地方被采样到，如下右图所示。它能够抓住点云的几何特征，使得注册精度更高，更稳定。计算速度可能会慢一些，并且不太适合噪音比较大的点云，因为噪音其实就是几何特征了。

一个比较理想的采样方法，既能够采样到点云的几何特征，分布上也能做到局部均匀，然后就是计算速度要快。下面是一个采样示例，左图是原始点云，中间图是均匀采样，右图是几何采样。

点对应

从ICP的名字，就能看出点对应怎么去找，也就是给每个采样点找最近点。查找最近点是比较简单的，一般用KD Tree来加速查找。这些点对，有些是无效的，需要剔除掉。常用的剔除策略有距离和法线：

• 距离：点云a和b一般只有部分重叠，b的采样点集里，有部分点在a里是没有对应的。记这部分点为c，那么c的对应点对是需要剔除掉的。经过观察，我们发现c的点对距离一般是比较大的，所以可以设置一个距离阈值，来过滤掉这些点对。这个阈值算是一个参数了，可以根据点云的平均距离来设置，迭代开始的时候设置的大一些，如果找到的点对数目太少，可以适当的增大这个距离阈值。ICP迭代过程中，点云距离会逐渐减小，这个距离阈值也可以随之动态减小。

• 法线：在ICP迭代初期，点云位姿相差比较大，很多距离相近的点对也是错误的无效点对。我们可以根据点的一些属性来过滤掉这些无效的点对。常用的属性就是点云法线。比如点法线夹角要小于一个角度阈值。这个阈值和距离阈值一样，也是动态变换的。

目标能量

常用的目标能量有两种：点到点的能量和点到平面的能量。直观上讲，点到点的能量如左图所示，优化的是有效点对之间的距离；点到平面的能量，如右图所示，优化的是点到点云局部平面的距离。

• 点到点的能量：∑ || a - T(s) ||：其中s是点云b的有效采样点，a是s对应的点，T是刚体变换

• 点到平面的能量：∑ || (a - T(s)) * n(s) ||：其中n(s)是采样点s的法线

这两个能量，各有优缺点：

• 速度：本质上两个能量都是优化两个点云曲面的距离，点到点能量是线性收敛，点到平面能量的迭代等价于Gauss-Newton迭代，它的收敛速度是优于线性收敛的，情况好的时候，可以达到二阶收敛速度。

• 点到点能量不需要法线信息。有时候可靠的法线信息不容易得到，比如曲面严重不光滑，噪音十分严重，或者点云相对于物体采样非常稀疏等。不可靠的法线会使得点到平面的能量优化不稳定。

求解目标能量

能量里面只有T的旋转变换是非线性的，可以转化成线性最小二乘求解。常用的两种转化方式：

• 一种是把T当作仿射变换，对求解得到的T做SVD分解，求得仿射变换在刚体变换空间中的投影

• 另一种是把旋转矩阵用欧拉角的方式来表示，这样T里面的非线性部分就是sin和cos。我们假设每次迭代刚体变换的旋转角度不大，则cos(theta) = 1, sin(theta) = theta。这样就把T转化为线性矩阵了。

迭代停止条件

迭代算法总要有个停止条件。这个看似平凡的步骤，却是所有迭代算法的关键。它能影响算法最终的效果和性能。ICP常见的迭代停止条件：

• 最大迭代次数

• 迭代过程中，刚体变换近似恒等变换了

• 迭代过程中，点云之间的距离小于一定的阈值

• 迭代过程中，点云之间的距离越来越大了，需要中止无效迭代。或者更新算法参数重新迭代。

推荐阅读：

吐血整理|3D视觉系统化学习路线

那些精贵的3D视觉系统学习资源总结(附书籍、网址与视频教程)

超全的3D视觉数据集汇总

大盘点|6D姿态估计算法汇总(上)

大盘点|6D姿态估计算法汇总(下)

机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划

汇总|3D点云目标检测算法

汇总|3D人脸重建算法 那些年，我们一起刷过的计算机视觉比赛 总结|深度学习实现缺陷检测 深度学习在3-D环境重建中的应用 汇总|医学图像分析领域论文 大盘点|OCR算法汇总

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会(ICRA/IROS/ROBIO/CVPR/ICCV/ECCV等)、顶刊(IJCV/TPAMI/TIP等)、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近1000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款 圈里有高质量教程资料、可答疑解惑、助你高效解决问题