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.