前言
ALOAM方法实现了低的漂移,并且计算的复杂度低,实时性很好.并且不需要高精度的lidar和惯导
这个方法的核心思想就是把SLAM问题进行了拆分,通过两个算法来进行.一个是执行高频率的前端里程计但是低精度的运动估计（定位）,另一个算法在比定位低一个数量级的频率执行后端建图（建图和校正里程计）.
那么其中需要处理的一个问题就是 低运算频率的后端,如何保障数据处理的实时性的
ALOAM的做法就是去掉过多的数据,仅处理能处理的数据.这样虽然有可能丢掉一部分数据,但是保障了后端的低延时和计算内存.
当运行ALOAM的时候你的终端,打印出里如下信息:
drop lidar frame in mapping for real time performance
这就意味着,后端的数据处理不过来了,为了保证后端的实时性,将会删掉前面的数据.
本篇博客主要介绍这部分内容在代码中是如何实现的,即如何保证后端的数据处理的低延时性.
代码解析
后端的处理主要在一个主线程 process函数中,其中是一个while的循环
void process()
{
while(1)
{ while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{首先判断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;
}以 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();取出处理后的数据的 最前面一个数据的 时间戳
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;
}判断时间是否相等,不等则会打印有问题的时间和打印同步失败,并且跳出本次循环
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中的本次处理的数据清空上次操作的上帧点云中的点 把本次要处理的数据转成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中的本次处理的数据取出本次处理数据的里程计信息,转成eigen的形式 当前帧在odom坐标系下的位姿
//考虑到实时性,就把队列里其它的都pop出去,不然可能出现延时的情况
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}考虑到实时性,就把队列里其它的都pop出去,不然可能出现延时的情况
计算资源也不会过载
比如 前端1s,可以出5帧数据,后端1s可以处理1帧数据,如果不像上面操作的话,
那么 Buf里的数据会随时间增长一直增加.直到计算资源过载.
并且会出现后端结果延时的情况,运行了5秒之后,后端,还在处理第一秒的数据,25秒之后,后端在处理第5s的数据.这种延时情况随时间增长也会越来越严重.
在ALOAM中,每次把Buf中的对应一帧的四组数据取出来后,则把所有的buf清空.则会丢掉很多帧的数据.但是计算资源和实时性可以得到保证.
测试
前端向后端发布的数据有
- 上一帧的角点
- 上一帧的面点
- 前端里程计位姿
- 所有的点云
通过终端查看下上一帧的角点发布的频率
rostopic hz /laser_cloud_corner_last
大概是10hz
后端的处理时间要根据场景和处理器的性能有关系了
由于在台式机上测试的,后端处理的比较快,电脑配置如下:
在运行ALOAM的时候,终端会打印出后端的处理时间:
whole mapping time在七八毫秒左右.完全可以处理10hz的数据
所以不会出现处理过多数据的情况,也就打印不出
drop lidar frame in mapping for real time performance
为了测试上面的情况,可以在后端中加个1s的延时,
ros::Duration(1).sleep();//人工加个延时.则会出现下面的情况
连续打印了10次.drop lidar
因为前端发布的是10hz的数据,那么在延时1s的时间内,buf里面则出现了10帧处理不过来的数据
pop掉每秒的10帧数据,则连续打印10次drop lidar
