我们知道SLAM可以用2D/3D雷达或者相机实现（视觉SLAM）。由于笔者是激光SLAM工程师，所以本文的ICP都是在激光雷达的基础上实现的。为了能让大家更直观的理解，我会尽量减少大段的公式，而改用编程思路来描述。

    ICP的翻译叫做迭代最近点，那么问题来了，什么是最近点？谁的最近点？怎么迭代？ICP的输出是什么？输入是什么？作用又是什么？

一、ICP的作用

    载有雷达（2D或3D）的机器人在移动时，由于机器人产生了一个平移（x，y方向）+一个旋转（机器人的角度），所以雷达扫描到的同一个物体得到的点云信息也相应的会出现一个变化，这个变化是一个经过了旋转和平移之后的位姿变化。  （下图是网上找的点云图）

   ICP可以做的事情就是，求出这个旋转和平移。

二、激光环境下的ICP的实现

    激光点云的传回数据是单一的，只有x和y的坐标值（当然也可以看作是距离和角度值）。那么激光环境下的ICP全部的依赖就是点的x和y，当然，是很多个x和y。

    如上面的点云图所示，雷达扫到了同一个物体，但是由于雷达本身有移动，所传回的物体的点云也有所移动，我们知道这些点云其实就是无数个x和y，那么我们如何通过这么多的x和y来求得雷达的移动距离和偏转角度呢？

   首先第一步，就是要找对应点，也就是迭代最近点的关键！

   何为对应点？在雷达环境下，下一帧的点云中距离自己最近的点，我们就定义它为对应点。这就是为什么叫迭代最近点。

   现在我们定义两帧点云，初始帧点云为A，下一帧点云为B，遍历A中的每一个点，去找B中离它最近的点，构成了一个长度和A一样的点集C。在进行了去中心化之后，然后对A和C进行SVD求解，就可以得到旋转R和T了。去中心化的具体操作可以看视觉slam14讲。

三、ICP的实现难点

   经过上面的步骤，其实就可以得到R和T了，但是，这时候就出现了一个问题。

   结果不准确。

   在算法实现中，如果出现了求解值不准确的情况，那么一般做法就是——多求几次，也就是迭代！

   所以ICP的完整步骤可以参考如下：

1.  从B点云中一一找到A中点的对应距离最近点，构成最近点集C

2. 把C点集存入Eigen矩阵中，和A点云去中心化后，求SVD分解，得到R矩阵和T向量（一个旋转一个平移）

3. 开始迭代，通过R×A+T得到新的点云A1

4. 重新执行1到3步骤，这次是从B中找A1的最近点

5. 求得到的点云An和它的最近点集Cn的平均距离dst，当dst小于设定的阈值时，跳出循环

四、激光环境下ICP的难点及处理建议

   在激光环境下，ICP很容易匹配出错，当ICP匹配出错时，说明它已经在进行匹配了，但是由于某些原因，它的迭代条件——也就是平均距离dst判断出错了，出现这种原因一般就是点云中出现了离散点，导致某两点的距离出现了异常，带动了整个dst判断出错。解决方案如下：

1.   遍历A点找寻最近点，如果A中的某个点Ai和它的最近点距离大于某个阈值，则剔除，不参与接下来的计算。
2.   从B点云中一一找到A中点的对应距离最近点，构成最近点集C
3.   把C点集存入Eigen矩阵中，和A点云去中心化后，求SVD分解，得到R矩阵和T向量（一个旋转一个平移）
4.  开始迭代，通过R×A+T得到新的点云A1
5. 重新执行1到4，每次执行都要剔除一下离散点。
6. 求得到的点云An和它的最近点集Cn的平均距离dst，当dst小于设定的阈值时，跳出循环

五、总结

      ICP的实现过程并不难，在这里我没有依赖PCL，单纯的用Eigen库就可以非常好的复现，由于工作原因，代码不能上传。有问题可以私信讨论，拒绝白嫖。