前面分析的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)
    {

        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