[SLAM前端系列]——一文读懂ICP_java

今天突然发现这篇博客写的并不详细，配不上一文读懂这四个字，特此回来更新。

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

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

一、ICP的作用

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

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

二、激光环境下的ICP的实现 2.1 迭代最近点确定对应点

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

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

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

何为对应点？在雷达环境下，定义两坨点云Ai,Bi，以Ai点云中的A1点为例子，Bi中离A1最近的点，就认为是A1在Bi点云中的对应点，Ai中的其它点云也一样。这就是为什么叫迭代最近点。比如下图的两个黑点，point1(30,39)的对应点就是point2(20,29)。因为只有point2到他的距离是最短的。

2.2 优化方程实现

我们知道icp主要就是为了求两个点云或是矩阵的R旋转和平移T的，那么由此我们就可以设计出初步的优化方程,我们假设原始点云为pi', 目标点云为pi, 原始点云经过变换之后的点云和目标点云相减，这个残差越小，证明匹配的越好：

也就是说，只要J取得了0，或者接近于0，那就证明我们的匹配效果非常nice了。所以，我们要做的事情就是对上述式子进行极值求解，在求极值之前，把能化简的化简。完整公式见下：

三、为什么要去中心化

这其实是个很少被人说明的问题，很多人的解释是去中心化的目的就是为了方便之后的化简，这么说来是没错的。但是并不是它的本质。

其实去中心化是有几何意义的，下面我用MATLAB作图解释：

上图是两个经过RT变换后的点云，星号点分别为两个点云的中心点（计算方式就是横纵坐标分别求和除以点云数量），去中心化后如下：

个人理解：去中心化相当于做了一次平移，缩短两个点云之间的距离，目的是为了将两个可能处于不同坐标系的点云近似转换到同一个坐标系下，同时也是为了防止出现局部最优的情况。

四、为什么SVD求出的就是R

为什么SVD求出的是R？
我们可以看到公式化简到最后的式子是 , 我们要求的是一个极值，那么是否可以考虑W是存在极值的，并且满足某种极值的形式？

答案是肯定的。因为W满足柯西不等式！！！！！！既然存在不等式关系，就说明它一定存在某个极值。对于W对应的柯西不等式证明，如下图：

三、ICP的实现难点