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