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这里在计算直线、平面方程的时候，采取的点数量更多。

CV&DL