LIO-SAM后端中初值计算及局部地图构建

这个是 mapOptmization.cpp 文件 ，整个 地图构建处理和 回环和 地图匹配求解位姿都在这里，其主要订阅的话题为 “lio_sam/feature/cloud_info” ，为了融合GPS 和 回环还订阅了 gpsTopic 、“lio_loop/loop_closure_detection”

这里主要讲解订阅 “lio_sam/feature/cloud_info” 话题的回调函数 laserCloudInfoHandler

下面详解的两个函数主要作用 是更新当前匹配结果的初始位姿 和 提取当前帧相关的关键帧并且构建点云局部地图

处理流程代码

    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
    {
   
        // extract time stamp
        // 提取当前时间戳
        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec();

        // extract info and feature cloud
        // 提取cloudinfo中的角点和面点
        cloudInfo = *msgIn;
        pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);
        pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);

        std::lock_guard<std::mutex> lock(mtx);

        static double timeLastProcessing = -1;
        // 控制后端频率，两帧处理一帧
        if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
        {
   
            timeLastProcessing = timeLaserInfoCur;
            // 更新当前匹配结果的初始位姿
            updateInitialGuess();
            // 提取当前帧相关的关键帧并且构建点云局部地图
            extractSurroundingKeyFrames();
            // 对当前帧进行下采样
            downsampleCurrentScan();
            // 对点云配准进行优化问题构建求解
            scan2MapOptimization();
            // 根据配准结果确定是否是关键帧
            saveKeyFramesAndFactor();
            // 调整全局轨迹
            correctPoses();
            // 将lidar里程记信息发送出去
            publishOdometry();
            // 发送可视化点云信息
            publishFrames();
        }
    }

updateInitialGuess()

• 函数作用：更新当前匹配结果的初始位姿

1. 将上一帧优化后的位姿转换成 旋转+平移 的存储方式 transformTobeMapped原本为 [roll pitch yaw x y z ]，第一次进来的时候这里都为 0

incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);

2. 第一次进来时，初始位姿由磁力计提供，但只会保存pitch和roll，x，y，z，row为不可观测量，无论在VIO还是LIO中都一样，将磁力计提供的位姿存到transformTobeMapped中作为初始值，和lastImuTransformation中（这个变量暂时还没看到用处）

        // 没有关键帧，也就是系统刚刚初始化完成
        if (cloudKeyPoses3D->points.empty())
        {
   
            // 初始的位姿就由磁力计提供
            transformTobeMapped[0] = cloudInfo.imuRollInit;
            transformTobeMapped[1] = cloudInfo.imuPitchInit;
            transformTobeMapped[2] = cloudInfo.imuYawInit;
            // 无论vio还是lio 系统的不可观都是4自由度，平移+yaw角，这里虽然有磁力计将yaw对齐，但是也可以考虑不使用yaw
            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;
            // 保存磁力计得到的位姿，平移置0
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;