A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释

Rhys___

已于 2024-05-08 15:35:08 修改

阅读量3.4k

点赞数 37

分类专栏：激光SLAM-LOAM系列文章标签：算法 c++ 自动驾驶

于 2022-08-13 17:56:11 首次发布

本文链接：https://blog.youkuaiyun.com/zysss_/article/details/126319583

版权

A-LOAM的后端采用scan-to-map算法，通过构建和匹配栅格地图来优化位姿并构建高精度地图。文章详细介绍了地图构成，特别是如何使用21x21x11的栅格地图存储，以及在当前位姿变化时如何动态调整栅格地图。算法流程包括从ROS消息到点云转换，根据前端结果获取初始值，以及构建局部栅格地图进行位姿估计和地图更新。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

后端描述

不同于前端的scan-to-scan的过程，LOAM的后端是scan-to-map的算法，具体来说就是把当前帧和地图进⾏匹配，得到更准的位姿同时也可以构建更好的地图。由于是scan-to-map的算法，因此计算量会明显⾼于scan-to-scan的前端，所以，后端通常处于⼀个低频的运⾏频率，但是由于scan-to-map的精度往往优于scan-to-scan，因此后端也有着⽐起前端来说更⾼的精度。

地图构成

⾸先需要了解的⼀件事情就是地图的构成，地图通常是当前帧通过匹配得到在地图坐标系下的准确位姿之后拼接⽽成，如果我们保留所有的拼接的点云，此时随着时间的运⾏，内存很容易就吃不消了，因此考虑存储离当前帧⽐较近的部分地图，同时，为了便于地图更新和调整，在原始LOAM中，使⽤的是基于珊格的地图存储⽅式。具体来说，将整个地图分成21×21×11个珊格，每个珊格是⼀个边⻓50m的正⽅体，当地图逐渐累加时，珊格之外的部分就被舍弃，这样可以保证内存空间不会随着程序的运⾏⽽爆炸。

在这里插入图片描述

但是，我们注意到，如果当前位姿远离的珊格覆盖范围，则地图也就没有意义了，因此，珊格地图也需要随着当前位姿动态调整，从⽽保证我们可以从珊格地图中取出离当前位姿⽐较近的点云来进⾏scan-to-map算法，借以获得最优位姿估计。

栅格地图调整

当当前位姿即将到达地图边界的时候，当前珊格地图就必须动态调整，如下图，我们以⼀维case为例

在这里插入图片描述
当前帧即将抵达地图的左边界时，我们需要把当前帧和地图整体右移⼀部分，保证当前帧处于⼀个安全
的位置

这样相对移动之前，当前帧就处在⼀个“安全的位置”，然后左边会空出来⼀个珊格。

算法流程

按照处理步骤来一点点描述
在 "laserMapping.cpp“的void process() 线程函数中运行，这是一个while循环

确保的buffer里都有数值，分别为cornerLastBuf、surfLastBuf、fullResBuf、odometryBuf ，以cornerLastBuf为基准，把时间戳小于其的全部pop出去，并判断所有数据的最前的时间戳是否一致，原则上来说都是一样的
将ROS消息格式转成pcl，雷达的odom结果转成eigen数据格式，把转换后还有剩的cornerLastBuf都pop出去，这是考虑到实时性问题
根据前端结果得到一个后端的初始值

void transformAssociateToMap()
{
   
	// T_w_curr = T_w_last * T_last_curr(from lidar odom)
	q_w_curr = q_wmap_wodom * q_wodom_curr;
	t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom