[SLAM前端系列]——一文读懂ICP_区块链

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

我们知道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的实现难点

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

结果不准确。

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

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

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

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

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

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

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

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

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

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

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