ICP定位算法学习笔记（一）

1 经典ICP

　　ICP的目的很简单，就是求解两堆点云之间的变换关系。怎么做呢？思路很自然，既然不知道R和t(针对刚体运动)，那我们就假设为未知量呗，然后通过某些方法求解。下面我们来看看具体怎么求的～没办法，要把问题描述清楚，数学是少不了的了。假设有两堆点云，分别记为两个集合X=x1,x2,…,xm和Y=y1,y2,…,ym（m并不总是等于n）。然后呢，我们不失一般性的，假设两个点云之间的变换为R（旋转变换）和t（平移变换），这两个就是我们要求的东西啦～那我们将求解这个问题描述成最小化均方误差：
　　

　　经典的ICP方法对上面的优化问题的处理思路如下：
　　（1）初始化R 和 t
　　确定初始的R 和 t 的方法很多，如果什么方法都不知道，那随便赋一个R 和 t ，然后就迭代的算呀。随便给一个值从原理上来说也可以得到最终的一个结果呀，但是准不准就不知道了。相信有基本的优化概念的人都知道，初始值的选取很重要，如果初始值选的不好很容易收敛到一个局部最优解，然后局部最优解好不好那就另说了。ICP发展了这么多年了，当然有很多的方法来估计初始的R和t了，像PCL给的SampleConsensusInitalAlignment函数以及TransformationEstimationSVD函数都可以得到较好的初始估计。
　　（2）迭代
　　得到初始的估计后，接下来的步骤就顺理成章了：对于X中的每一个点用当前的R 和 t在Y中找最近的点（比如用欧式距离），然后这两个点就成了一对了～就这样，对所有的点都这么做一次，然后我们就得到了所有的匹配对了～然后呢，用每一对的坐标列一个方程，就得到一系列的方程。然后就求解最优的R和t最小化上面的误差。如此循环往复。
2 ICP变种

　　除了经典的ICP方法外，还有一些变种，如point-to-point的，point-to-plane的以及plane-to-plane的，那么这三种方法到底是啥呢？
　　其实很简单，就是上面的误差函数的定义不一样而已。在上面讲经典ICP的时候，求和的每一项不就是X中的每一个点到Y中的每一个点的距离吗？那就是point-to-point了，那么将求和的每一项变成X中的每一个点到Y中的平面的距离，那就是point-to-plane了呀～类似的，如果把求和的每一项变成X中的平面到Y中的平面的距离，那就是plane-to-plane了。我们说了这么久的平面，那么平面到时是怎么定义的呢？
　　point-to-plane的误差函数定义为：
　　
　　
迭代最近点(Iterative Closest Points, ICP)算法包括两部分：对应点搜索和位姿求解。它的目的是寻求点集之间的匹配关系，求解的结果是两点集之间的平移及旋转量。假设M、P是两个点集，P为待配准点集，M为基准数据点集（可以理解为两个不同坐标系中的点集），其基本原理如下：
1、搜索最近点：取P中一点p（i），在M中找出距离p（i）最近的m（i），则（pi，mi）就构成了一组对应点对集，但是p（i）与m（i）之间存在着某种旋转和平移关系（R，T），这也就是我们要求的。
2、求解变换关系（R，T）：n对点（pi，mi）对于n个方程组，那么就一定能运用数学方法求解得出（R，T），但是为了求解更加精确的变换关系，采用了迭代算法。
3、应用变换：对点集P中的每一个点pi运用变换关系得到点集P2，定义函数E，

根据精度要求，定义终止迭代的条件，即E小于一个具体值时终止迭代。（可以把E理解为经过变换后的P2中每个点与M点集中对应点的距离和）。
4、重复迭代：如果某次迭代满足要求，则终止迭代，输出最优（R，T），否则继续迭代，但是要注意一点：在每一次迭代开始时都要重新寻找对应点集。也就是说要把结果变换的Pn带入函数E中继续迭代。
参考：
http://www.cnblogs.com/jian-li/articles/4945676.html
http://blog.csdn.net/qq_26849233/article/details/49050575