目录
1、框架
loam框架核心是两部分,高频率的里程计和低频率的建图。两种算法处理。里程计通过高频率低数量的点云,进行scan-scan匹配,估算出两帧之间的运动关系,输出结果给mapping算法;建图是1hz的频率将去畸变的点云匹配并对齐到地图上,应用的是scan-map的匹配方式。最后将两种算法发布的姿态变换集成,得到激光雷达姿态对于地图约10hz的变换输出。
PS:主要结合论文和一些知乎博客总结了LOAM 算法中核心关键点。
2、特征点提取
核心是在Pk点云中提取两种特征点,分别为角点与面点
计算曲率的公式为:
j就是i周围的点
曲率 = (当前点到其附近点的距离差 / 当前点的值)的总和再求平均 = 平均的距离差
角点与面点选取的依据
扫描中的点根据c 值进行排序,然后选择曲率大的为角点,曲率c小的为面点。论文中为了在环境中均匀分布特征点,将扫描分为四个相同的子区域,如果是360度,也就是80度一个区域,每个子区域最多提取2个角点,4个面点。仅当点的曲率大于或小于阈值且所选点的数量不超过最大值,才将该点选为特征点。
同时还考虑了匹配会出问题的两种情况。一种避免选择周围点已经被选择的点,或局部平面上与激光束大致平行的点,下图的a。另一种避免位于遮挡区域边界上的点,例如下图b,因为连接表面被另外一个对象遮挡,激光运动过程中旋转了视角,有可能就看不到A了。
因此选择特征点时需要满足条件:
-
特征点数量满足子区域最大值,每个子区域的特征点数目是一定的。
-
特征点周围点不能再次被选择,本来每个子区域的特征点数量就比较少了,进一步防止特征点集中分布。
-
特征点非平行激光束的平面点,非被遮挡的平面点,非离群点。这些都是不可靠的点云数据,不能在连续两帧点云数据中得到,不能作为特征点。
3、点云去畸变
其中通过激光角度关系对应tk--tk+1中占比来估计点的运动。
4、帧间匹配
特征关联与损失函数计算
a 线特征
点到线的距离
换一种画图方法
也就是在K中找到找到距离i最近的点j。在同一帧扫描的上下不同线上找一个最近点l。从而计算i到 jl 所在直线的距离。
公式2的直观解释可以通过下图展示,简单理解就是平行四边形的高等于面积/底边
b 面特征
点到面的距离
另一个角度
点到面的匹配,就是在k中找到最近特征点j,然后在同一束激光中找到最近的点l,同时在同一时刻的上下束激光中找到最近距离的m 点,j l m三个点组成平面,然后计算点到面的距离
将公式2 和公式3 放在一个模型中,得到总的代价函数为
5、运动估计
为了获得这一帧数据中的点与上一帧数据中点的对应关系,利用一个旋转矩阵R和平移两T表示。
这里需要把时间点对齐,才能求解上面的点到线和点到面的距离d!
欧拉角求导非常复杂,所以用罗德里杰斯公式将旋转矩阵R展开:
其中 theta为
至此我们获得了点到线和面的距离,并获得用于优化的误差函数:
论文是使用LM 非线性优化方法求解
代码中实现方法是通过求欧拉角的方式计算的
具体推导如下:
激光里程计 算法流程
6、建图
累计一定帧 数目的点云数据才会进行建图,频率较低,但是精度很高。目的是将点云数据融入到世界地图中,精确估计激光在世界坐标系的位姿。与里程计相比,这里构建边缘线,平面使用的点数大大增加,因此采用其他方法得到线向量和法向量。论文作者使用了协方差矩阵。
注意:上图说使用的map to map 的方法是不对的,实际上是scan to map
7、姿态融合
这一部主要是将Lidar Odometry中得到的姿态信息和Lidar Mapping中得到的信息全部都放入到rviz中,方便观看和处理。如果是为了使用LOAM作为前端的话,到Lidar Mapping就完全够用了。
8、LOAM 优劣势
LOAM的优点:
1、将里程计和建图分隔开,一个高频低精(处理每次帧数据),一个低频高精(累积一定次数) 整理框架是串行结构,将整个问题逐步划分为多个层次。
2、运动补偿(时间戳)
3、融合了scan-scan scan-map
4、可实时建图的开源3D Lidar SLAM
LOAM的缺点:
1、点云特征处理可进一步改善:运动的人等物体 无回环检测 假设匀速运动模型
2、不能处理大规模的旋转变换(旋转向量的求解)
9、参考连接
LOAM: Lidar Odometry and Mapping in Real-time_林北不要忍了的博客-CSDN博客
loam论文翻译_每天都在努力学习SLAM的小黑的博客-CSDN博客_loam论文翻译