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

后端描述

• 不同于前端的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循环

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

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;
}

第一次进入这里时的map_odom是初始化为单位矩阵的，即默认开始odom与map坐标系重合，前端只给到 odom_curr的变换，map-odom-curr三者坐标系关系不懂的同学可以看一下我总结的这篇文章关于ROS中map、odom、base_link三个坐标系的理解

4. 在A-LOAM中的栅格地图总大小为21 * 21 * 11，局部栅格地图为10 * 10 * 5，这里的单位是栅格，一个栅格的大小为50米

int laserCloudCenWidth = 10;
int laserCloudCenHeight = 10;
int laserCloudCenDepth = 5;
const int laserCloudWidth = 21;
const int laserCloudHeight = 21;
const int laserCloudDepth = 11;

重点理解：局部栅格地图的构建

centerCubeI、centerCubeJ、centerCubeK的作用：判断当前位姿在全局栅格地图中的哪个栅格里面，分别对应x、y、z
首先看代码

int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth; 
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight; 
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth; 

• 这一部分的作用是用来判断当前位姿在整个大栅格地图中的哪个位置，即在哪个栅格中
• 这里的25，50的单位是米，后面加的3个变量在上一段代码有展示，其单位为栅格
• 这里默认开机在整个地图中间，接下来通过计算展示

假设 t_w_curr.x = 1 米 即开始计算走了1米，（1+25）/ 50 = 0.52 由于 int 去尾的操作，所以这块 = 0，0+laserCloudCenWidth =0 + 10 ; x方向在第10个栅格的位置，刚好在栅格地图中间的栅格
再假设走了 26 米时 （26+25）/ 50 = 1.02 ，int 去尾变成 1 ，1+10 = 11，这是因为25是一个栅格的中心，一个栅格的长度为50米，所以中心就为 25米，从中间开始走超过 25米 就很明显越过当前栅格了

这部分是当 robot 向前走的情况

• 向后走时栅格中心的判断
  还是先看代码

			
if (t_w_curr.x() + 25.0 < 0)
	centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
	centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
	centerCubeK--;

这是因为当走了-26米时，(-26+25) / 50 = -0.02 ，根据 int 的去尾是 = 0，所以要这样额外判断去使栅格id往后移

关于 int 去尾操作：1.01~1.99 都会变成1，-0.99 ~ -0.01 都会变成0

5. 如果当前珊格centerCubeI索引小于3,就说明当前点快接近地图边界了，需要进行调整，相当于地图整体往x正方向移动，反方向同理，y，z方向同理，操作见前面的一维描述，最后附的代码也可以看到，以上操作相当于维护了一个局部地图，保证当前帧不在这个局部地图的边缘，这样才可以从地图中获取足够的约束
6. 选取一定范围的点云，以当前格子为中心，x、y方向是各自前后取2个栅格，即x，y方向加上本身共有5个栅格，z方向上下各取1各栅格，则共有3个栅格，此时栅格大小为5 * 5 * 3，一个栅格为50米，则为250米 * 250米 * 150米 大小的小局部地图
7. 开始构建用来这一帧优化的小的局部地图，用格子索引提取出对应格子里面的角点与面点，构成一个小的局部地图

for (int i = 0; i < laserCloudValidNum; i++)
{
	*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
	*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}

8. 对当前帧的点云进行体素滤波下采样
9. 判断小的局部地图里的点云数量是否满足要求

if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)

10. 满足要求则开始提取点构建约束，具体见A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释

A-LOAM系列讲解
A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释
A-LOAM(前端-2)异常点的剔除-算法流程+代码注释
A-LOAM(前端-3)的雷达畸变及运动补偿-算法流程+代码注释
A-LOAM(前端-4)的帧间lidar里程计-算法流程+代码注释
A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释
A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释
A-LOAM总结-(前端+后端)算法流程分析
关于ROS中map、odom、base_link三个坐标系的理解