前言

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

这个方法的核心思想就是把SLAM问题进行了拆分,通过两个算法来进行.一个是执行高频率的前端里程计但是低精度的运动估计（定位）,另一个算法在比定位低一个数量级的频率执行后端建图（建图和校正里程计）.
这个两个算法都需要提特征点,就是经典的角点和面点,然后进行配准.在前端的那个算法中也就是里程计算法,特征点的提取会用到快速计算的方法.在建图的后端算法中,相互关联的特征点是通过特征值和特征向量来获得的.

前几篇博客分析了ALOAM的前端链接如下:
ALOAM:帧间里程计代码解读
ALOAM:Ceres 优化部分及代码解析
ALOAM:激光雷达的运动畸变补偿代码解析
ALOAM:异常点剔除机制代码解析
A-LOAM :前端lidar点特征提取部分代码解读
A-LOAM :前端lidar点预处理部分代码解读

剩下的就是ALOAM的后端了!本篇主要分析后端laserMapping的代码结构与数据处理

ALOAM 后端laserMapping

建图算法 运行 的频率要比里程计低
每一帧 执行 一次

在k+1帧的最后,激光里程计得到了经过畸变校正的点云 ^Pk+1,并且同时得到了 一个位姿变换 Tk+1.
建图算法就是将 ^Pk+1 配准到世界坐标系中.

Qk 是 已存在地图中的点云 是前k帧的累积
Tw 是 地图中 最后一帧 k 的位姿 在tk+1时刻

用来自激光里程计的输出,建图算法 将 ^Pk+1 投影到世界坐标系中,形成 ^Qk+1

然后就是配准 ^Qk+1 和Qk, 这个过程会优化lidar的位姿 Twk+1
在这里插入图片描述
订阅四种消息

  • 当前帧全部点云(经过一次降采样)
  • 上一帧的边线点集合
  • 上一帧的平面点集合
  • 当前帧的位姿粗估计

发布六种消息

  • 附近5帧组成的降采样子地图 for rviz
  • 所有帧组成的点云地图
  • 经过Map to Map精估计优化后当前帧位姿精估计
  • 当前帧原始点云（从velodyne_cloud_3订阅来的点云未经其他处理）
  • 里程计坐标系位姿转化到世界坐标系位姿（地图坐标系），mapping输出的1Hz位姿，odometry输出的10Hz位姿，整合成10hz作为最终结果
  • 经过Map to Map 精估计优化后的当前帧平移

代码解析

ALOAM的后端的模块是laserMapping .代码就是在laserMapping.cpp里面

laserMapping.cpp也是一个ROS的node.所以可以从main函数看起

int main(int argc, char **argv)
{
    ros::init(argc, argv, "laserMapping");
    ros::NodeHandle nh;

ros的基本操作

    //配置线特征和面特征点云的分辨率,之后通过体素滤波进行下采样
    float lineRes = 0;//声明线特征分辨率
    float planeRes = 0;//声明面特征分辨率
    nh.param<float>("mapping_line_resolution", lineRes, 0.4);//从参数服务器读取线特征分辨率
    nh.param<float>("mapping_plane_resolution", planeRes, 0.8);//从参数服务器读取面特征分辨率
    printf("line resolution %f plane resolution %f \n", lineRes, planeRes);//打印设置的分辨率
    downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);//设置线特征的分辨率
    downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);//设置面特征的分辨率

配置线特征和面特征点云的分辨率,之后通过体素滤波进行下采样

然后订阅了4种消息

    ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);

    ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);

    ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);

    ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);
  • 上一帧的角点
  • 上一帧的面点
  • 前端里程计位姿
  • 所有的点云

可以先看下各回调函数做了什么事情

void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
    mBuf.lock();//线程锁 加锁
    cornerLastBuf.push(laserCloudCornerLast2);//存入Buf
    mBuf.unlock();//线程锁 解锁
}

void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
{
    mBuf.lock();
    surfLastBuf.push(laserCloudSurfLast2);
    mBuf.unlock();
}

void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
    mBuf.lock();
    fullResBuf.push(laserCloudFullRes2);
    mBuf.unlock();
}

三个点云的回调函数处理很简单,就是存入到各自的数据Buf中
注意:回调函数要加线程锁,因为一个回调函数就是一个线程,主函数里还有一个线程,两个线程都会访问Buf信息,避免线程冲突,在存入数据前加锁,写入后解锁

然后来看里程计的回调函数做的事情

//前端激光雷达里程计回调函数
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{

    mBuf.lock();
    odometryBuf.push(laserOdometry);
    mBuf.unlock();

首先和点云一样写入Buf中

之后需要发布最新的位姿信息(这部分需要计算了).

前端里程计会定期向后端发送位姿,这个位姿是当前帧和里程计初始位姿直接的位姿(可以叫做T_odom_curr),在后端mapping模块中,需要以map坐标系为初始位姿,然后得到当前帧和map的位姿变换(可以叫做T_map_curr),如何进行这个两个位姿的转换呢?
后端mapping模块就是解决这个问题,它会估计出里程计初始位姿和map的初始位姿的位姿变换(可以叫做T_map_odom),然后
T_map_curr = T_map_odom×T_odom_curr
上面就是计算的原理
再补充一句,里程计坐标系和map坐标系在初始时刻是对齐的,然后里程计是累加转的时间坐标系下的,所以会随着时间的推移,越来越不准,通过后端的处理,map坐标系下的位姿会相对准一些
回到代码,就是的上面位姿变换的处理,收到一个odom的位姿,就发布一个map下的位姿

    Eigen::Quaterniond q_wodom_curr;
    Eigen::Vector3d t_wodom_curr;
    q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
    q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
    q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
    q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
    t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
    t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
    t_wodom_curr.z() = laserOdometry->pose.pose.position.z;

将ros的消息,转成eigen的格式

    Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
    Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;

这个就是上面的公式,将odom的位姿转成map的位姿
q_wmap_wodom和t_wmap_wodom 后端优化的odom到map的位姿变换

    nav_msgs::Odometry odomAftMapped;
    odomAftMapped.header.frame_id = "/camera_init";
    odomAftMapped.child_frame_id = "/aft_mapped";
    odomAftMapped.header.stamp = laserOdometry->header.stamp;
    odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
    odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
    odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
    odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
    odomAftMapped.pose.pose.position.x = t_w_curr.x();
    odomAftMapped.pose.pose.position.y = t_w_curr.y();
    odomAftMapped.pose.pose.position.z = t_w_curr.z();
    pubOdomAftMappedHighFrec.publish(odomAftMapped);

转成ROS的里程计信息 发布出去

继续回到main函数中之后是定义本节点发布的消息(6种)

    pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);
    pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);
    pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);
    pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);
    pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);
    pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);

定义本节点发布的5种消息

    //地图的数组进行 重置 分配地址 
    for (int i = 0; i < laserCloudNum; i++)
    {
        //后端地图的数组 用栅格的形式
        laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());
        laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());
    }

地图的数组进行 重置 分配地址

    std::thread mapping_process{process};

主线程
process是一个 while(1)的循环

void process()
{
    while(1)
    {

主循环一直运行. 后端的优化部分也就是在这主循环里一直执行.

这个就是ALOAM后端的整体代码结构