loam框架的注意点，公式推导细节，lego-loam对其的改进及自身不足，

目录

前端流程及注意细节：

总体结构：

特征提取部分：

特征关联匹配部分：

最小二乘优化部分：

后端部分：

lego-loam对其的改进：

---

前端流程及注意细节：

总体结构：

1.激光slam框架loam的流程图如下。slam的过程使用帧间匹配作为里程计，但在实际工程应用中，可以与先验地图做匹配实现定位，免去了不必要的建图流程（但又会牵扯出地图更新问题，即long-term slam）。

特征提取部分：

2.loam利用机械式多线激光雷达模型，将点云投影到为透视图，这样在单帧点云中，进行临近搜索和计算的效率会特高很多。

3.投影的方式有两种：

1）可根据点云坐标解算每个点所属的行和列，这样计算量大，对16线雷达（如velodyne16线的行/扫描线夹角为2°）的效果尚可，但随着线数增多，可能会出现解算错误（单个点甚至会算错2行）。

2）对于直接在点云数据中提供行号的雷达，可通过雷达驱动自带的点云格式中，直接读取每个点的行号，避免了计算量和计算错误。

4.下图为每个点曲率的计算公式（看着很复杂），但实际的代码中，就是计算了每根扫描线上的二维曲率（即前后各5个点形成的夹角）。

5.还对一些异常点做了剔除处理。如遮挡严重的B1和间断点A2。

6.下来对提取到的特征按照曲率大小进行分类，分类平面点和边缘点。细分为以下四类是为了匹配和后端建图使用。即匹配使用特征点ac，bd则为对应关联的线和面特征点，且建图时使用的是bd。

特征关联匹配部分：

7.前端里程计仅使用相邻帧间匹配。

8.首先将当前时刻k+1的特征点投影到k时刻（根据一个预估的R，t）特征所在的坐标系，投影之后的点记为X^。

9.通过关联最临近边缘和面特征来计算残差，使得残差最小的Rt即为帧间位姿变换：

1）边缘点：对于当前帧投影而来的点i，寻找上一帧点云中对应的最临近点j（使用kdtree），并在上一帧点云中的上或下相邻行，找相邻j最近的点l，计算点i到线jl的距离，即为线特征的残差；

2）平面点：对于当前帧投影而来的点i，寻找上一帧点云中对应的最临近点j（使用kdtree），再在同一行找距离j最近的点l，并在上一帧点云中的上或下相邻行，找相邻j最近的点m，计算点i到jlm构成的面的距离，即为面特征的残差；

注意：此处仅点i为当前帧的特征点云，其余皆为上一帧的特征点云。

10.上一步构建完误差损失函数。下来具体的计算，需要代入预设好的位姿变换Rt。

11.此处公式中对旋转的参数描述使用的欧拉角（公式和代码会比较繁琐，现在多用四元数形式计算），旋转顺序为z->x->y。写成对应的矩阵形式，最后的损失函数如下。

12.这里需要注意的是参数符号的变化，带^的为转换到同一坐标系下的坐标，不带的为在自身雷达坐标系下的坐标（可直接读取点云获得）。

最小二乘优化部分：

13.使用LM进行最小二乘优化，重点在求损失函数的雅克比矩阵上。

14.求解过程如下，先将求导公式链式分解为两部分。

15.第一部分直接求导很麻烦，可根据其物理意义求：

思路跳出这个公式，我们通常对D求X的偏导数，意思是求X如何变化，能让D的改变最大，即为D的数值的梯度方向（梯度方向即为变化最大的方向）。

由D（即点到线和点到面的距离）的物理意义可知，垂直（梯度）方向即为误差D变化最大（优化减小到0）的方向。

16.实际计算中，线面特征误差的梯度方向，皆可通过上述关联点的坐标构建向量，计算向量投影得到。

17.下来就是对雅克比分解的第二项求导，即投影点坐标对6个自由度分量分别求导。计算规则如下，整合形式如红笔所写。

后端部分：

1.每10帧点云里选一帧，与之前建好的地图做匹配，地图分为一个个cube块。在cube内做关联和更新。

2.由于之前所有帧都叠加在了一起，没有上一帧和行号的概念，就是用kdtree进行粗暴的搜索，找地图中最临近的5个点，计算点到线和点到面的残差。

3.此处的代码中，线方程使用PCA主成分分析法求，面方程默认搜索到的就是平面点，直接拟合平面方程。

4.构建误差，优化之后得到位姿，将新一帧的特征点云添加到对应的cube中去。

lego-loam对其的改进：

1.进行了地面分割，匹配用的平面特征点只在地面点中寻找。

2.对非地面点进行了dfs聚类，剔除了小于30个点的聚类，相当于去了噪点。

3.在聚类后剩余的非地面点中，提取平面和边缘点，但在匹配时，只选取边缘点。

4.在其论文中，先使用了地面点做匹配，求出了3个自由度，再代入边缘点的匹配中作为初值再计算。两端LM优化。。。但在后期的代码中，又改回了直接优化6自由度的方法（此处无法考证原因，精度？优化效率？）。

5.loam对地图使用栅格，丢失了每一帧的信息，且后端优化中无法对地图做校正。而lego-loam使用每一个关键帧特征点云单独的格式存储地图。这样做既就克服了这两个缺点，此后的升级版lio-sam也是基于此技术，

6.lego-loam的论文中没有说回环检测，但是其代码中加入了回环，方法比较粗暴，对特征直接使用gtsam优化。

7.lego-loam还加入了imu信息的可选项，实验验证，加入imu信息，定位建图的精度和鲁棒性都会好很多，但是其对imu的信息处理为松耦合，比较粗糙，不如lio-sam的紧耦合。（代码中外参设置部分甚至似乎只考虑了旋转，而没有平移）。

完。。。。。。。。。。

矩阵论知识忘了好多，得好好复习下了