搞懂ICP（最近邻迭代）配准算法 - 2

前文我们已经有做配准的工具和数据（1. 三维点云配准的简介、工具和数据集），接下来讲配准里最经典的算法，迭代最近邻算法，英文Iterative Closest Point（ICP）。提到Iterative有没有想到代码里出现频繁的i++大兄弟？是个循环呀。我看了很多介绍ICP算法的，上来都是介绍一次配准算法公式（p=Rq+T，详见：https://mp.weixin.qq.com/s/V6ZZ42cFQC3SxpIhcroB7A或者高博的SLAM十四讲），ICP为啥叫最近邻迭代的背景没讲，ICP里有啥也没讲，导致新手碰到一次配准计算公式以外的东西，一头雾水。

ICP其实是一种算法框架，一种算法思路，一个循环底下包括一次匹配关系估计、一次匹配关系的约束（可能是多个约束的叠加）、一次配准计算、一次源数据和配准矩阵的变换、以及两个中止条件的判断，见下图。

那问题来了，为什么会有ICP算法，存在即是合理呀？

我们先看一次匹配计算公式，正常来讲，黄色的的source方块和绿色的target方块正确估计匹配关系（黑色的线），一次匹配计算公式配准就结束了，通常我们做的标定就是人为的把匹配关系对应起来，计算的二者的旋转平移矩阵即可。

那么在实际应用过程中，计算机还没聪明到准确知道黄色source方块和绿色的target方块之间的匹配关系，可能算成下图左边图示的模样（计算机通过FLANN查找最近邻的匹配关系），另外实际应用时，三维点云数据是有噪声的，这时候连人工匹配它们之间的关系都不好使了。

所以就有了通过多次迭代循环来近似求解，源点云循环一次动一下，下一次再估计一下最近邻的匹配关系，再来求解一次位姿变换，最终只要邻近的两次位姿变换太小了（是ICP陷入局部最优解的根本原因），或者超过设定的循环次数就跳出迭代了。

按照ICP算法框架，部分ICP的PCL代码如下（预留）：

 

ICP一般用作配准里的精确配准，为了节省时间，一般会为ICP提供一个粗配准输出的初值，一般用于刚性物体的配准。柔性ICP配准算法比刚性ICP算法复杂（比刚性ICP好玩），输出的是多个节点的变换位姿。

刚柔配准我的闲鱼号都有视频，欢迎交流。