前面分析的ALOAM相关的链接如下:
ALOAM:后端laserMapping代码结构与数据处理分析
ALOAM:帧间里程计代码解读
ALOAM:Ceres 优化部分及代码解析
ALOAM:激光雷达的运动畸变补偿代码解析
ALOAM:异常点剔除机制代码解析
A-LOAM :前端lidar点特征提取部分代码解读
A-LOAM :前端lidar点预处理部分代码解读

前言

ALOAM方法实现了低的漂移,并且计算的复杂度低,实时性很好.并且不需要高精度的lidar和惯导

这个方法的核心思想就是把SLAM问题进行了拆分,通过两个算法来进行.一个是执行高频率的前端里程计但是低精度的运动估计（定位）,另一个算法在比定位低一个数量级的频率执行后端建图（建图和校正里程计）.

那么其中需要处理的一个问题就是 低运算频率的后端,如何保障数据处理的实时性的

ALOAM的做法就是去掉过多的数据,仅处理能处理的数据.这样虽然有可能丢掉一部分数据,但是保障了后端的低延时和计算内存.

当运行ALOAM的时候你的终端,打印出里如下信息:

drop lidar frame in mapping for real time performance

这就意味着,后端的数据处理不过来了,为了保证后端的实时性,将会删掉前面的数据.

本篇博客主要介绍这部分内容在代码中是如何实现的,即如何保证后端的数据处理的低延时性.

代码解析

后端的处理主要在一个主线程 process函数中,其中是一个while的循环

void process()
{
	while(1)
	{
1
2
3
4

		while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
			!fullResBuf.empty() && !odometryBuf.empty())
		{
1
2
3

首先判断4个buf里数据是非空的,然后再进入整体循环.

			// 处理odom的数据
			while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
				odometryBuf.pop();//最前面的数据时间戳小于基准时间则pop
			if (odometryBuf.empty())//pop后为空了
			{
				mBuf.unlock();//线程锁放开,可以继续存数据
				break;//跳出本次循环
			}
			// 处理surfLast的数据
			while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
				surfLastBuf.pop();
			if (surfLastBuf.empty())
			{
				mBuf.unlock();
				break;
			}
			// 处理fullResBuf的数据
			while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
				fullResBuf.pop();
			if (fullResBuf.empty())
			{
				mBuf.unlock();
				break;
			}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

以 cornerLastBuf 的时间戳为基准,把时间小于它的pop出去
对于三个数据的操作类似

			/*取出处理后的数据的 最前面一个数据的 时间戳*/
			timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
			timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
			timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
			timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
1
2
3
4
5

取出处理后的数据的 最前面一个数据的 时间戳

			if (timeLaserCloudCornerLast != timeLaserOdometry ||
				timeLaserCloudSurfLast != timeLaserOdometry ||
				timeLaserCloudFullRes != timeLaserOdometry)
			{
				printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
				printf("unsync messeage!");
				mBuf.unlock();
				break;
			}
1
2
3
4
5
6
7
8
9

判断时间是否相等,不等则会打印有问题的时间和打印同步失败,并且跳出本次循环

			laserCloudCornerLast->clear();
			pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);//把本次要处理的数据转成PCL的格式
			cornerLastBuf.pop();//pop掉buf中的本次处理的数据

			laserCloudSurfLast->clear();
			pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);//把本次要处理的数据转成PCL的格式
			surfLastBuf.pop();//pop掉buf中的本次处理的数据

			laserCloudFullRes->clear();
			pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);//把本次要处理的数据转成PCL的格式
			fullResBuf.pop();//pop掉buf中的本次处理的数据
1
2
3
4
5
6
7
8
9
10
11

清空上次操作的上帧点云中的点 把本次要处理的数据转成PCL的格式
op掉buf中的本次处理的数据

			/*取出本次处理数据的里程计信息,转成eigen的形式  当前帧在odom坐标系下的位姿 */
			q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
			q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
			q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
			q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
			t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
			t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
			t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
			odometryBuf.pop();//pop掉buf中的本次处理的数据
1
2
3
4
5
6
7
8
9

取出本次处理数据的里程计信息,转成eigen的形式 当前帧在odom坐标系下的位姿

			//考虑到实时性,就把队列里其它的都pop出去,不然可能出现延时的情况
			while(!cornerLastBuf.empty())
			{
				cornerLastBuf.pop();
				printf("drop lidar frame in mapping for real time performance \n");
			}
1
2
3
4
5
6

考虑到实时性,就把队列里其它的都pop出去,不然可能出现延时的情况
计算资源也不会过载

比如 前端1s,可以出5帧数据,后端1s可以处理1帧数据,如果不像上面操作的话,

那么 Buf里的数据会随时间增长一直增加.直到计算资源过载.
并且会出现后端结果延时的情况,运行了5秒之后,后端,还在处理第一秒的数据,25秒之后,后端在处理第5s的数据.这种延时情况随时间增长也会越来越严重.

在ALOAM中,每次把Buf中的对应一帧的四组数据取出来后,则把所有的buf清空.则会丢掉很多帧的数据.但是计算资源和实时性可以得到保证.

测试

前端向后端发布的数据有

• 上一帧的角点
• 上一帧的面点
• 前端里程计位姿
• 所有的点云

通过终端查看下上一帧的角点发布的频率

rostopic hz /laser_cloud_corner_last
1

大概是10hz

后端的处理时间要根据场景和处理器的性能有关系了

由于在台式机上测试的,后端处理的比较快,电脑配置如下:

在运行ALOAM的时候,终端会打印出后端的处理时间:

whole mapping time在七八毫秒左右.完全可以处理10hz的数据
所以不会出现处理过多数据的情况,也就打印不出
drop lidar frame in mapping for real time performance

为了测试上面的情况,可以在后端中加个1s的延时,

ros::Duration(1).sleep();//人工加个延时.
1

则会出现下面的情况

连续打印了10次.drop lidar

因为前端发布的是10hz的数据,那么在延时1s的时间内,buf里面则出现了10帧处理不过来的数据
pop掉每秒的10帧数据,则连续打印10次drop lidar