[论文阅读]MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square

mulls文章中提到的主要贡献：

 （1）在各种场景中可以达到低漂移、实时，在kitti数据集中排第10.

 （2） 提出了一种点到线、点到面误差测度的mulls-ICP算法，利用了ground, facade, pillar, beam四种特征点。

mulls-SLAM流程图:

A. 运动补偿

     插值旋转平移对点进行补偿。

B. 几何特征点提取和编码

    1)双阈值地面点过滤

        补偿后的点投影到参考平面，参考平面是水平面或上一帧的地面点拟合得到的平面，对于非水平条件下采集数据的激光雷达数据，需要知道一个初始方向，在得到初始方向后，把点投影在该初始方向所确定的平面上，对该平面进行网格划分，分别记录每一个网格和它3x3邻域网格中的最小高度点的高度，分别为hmin和hneighmin,确定两个阈值，根据公式来确定网格中每一个点是否是地面点。

然后在网格中采用RANSAC平面拟合，内点划分为地面点G，他们的法向量是该拟合平面的法向量。

1.2 基于PCA的非地面点分类

非地面点首先进行降采样，然后投入到PCA分解中，获得C：

对C进行特征值和特征向量分解，并构成线性度σ1D, 平面度σ2D, 曲率σc。

根据这三个指标，F立面，R屋顶，P柱，B BEAM， 顶点 这五类点可以被提取到。

P,B是线性点，F R是面点，V点特征。

3）邻域上下文特征编码

    

        N为总的点个数，Imax为点强度最大值，hmax为地面点最大高度。

C.多测度线性最小二乘

    点到相关点距离表示为:

构建的最小二乘优化方程为:

(1)权重计算

其中，k在论文中一直取1。

||表示相关点的数量，|F|表示相关里面点数量。

   表示相关点强度权重。

（2）误差方程构建

    

RP+T对(alpha,beta,gama)三个角度求导为P的反对称矩阵，对T求导为单位矩阵。

ni(RP+T)对(alpha,beta,gama)三个角度求导为P的反对称矩阵乘上ni，也就是P叉乘ni，对T的求导为ni。

vk X (RP+T - q点)，对T求导为vk的反对称矩阵，对R的求导为vk的反对称乘上 P的反对称，就等于Ak(po-li）的右项式中的第二部分。

 法方程系数构建：

加权最小二乘参数求解：

残差的距离加权平方作为配准质量评估的指标:

D. mulls前端

    scan-to-scan 帧到上一帧的配准。

   scan-to-map 使用scan-to-scan得到的配准结果作为初值，使用Mulls-ICP再次做scan-to-map的配准。

   在激光雷达一定距离内的非地面点，如果原理它的同类别的最邻近点，则过滤掉该簇非地面点。

   然后裁剪出来以当前位置为中心一定半径内的地图作为局部地图，地图中稠密的特征点组作被用来做下一帧的配准。

E. 后端

    构建子图，子图闭环通过TEASER来构建和验证，TEASER是一种全局的配准方法，初始的子图相关是用NCC对所构建的f特征来做的相关性匹配的，通过子图-子图间的配准来计算闭环的相对位置和姿态，高sigma和比阈值小的Ots的边将被删除，如果一个闭环边被增加，就做PGO全局优化，优化每一个子图的一个位姿，然后根据每一个子图的位姿调整子图内每一帧的位姿。

论文比较容易看懂，后续把代码注释一下，贴到CSDN中

参考了Mulls文献

[1]Pan Y, Xiao P, He Y, et al. MULLS: Versatile LiDAR SLAM via multi-metric linear least square[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021: 11633-11640.

[2].https://www.cnblogs.com/long5683/p/15420488.html