ROS-3DSLAM（七）lvi-sam源代码阅读5

2021SC@SDUSC

（七）lvi-sam源代码阅读5

写在前面 团队进展与计划

a. 进展

​ 首先，我们团队分析的是LVI-SAM两大模块中的lidar模块。如今，我们已经完成了lidar模块中/lvi_sam_imuPreintegration(IMU预积分节点)，/lvi_sam_featureExtraction(激光雷达特征点提取节点) ，/lvi_sam_imageProjection(生成深度图)，总共3个节点的分析。从代码开始，通过代码的阅读，不断补充相关的知识点，从ROS的语法到代码运用到的数学原理，我们都有所收获。

b. 计划

​ 接下来，我们团队，将继续分析lidar模块中的/lvi_sam_mapOptmization(因子图优化节点)，把lidar模块中的最后一个节点分析完，然后会串联lidar模块中的各个节点，并且和LIO-SAM进行比较，分析LVI-SAM中的优化的内容。最后，visual模块尝试进行分析，但不能保证在这学期结束能分析完。在其中一个队友继续分析/lvi_sam_mapOptmization的时候，其他队友会继续分析visual模块，等待完成/lvi_sam_mapOptmization的分析。

lidar_odometry

mapOptmization.cpp

在构造函数中，主要订阅了三个话题；下面主要分析这三个话题的回调函数：

//订阅当前激光帧点云信息，来自featureExtraction
subLaserCloudInfo = nh.subscribe<lvi_sam::cloud_info>(PROJECT_NAME + "/lidar/feature/cloud_info", 5, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
//订阅GPS里程计
subGPS = nh.subscribe<nav_msgs::Odometry>(gpsTopic,50, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
//订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上
subLoopInfo = nh.subscribe<std_msgs::Float64MultiArray>(PROJECT_NAME + "/vins/loop/match_frame", 5, &mapOptimization::loopHandler, this, ros::TransportHints().tcpNoDelay());

laserCloudInfoHandler

订阅激光帧点云信息

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

    // extract info ana feature cloud 提取当前激光帧角点、平面点集合
    cloudInfo = *msgIn;
    //pcl::PointCloud<PointType>::Ptr
    pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);
    pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);

    // 设置锁（同步）
    std::lock_guard<std::mutex> lock(mtx);

    //静态变量，第一次声明后就会一直存在
    static double timeLastProcessing = -1;
    //mapping执行频率控制
    // 如果 当前激光帧 与 上一帧激光帧之间的差值 大于 某个值（暂时认为是执行时间间隔要大于这个时间）
    //mappingProcessInterval 在utility.h中被初始化为0.15 or PROJECT_NAME + "/mappingProcessInterval"
    if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval) {

        //更新时间戳
        timeLastProcessing = timeLaserInfoCur;

        //当前帧位姿初始化
        //1.如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
        //2.后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
        updateInitialGuess();

        //提取局部角点、平面点云集合，加入局部地图
        //1.对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
        //2.对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部地图中
        extractSurroundingKeyFrames();

        /*
        将一组类似的激光雷达位姿 获得的点云图 进行匹配
        */

        //当前激光帧角点、平面点集合降采样
        downsampleCurrentScan();

        // 1、要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化。
        // 2、迭代30次（上限）优化。
        // 1) 当前激光帧角点寻找局部map匹配点
        // a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了；
        // b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
        // 2) 当前激光帧平面点寻找局部map匹配点
        // a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
        // b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
        // 3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
        // 4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
        // 3、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标。
        scan2MapOptimization();//最长（最坏情况下看到这个


        //因子图优化，更新所有关键帧位姿
        /**
         * 设置当前帧为关键帧并执行因子图优化
         * 1.计算当前帧与前一帧位姿变化，如果变化太小，不设为关键帧，反之设为关键帧
         * 2.添加激光里程计因子、GPS因子、闭环因子
         * 3.执行因子图优化
         * 4.得到当前帧优化后位姿，位姿协方差
         * 5.添加cloudKeyPoses3D,cloudKeyPoses6D,更新transformToboMapped,添加当前关键帧的角点、平面点集合
         * 
         * 该优化是独立于scan-map的另一个优化
         */
        saveKeyFramesAndFactor();

        //更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
        correctPoses();

        //发布激光里程计
        publishOdometry();

        //发布里程计、点云、轨迹
        //1.发布历史关键帧位姿集合
        //2.发布局地图降采样平面点集合
        //3.发布历史帧（累加的）的角点、平面点降采样集合
        //4.发布里程计轨迹
        publishFrames();
    }
}

1.updateInitialGuess()

当前帧的位姿初始化

void updateInitialGuess()
{        
    // 前一帧的位姿，这里指 lidar 的位姿 
    //Affine3f 仿射变换矩阵（移向量+旋转变换组合而成，可以同时实现旋转，缩放，平移等空间变换。）  
    //https://blog.csdn.net/weixin_42503785/article/details/112552689
    static Eigen::Affine3f lastImuTransformation;
    // system initialization
    // 如果 关键帧集合 为空，继续进行初始化
    if (cloudKeyPoses3D->points.empty())
    {
        //大小为6的数组
        //当前帧位姿的旋转部分，用激光帧信息中的RPY（来自IMU原始数据）初始化
        transformTobeMapped[0] = cloudInfo.imuRollInit;
        transformTobeMapped[1] = cloudInfo.imuPitchInit;
        transformTobeMapped[2] = cloudInfo.imuYawInit;

        //在utility.h中定义
        if (!useImuHeadingInitialization)
            transformTobeMapped[2] = 0;
        //应该是坐标 + 旋转角？？？
        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
        return;
    }

    // use VINS odometry estimation for pose guess
    // 用当前帧和前一帧对应的imu里程计计算相对位姿变换，再用前一帧的位姿与相对变换，计算当前帧的位姿，存 transformTobeMapped
    static int odomResetId = 0;
    static bool lastVinsTransAvailable = false;
    static Eigen::Affine3f lastVinsTransformation;
    //imageProjection.cpp： cloudInfo.odomResetId = (int)round(startOdomMsg.pose.covariance[0]);
    //里程计
    if (cloudInfo.odomAvailable == true && cloudInfo.odomResetId == odomResetId)
    {
        // ROS_INFO("Using VINS initial guess");
        if (lastVinsTransAvailable == false)
        {
            // ROS_INFO("Initializing VINS initial guess");
            // 如果是首次积分，则将 lastVinsTransformation 赋值为根据 odom 的 xyz，rpy 转换得到的 transform
            lastVinsTransformation = pcl::getTransformation(cloudInfo.odomX,    cloudInfo.odomY,     cloudInfo.odomZ, 
                                                            cloudInfo.odomRoll, cloudInfo.odomPitch, cloudInfo.odomYaw);
            lastVinsTransAvailable = true;
        } else {
            // ROS_INFO("Obtaining VINS incremental guess");
            //首先从 odom 转换成 transform，获得当前 transform 在 lastVinsTransformation 下的位置

            // 当前帧的初始估计位姿（来自imu里程计），后面用来计算增量位姿变换
            //
            Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.odomX,    cloudInfo.odomY,     cloudInfo.odomZ, 
                                                               cloudInfo.odomRoll, cloudInfo.odomPitch, cloudInfo.odomYaw);
            // inverse 逆矩阵
            //当前帧相对于前一帧的位姿变换---imu里程计计算得到
            Eigen::Affine3f transIncre = lastVinsTransformation.inverse() * transBack;
            //前一帧的位姿
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            //当前帧的位姿
            Eigen::Affine3f transFinal = transTobe * transIncre;
            //更新当前帧位姿transformTobeMapped
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

            // 将视觉惯性里程计的 transform 赋值为 odom 的位姿
            lastVinsTransformation = pcl::getTransformation(cloudInfo.odomX,    cloudInfo.odomY,     cloudInfo.odomZ, 
                                                            cloudInfo.odomRoll, cloudInfo.odomPitch, cloudInfo.odomYaw);

            // 保存当前时态的 imu 位姿
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
            return;
            /// transformTobeMapped存储激光雷达位姿
        }
    } else {
        // ROS_WARN("VINS failure detected.");
        // 没有 odom 信息，或者是第一帧进入的时候
        lastVinsTransAvailable = false;
        odomResetId = cloudInfo.odomResetId;
    }

    //当odo不可用 或者 lastVinsTransAvailable == false

    // use imu incremental estimation for pose guess (only rotation)
    // 只在第一帧调用（注意上面的return），用 imu 累计估计位姿（只估计旋转的）
    //能进入这个if 说明是
    if (cloudInfo.imuAvailable == true)
    {
        // ROS_INFO("Using IMU initial guess");
        // 首先从 imu 转换成 transform，获得当前 transform 在 lastImuTransformation 下的位置
        // 当前帧的姿态角（来自原始imu数据）
        Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
        // 当前帧相对于前一帧的姿态变换
        Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;

        // 将上一时态的 imu 的 transform 点乘相邻两个 imu 时态间的位姿变换，将其赋值给 transformTobeMapped 数组
        // 前一帧的位姿
        Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
        Eigen::Affine3f transFinal = transTobe * transIncre;
        pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                      transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
        return;
    }
}

2 extractSurroundingKeyFrames()

提取局部角点、平面点云集合，加入局部地图

void extractSurroundingKeyFrames()
{
    //关键帧集合为空 直接返回
    if (cloudKeyPoses3D->points.empty() == true)
        return; 

    extractNearby();
}

void extractNearby()
{
    pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
    std::vector<int> pointSearchInd;
    std::vector<float> pointSearchSqDis;

    // extract all the nearby key poses and downsample them 提取所有附近的关键点姿势并对其进行下采样
    // kdtree的输入，全局关键帧位姿集合（历史所有关键帧集合）
    //kdtree作为一个搜索的工具 
    kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
    // surroundingKeyframeSearchRadius默认值为 50.0，scan-to-map 优化的距离
    // 对最近的一帧关键帧，在半径区域内搜索空间区域上相邻的关键帧集合
    // 搜索的结果存储在：pointSearchInd和pointSearchSqDis中
    /
    //开始对back理解错了，back应该就这个点云里的最后一个点
    //这个函数 的第一个参数 应该传入一个点 在树中，搜索这个所有到这个点的距离 < 某个值的点集
    kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
    for (int i = 0; i < (int)pointSearchInd.size(); ++i)
    {
        //将搜索到的每个点，通过id在cloudKeyPose3D中找到对应的关键帧（点云）并加入
        int id = pointSearchInd[i];
        //加入 相邻关键帧位姿集合 中
        surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
    }

    //降采样
    downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
    //降采样后的结果存储在DS中
    downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);

    // also extract some latest key frames in case the robot rotates in one position 同时提取一些最新的关键帧，以防机器人在一个位置旋转
    // 加入时间上相邻的一些关键帧，比如当载体在原地转圈，这些帧加进来是合理的
    // 有个问题：原地旋转为什么不算做空间上相邻的关键帧？？？？？？

    /*
    暂时的想法是：
    3D点云集合中只保存位置信息（xyz）；6D点云中保存位置+旋转信息（xyz+rpy）
    当原地旋转时，关键帧的xyz应该是保持不变的 --> 所以连续的若干帧会在3D点云集合中只保存一个坐标（是不是这样需要后续看cloudKeyPose3D是怎么添加帧的）
    所以使用3D点云集合搜索空间邻域时，会丢失旋转前后的帧（xyz相同 rpy不同）；所以需要重新对事件邻域进行添加（不过这里的添加是将时间段内的帧全部添加）
    这也解释了：为什么楼下判断时，使用的时6D的时间戳（因为6D会保存旋转时的全部帧）
    */

    int numPoses = cloudKeyPoses3D->size();
    for (int i = numPoses-1; i >= 0; --i)
    {
        //当前帧时间戳
        // 为啥用6D
        if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
            surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
        else
            break;
    }

    //将相邻关键帧集合对应的角点、平面点，加入到局部map中，作为scan-to-map匹配的局部点云地图
    extractCloud(surroundingKeyPosesDS);
}

// 相邻关键帧集合对应的角点、平面点，加入到局部 map 中
// 称之为局部 map，后面进行 scan-to-map 匹配，所用到的 map 就是这里的相邻关键帧对应点云集合
void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
{
    std::vector<pcl::PointCloud<PointType>> laserCloudCornerSurroundingVec;
    std::vector<pcl::PointCloud<PointType>> laserCloudSurfSurroundingVec;

    laserCloudCornerSurroundingVec.resize(cloudToExtract->size());
    laserCloudSurfSurroundingVec.resize(cloudToExtract->size());

    // extract surrounding map
    // 遍历当前帧（实际是取最近的一个关键帧来找它相邻的关键帧集合）时空维度上相邻的关键帧集合
    #pragma omp parallel for num_threads(numberOfCores)
    for (int i = 0; i < (int)cloudToExtract->size(); ++i)
    {
        //相邻关键帧索引
        
        //所以能理解成这玩意自动按照intensity排好序了？
        int thisKeyInd = (int)cloudToExtract->points[i].intensity;
        // 提取 50 米范围内的点，transformPointCloud 作用是返回输入点云乘以输入的 transform
        //自定义函数 计算两个点云图之间的距离
        if (pointDistance(cloudKeyPoses3D->points[thisKeyInd], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
            continue;
        // 相邻关键帧对应的角点、平面点云，通过 6D 位姿变换到世界坐标系下
        //transformPointCloud 自定义函数??????????????
        laserCloudCornerSurroundingVec[i]  = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
        laserCloudSurfSurroundingVec[i]    = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
    }

    // fuse the map
    // 赋值线和面特征点集，并且进行下采样
    // laserCloudCornerFromMapDS 比较重要
    laserCloudCornerFromMap->clear();
    laserCloudSurfFromMap->clear(); 
    for (int i = 0; i < (int)cloudToExtract->size(); ++i)
    {
        // 加入局部map
        *laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i];
        *laserCloudSurfFromMap   += laserCloudSurfSurroundingVec[i];
    }

    //降采样
    // Downsample the surrounding corner key frames (or map)
    downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
    downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
    // Downsample the surrounding surf key frames (or map)
    downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
    downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
}

3 downsampleCurrentScan()

当前激光帧角点、平面点集合降采样

void downsampleCurrentScan()
{
    // Downsample cloud from current scan
    //对第二部得到的角点和平面点集合进一步降采样
    laserCloudCornerLastDS->clear();
    downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
    downSizeFilterCorner.filter(*laserCloudCornerLastDS);
    laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();

    laserCloudSurfLastDS->clear();
    downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
    downSizeFilterSurf.filter(*laserCloudSurfLastDS);
    laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
}

前置知识

参数服务器

https://blog.csdn.net/qq_42145185/article/details/106860227

在utility.h文件中定义了很多变量，这些变量都是在paramServer函数中赋值的，使用了ros::NodeHandle.param<>("/一个路径",被赋值的变量,常量)，当参数服务器中有参数1的路径里的值的时候，会将该值赋值给变量；找不到该路径的话，就将参数3赋值。这里探究一下参数服务器。

参数服务器是节点存储参数的地方、用于配置参数、全局共享参数。参数服务器使用互联网传输，在节点管理器中运行，实现整个通信过程。

包括以下类型：32位整数、布尔值、字符串、双精度浮点、ISO 8601日期、列表(List)、基于64位编码的二进制数据。

可以使用三种方式操作参数服务器：

1 命令行操作

ROS中关于参数服务器的工具是rosparam。其支持的参数如下所示：

命令	作用
rosparam list	列出了服务器中的所有参数
rosparam get [parameter]	获取参数值
rosparam set [parameter] [value]	设置参数值
rosparam delete [parameter]	删除参数
rosparam dump [file]	将参数服务器保存到一个文件
rosparam load [file]	加载参数文件到参数服务器

2 .launch文件内读写

launch文件中有两个标签<param/> <rosparam/>用来给参数赋值。其中param用来给单个参数赋值；rosparam用于加载参数文件，参数文件中包含若干参数

lvi-sam项目中的例子：

modeule_sam.launch

<launch>

    <arg name="project" default="lvi_sam"/>
    <!-- Lidar odometry param -->
    <rosparam file="$(find lvi_sam)/config/params_lidar.yaml" command="load" />
    <!-- VINS config -->
    <param name="vins_config_file" type="string" value="$(find lvi_sam)/config/params_camera.yaml" />
    
    .
    .
    .

</launch>

/config/params_camera.yaml

# project name
PROJECT_NAME: "lvi_sam"

lvi_sam:

  # Topics
  pointCloudTopic: "/points_raw"               # Point cloud data
  imuTopic: "/imu_raw"                         # IMU data

  # Heading
  useImuHeadingInitialization: true          # if using GPS data, set to "true"
      .
      .
      .

• $(find lvi_sam) lvi_sam：（pkg_name） https://blog.csdn.net/qq_33835307/article/details/81533140

3 在源代码中使用函数

ros::NodeHandle.param();

utility.h

nh.param<std::string>("/PROJECT_NAME", PROJECT_NAME, "sam");

nh.param<std::string>("/robot_id", robot_id, "roboat");

nh.param<std::string>(PROJECT_NAME + "/pointCloudTopic", pointCloudTopic, "points_raw");
nh.param<std::string>(PROJECT_NAME + "/imuTopic", imuTopic, "imu_correct");
nh.param<std::string>(PROJECT_NAME + "/odomTopic", odomTopic, "odometry/imu");
nh.param<std::string>(PROJECT_NAME + "/gpsTopic", gpsTopic, "odometry/gps");