小学生入门激光SLAM<二>、2.1-前端点云帧间匹配

      帧间匹配算法

• 1、ICP匹配方法(略)
• 2、PL-ICP匹配方法
• 3、基于优化的匹配方法（catergtapher、Hecterslam）
• 4、相关匹配方法及分支定界加速（CSM，感觉了解下就行了）

1、ICP匹配方法(略)

　　有关ICP的详细证明，参考《视觉SLAM十四讲》，书中没有证明的部分，我放到下面：

上面的数学可以再查下，暂时没看透彻

　　ICP算法是EM算法的一个特例，可以参考上一节：https://www.cnblogs.com/winslam/p/14892583.html

　　EM算法是一种求解非线性MLE的一种工具。

2、PL-ICP匹配方法

　　ICP是针对points to point，PL-ICP显然是points to line的算法。

　　例如：现在有两帧点云（参考帧点：Pj1、Pj2，当前帧点：Pi），ICP依据距离最近，认为 Pj2点和 Pi 匹配上了，但是由于机器人的运动，在空间中，Pj 点和 Pi 点并不严格对应目标物体上同一个点，所以还是会引入误差。

　　PL-ICP匹配策略不同，它是：依据点到线距离最近，例如：Pi点到 直线 Pj1-Pj2距离最短（当然直线的两个端点是依据距离最近得到），那么这种关系被视为最佳匹配。

　　如下是PL-ICP点云配准目标函数，下面对标注的变量进行解释：

• 1、表示如下右图直线Pj2-Pj1的单位法向量；
• 2、表示向量Pi-Pj1（1与2之间其实是点积，公式中没写全，显然是求的是点Pi到直线Pj2-Pj1的距离，在RANSAC空间直线拟合的项目中用过的）；
• 3、表示将Pi点从当前帧转到参考帧（qk+1表示位姿咯），当成如下右图Pi点
• 4、参考帧点，当成如下右图Pj1点

 　　下面给出两种算法的区别，其实除了目标函数，其余没有区别。PL-ICP收敛更快。