小学生入门激光SLAM<六>、3D激光SLAM-Loam

• 1、3D激光SLAM
• 1.1、基于Point to Plane ICP的帧间匹配
• 1.2、基于3D描述子ICP的帧间匹配
• 1.3、基于NDT的帧间匹配
• 2、LOAM
• 2.1、里程计
•      2.1.1 特征点检测
•      2.1.2 特征点匹配
•      2.1.3 运动估计
• 2.2 建图

1、3D激光SLAM

　　2D激光SLAM三个自由度(x、y、theta)，3D激光雷达6DoF。

3D地图 

 

　　　　　　　　2D激光SLAM　　　　　　　　　　　　　　　　　　　　　　3D激光SLAM

　　　　　　　　单线激光雷达 　　　　　　　　  　　　　　　　　　　　　　  多线激光雷达(300线激光雷达和depth相机一样了)
　　　　　　　　二维栅格地图 　　　　　　　　　　　　　　　　　　　　　　三维点云地图或者三维栅格地图
　　　　　　　　室内移动机器人 　　　　　　　　　　　　　　　　　　　　     室外移动机器人

                                                                           

1.1、基于Point to Plane ICP的帧间匹配

　　3D 激光SLAM中，回环、前端都是帧间匹配，常用的例如：point to plane点面距离：

 1.2、基于3D描述子ICP的帧间匹配

　　和视觉提取特征点类似 ，这里是已经对应点的ICP匹配问题，比较简单。

 1.3、基于NDT的帧间匹配

　　下图中，步骤1、2就是NDT算法核心了。

• 步骤1中，将点云空间划分出多个方格。
• 步骤2中，利用观测值xi计算二维高斯分布协方差矩阵、期望值。
• 步骤3中，将预测值(和VSLAM一样)代入高斯分布中，经过归一化打分。
• 步骤4为一帧激光数据的目标函数。

　　NDT算法和似然场差不多，只不过NDT中的Gaussian分布是连续的，似然长是离散的（必要的时候，需要插值）。

 　　NDT也不需要搜索，因为目标函数是连续的。好的SLAM算法更加依赖于公式，公式是重要，但在工程化过程中，实践技巧更加重要。以上三种算法在PCL中都有实现。

　　3D激光雷达中，里程计、回环本质都是帧间匹配，后端都是一个套路。和VSLAM比起来，激光SLAM精度相当高，例如：双目SLAM中，点匹配差一个像素，深度误差达到8cm。所以，在小场景，激光SLAM不做回环，大场景还是要做。

2、loam

　　loam没有回环，主要包含两个部分：里程计+mapping；主要是提取特征点。

 　　如下是框架图：1、2没啥好说了。0处的点云注册是啥意思？首先，一帧完整的激光点云数据包含多个scan，例如:16线激光雷达就对应16个scan，下图中 Point Cloud Registeration 表示将多个scan拼接到一起的过程(2D 激光SLAM依靠电机，雷达绑到步进电机上，所以角度是知道的)，然后给Lidar Odometry。后面位姿集成，没看懂。

　　激光里程计输出的1Hz、10Hz是怎么计算输出的？

 2.1、里程计

2.1.1 特征点检测

　　如下图，这里曲率计算公式如下图，思想和我所理解的不一样。如下图，直线L1、L2，向量aa1 - 向量aa2 = 0向量，而向量bb1 - 向量bb2很大，理论上夹角越小，得到的至越大，曲率越大。

　　一个水平激光帧对应一个scan，一个scan分为四个区域，分别提取特征点，我猜目的就是为了就是让特征不那么集中。

　　去除不稳定特征点：如下图1，B点，如果激光束再偏一点点，B点就没了；如下图2，A点是边缘特征点，但是如果激光雷达再向左边平移一点点，A点就成了平面点。

 2.1.2 特征点匹配

　　tk -> tk+1才是一个完整的扫描周期。特征点检测上一节已经讲解，这里说下如何匹配的。

　　边缘特征点的匹配：如下图，当前帧点云橙色，投影（依据什么投影的？ICP？）到参考帧点云中，例如：对于当前帧边缘点j，在参考帧中找一次近边缘点l（note：这两点必定不在同一个scan序列中，最近边缘点是当期帧的点i），所以特征匹配目标函数就是最小化：点i到直线lj的距离。

　　平面特征点的匹配：同理。

　　特征点无脑匹配，很简单，总共的点也不多（需要最小化误差函数，才能求解出粗略位姿）。

 2.1.3 运动估计

 　　构建目标函数，LM算法优化。

 　　对前面三个小结进行总结（其中不懂的变量，可对比上图进行对比观看）：

• 特征点检测
• 寻找匹配点
• 构建非线性方程组
• 求解非线性方程组
• 如果到了当前帧的末尾,则投影到当前帧的时刻,开始进行下一帧的求解(这一句你好好理解！！！！！！)

 　　上图变量不懂，可以看下图

2.2 建图

　　Qk一横(你懂得)  表示完整的，去除了运动几遍的sweep，其与局部子图Qk-1进行匹配，得到位姿得到Tk(如下图)。

 参考：https://zhuanlan.zhihu.com/p/111388877

　　至少到现在，应该直到 scan to scan 和 map to map的区别了 

 　　这里有个cubic的限制，其目的在于：多找几个边缘点，方便后续作基于SVD的3D直线拟合（这玩意我以前是做过的，不多BB），下面链接有个动态图，很好说明了SVD中最大奇异值对应的右奇异矩阵第一列就是：这堆3D点的方向向量。

链接：https://zhuanlan.zhihu.com/p/111388877 

　　对于点-平面匹配，原理一样。

　　不同于里程计，mapping这里在计算直线、平面方程的时候，采取的点数量更多。