LIO-SAM框架：点云匹配前戏之初值计算及局部地图构建

前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。

其中建图优化节点整体如下图

本篇主要分析： 点云匹配前戏之初值计算及局部地图构建

建图优化代码

首先来看 mapOptmization.cpp的 主函数部分

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lio_sam");
1
2
3

节点初始化

mapOptimization MO;
1

地图优化对象
其中点云匹配及因子图优化的内容都在这个类里面

    std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
1
2

定义两个线程
回环检测线程
可视化线程 rviz可视化接口的发布

下面来看mapOptimization的构造函数

    mapOptimization()
    {
1
2

        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1;
        isam = new ISAM2(parameters);
1
2
3
4

初始化参数

        pubKeyPoses                 = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
        pubLaserCloudSurround       = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
        pubLaserOdometryGlobal      = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
        pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1);
        pubPath                     = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
1
2
3
4
5

定义发布的句柄

        subCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
        subGPS   = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
        subLoop  = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());
1
2
3

定义订阅消息的句柄

• cloudinfo 包括 角点、面点、初值等
• gps
• 回环

        downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
        downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
1
2
3
4

设置各体素滤波的尺寸大小

allocateMemory();
1

预先分配内存

下面来看 laserCloudInfoHandler 这个点云回调函数，主要内容都在这个函数中

    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
    {
1
2

        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec();
1
2

提取当前时间戳

        cloudInfo = *msgIn;
        pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);
        pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
1
2
3

提取cloudinfo中的角点和面点

static double timeLastProcessing = -1;
1

上次处理的时间 初始化为-1 第一帧肯定进不去

        if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
        {
1
2

控制后端频率，两帧处理一帧
mappingProcessInterval 在配置文件（params.yaml）中是0.15s 一帧是0.1s 这个可以根据算力进行调整

timeLastProcessing = timeLaserInfoCur;
1

更新上次的处理时间

            updateInitialGuess();
1

更新当前匹配结果的初始位姿

点云匹配前戏之初值计算

终于到了本节内容的地方

作为基于优化方案的点云匹配，初始值是非常重要的，一个好的初始值会帮助优化问题快速收敛且避免局部最优解的情况

点云匹配前的初值计算 updateInitialGuess() 这个函数
进到内部

    void updateInitialGuess()
    {
1
2

incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
1

transformTobeMapped 是上一帧优化后的最佳位姿 是一个 6维数组 [x y z roll pitch yaw] 该框架下是用欧拉角来描述姿态的 最好用四元数

        if (cloudKeyPoses3D->points.empty())
        {
1
2

没有关键帧，也就是系统刚刚初始化完成 cloudKeyPoses3D是存的xyz 关键帧的位置

            transformTobeMapped[0] = cloudInfo.imuRollInit;
            transformTobeMapped[1] = cloudInfo.imuPitchInit;
            transformTobeMapped[2] = cloudInfo.imuYawInit;
1
2
3

初始的位姿由磁力计提供

            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;
1
2

这里虽然有磁力计将yaw对齐，但是也可以考虑不使用yaw

 lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); 
1

保存磁力计得到的位姿，平移置0

        if (cloudInfo.odomAvailable == true)
        {
1
2

如果有预积分节点提供的里程计

            Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX,    cloudInfo.initialGuessY,     cloudInfo.initialGuessZ, 
                                                               cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
1
2

将提供的初值转成eigen的数据结构保存下来

            if (lastImuPreTransAvailable == false)
            {
                lastImuPreTransformation = transBack;
                lastImuPreTransAvailable = true;
            }
1
2
3
4
5

这个标志位表示到第一帧预积分里程计信息
将当前里程计结构记录下来
收到第一个里程计数据以后这个标志位就是true

else {
                Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
1
2

计算上一个里程计的结果和当前里程计结果之间的 delta pose

Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
1

把上一帧的最优位姿估计 转成eigen的格式

Eigen::Affine3f transFinal = transTobe * transIncre;
1

将这个增量加到上一帧最佳位姿上去，就是当前帧位姿的一个先验估计位姿

                pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
1
2

将eigen变量转成欧垃角和平移形式 此时transformTobeMapped变成了当前帧的 先验 位姿估计，之前是上一帧的

lastImuPreTransformation = transBack;
1

同理，把当前帧的值保存下来

lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
1

虽然有里程计信息，仍然需要把imu磁力计得到的旋转记录下来

 return;
1

注意这里有return 如果有里程计信息，则不用下面的imu信息了

        if (cloudInfo.imuAvailable == true)
        {
1
2

如果没有里程计信息 ， 就是 用 imu 的旋转信息来更新，因为单纯使用imu无法得到靠谱的平移信息，因此，平移直接置0

Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit)
1

当前帧的 姿态角

Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
1

当前帧和上一帧的姿态角的变化量

Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
1

把上一帧的最优位姿估计 转成eigen的格式

Eigen::Affine3f transFinal = transTobe * transIncre;
1

将这个增量加到上一帧最佳位姿上去，就是当前帧位姿的一个先验估计位姿

            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
1
2

将eigen变量转成欧垃角和平移形式 此时transformTobeMapped变成了当前帧的 先验 位姿估计，之前是上一帧的

lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); 
return;
1
2

更新上一次的 imu 姿态

总结

点云匹配之前戏局部地图构建

上一节解读完初值计算后，回到 laserCloudInfoHandler 函数中
则到了这个函数

extractSurroundingKeyFrames();
1

提取当前帧相关的关键帧并且构建点云局部地图

    void extractSurroundingKeyFrames()
    {
        if (cloudKeyPoses3D->points.empty() == true)
            return;
1
2
3
4

如果当前没有关键帧 ，就直接return 了

 extractNearby();
1

有关键帧则进行局部地图构造

kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); 
1

cloudKeyPoses3D 存储 关键帧的3d信息 lidar是360的，所以没保存姿态
以关键帧位置形成的点云建立kd-tree

 kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
1

根据最后一个KF的位姿，进行最近邻搜索，半径是50m，在param中设置，提取一定距离内的关键帧

        for (int i = 0; i < (int)pointSearchInd.size(); ++i)
        {
            int id = pointSearchInd[i];
            surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
        }
1
2
3
4
5

根据查询的结果，把这些点的位置存进一个点云结构中

        downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
1
2

避免关键帧过多，因此做一个下采样

        for(auto& pt : surroundingKeyPosesDS->points)
        {
            kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
            pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
        }
1
2
3
4
5

确认每个下采样后的点的索引

        int numPoses = cloudKeyPoses3D->size();
        for (int i = numPoses-1; i >= 0; --i)
        {
            if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
                surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
            else
                break;
        }
1
2
3
4
5
6
7
8

刚刚是提取了一些空间上比较近的关键帧，然后再提取一些时间上比较近的关键帧
最近十秒的关键帧也保存下来

extractCloud(surroundingKeyPosesDS);
1

根据筛选出来的关键帧进行局部地图构建，来看函数内部具体内容

   void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
    {
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear(); 
1
2
3
4

分别存储角点和面点的局部地图清空

        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {
1
2

循环处理每个点

            if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
                continue;
1
2

校验一下关键帧距离不能太远

int thisKeyInd = (int)cloudToExtract->points[i].intensity;
1

取出提出来的关键帧的索引

            if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end()) 
            {
                *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
                *laserCloudSurfFromMap   += laserCloudMapContainer[thisKeyInd].second;
            }
1
2
3
4
5

如果这个关键帧对应的点云信息已经存储在一个地图容器里
直接从容器中取出来加到局部地图中

 else {
                pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
                pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
1
2
3

如果这个点云没有实现存储，那就通过该帧对应的位姿，把该帧点云从当前帧的位姿转到世界坐标系下

                *laserCloudCornerFromMap += laserCloudCornerTemp;
                *laserCloudSurfFromMap   += laserCloudSurfTemp;
1
2

点云转换之后加到局部地图中

laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
1

把转换后的面点和角点存进这个容器中，方便后续直接加入点云地图，避免点云转换的操作，节约时间

        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
        laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
1
2
3
4
5
6

将提取的关键帧的点云转到世界坐标系下后，避免点云过度密集，因此对面点和角点的局部地图做一个采样的过程

        if (laserCloudMapContainer.size() > 1000)
            laserCloudMapContainer.clear();
1
2

如果这个局部地图容器过大，就clear一下，避免占用内存过大

总结