【LVI-SAM代码学习记录】

---

前言

最近打算在LVI-SAM的框架上改点东西，面临的问题：细节太多+编程能力不足+没人带，本来想开始摆烂了但是室友和对象早上8点出门学习的关门声重重的敲在了我弱小的心灵上，还是决定浅浅支棱下试试，也不知道第一步干什么，那就先从阅读代码框架和流程开始吧（长文预警，持续更新）

一、思路

刚上来也不知道从哪个文件看起，决定先找个流程图，还是按经典的总分总来学(老文科生思维了)。

来自【SLAM】LVI-SAM解析——综述_iwander。的博客-CSDN博客_lvi sam

【SLAM】LVI-SAM解析——综述_iwander。的博客-CSDN博客_lvi sam 可以看到，重点要找两个子系统间的信息交互部分，去畸变的点云通过cloudHandler下的publishCloud()，通过lidar_callback传给视觉子系统，然后通过img_callback下的get_depth()将雷达点云的深度和图像特征点匹配，视觉回环那有一个检测回环信息的函数publish(match_msg)回传信息给激光子系统的loopHandler()，因为视觉那的回环被删除了，只进行检测，然后回环处理是由激光那边完成的。还有imuHandler()下的函数将imu里程计传给了vins_estimator下的odom_callback()，然后给getOdpmetry，这里存在一点小问题，图里标的imuHandler()这里传过去的imu里程计信息，本来应该是odom_callback这个回调函数应该是获取雷达里程计odom信息，转相机系⽤于VIO初始化的，激光惯性里程计信息是否是在这里传的还是图的作者简写了传的信息类型？后续看到了具体代码再来填坑。初步分析下，LIO初始化快，VIO初始化慢，讲道理在LIO没有失效的情况下都应该是用LIO传过来的里程计信息进行初始化，这里传的应该是激光惯性里程计信息。然后视觉部分中imu_callback()获取imu信息通过pubLatestOdometry()进行位姿变化，主要是获取IMU原始信息，中值积分算位姿，⽤于下⼀帧图像未到来之前的⾼频位姿，这里得到的位姿转成Twl也就是转到雷达坐标系下然后传给激光部分的odometryHandler，用作LIO匹配的初值。总结下，LIO和VIO主要存在以上存在四部分关联，对应分别是深度关联（LIO->VIO）、回环检测（VIO->LIO）、视觉位姿给LIO作初值（VIO->LIO）、LIO给VIO初始化（LIO->VIO）。这么来看，最好的做法是分别看懂视觉和激光部分的内容，再利用关联部分的信息进行串联，我选择先从激光部分看起毕竟这是主系统，并且激光部分内容比视觉少，那就开始啦。

然后程序中有挺多回调函数 特地再去理解了下回调函数的概念 是否有点多线程的味道？编程小白欢迎大佬指导。

有大量回调函数 小白还要  (128条消息) 通俗理解“回调函数”_昂刺鱼人工智能的博客-CSDN博客_回调函数

也就是当在A类一个方法中处理一个费时操作时，调用另外类B来处理，B处理完后，回调A中的方法，当然A中需要B的引用，（调用B中方法使用），A调用B中方法需要传入自己的引用（这个过程可以理解为注册），后面B可以回调A中的回调方法，有点多线程内味？还是后面结合代码具体分析吧

二、LIO（激光惯性里程计）部分代码阅读

1.imagaProjection()

我们先看imagaProjection函数，找到主函数，发现这边内容少的可怜，只用了ImageProjection初始化了一个对象，其他的都是ros的操作。既然看到了，也学了下，ros::MultiThreadedSpinner spinner(3);是ros中的多线程操作，括号内是线程数，有以下几种实现：

• 多线程：订阅多个Topic，多个Spinner threads，防止单线程阻塞浪费时间
• 订阅一个Topic，多个Spinner threads，可以在发布端频率比较高的时候尽可能多的响应消息
• 订阅多个Topic，每个Subscriber一个Callback queue
• 详见动手学ROS（7）：精讲多线程之MultiThreadedSpinner - 知乎 (zhihu.com)

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar");
 
    ImageProjection IP;
 
    ROS_INFO("\033[1;32m----> Lidar Cloud Deskew Started.\033[0m");
 
    ros::MultiThreadedSpinner spinner(3);
    spinner.spin();
 
    return 0;
}

跳转到ImageProjection类里，先定义了一堆发布和订阅，然后在构造函数里将点云按行列存在fullcloud里并对每个获取的lidar message进行参数重置，然后到类内函数：

imuHandler、odometryHandler分别来读取imu和里程计的信息，然后存入各自的队列imuQueue和odomQueue里。cloudHandler里传入雷达点云信息，操作较多：

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
        //1 检查队列里面的点云数量是否满足要求 并做一些前置操作
        if (!cachePointCloud(laserCloudMsg))
            return;
        //2 对IMU和视觉里程计去畸变
        if (!deskewInfo())
            return;
        //3 获取雷达深度图
        projectPointCloud();
        //4 点云提取
        cloudExtraction();
        //5 发布点云
        publishClouds();
        //6 重置参数
        resetParameters();
    }

bool cachePointCloud是检查队列里的点云数量、是否去除无效点、是否含有ring通道和时间戳；bool deskewInfo里通过imuDeskewInfo()和odomDeskewInfo()对imu和视觉里程计去畸变（Q：这俩去啥畸变？），视觉里程计这里从vins_estimator/utility/visualization.cpp传过来的视觉里程计信息要注意格式，比如最后一位failureCount，每次失败都会加一，用来提示视觉里程计重启过不准了，自己改框架的话这种细节很重要。然后通过projectPointCloud将深度信息投影到RangeImage上，然后用deskewPoint对点云进行去畸变。去完畸变后通过cloudExtraction提取点云的Range Image，去点前后各五个因为要算曲率啥的前后都没东西自然就没用了，然后应该是赋给点云的线束信息啥的，这里听了课回来再补一下，先标黄。

2.featureTracker()

然后到了提取特征点的这部分文件featureTracker()

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar");
 
    //构建特征提取对象->构造器
    FeatureExtraction FE;
 
    ROS_INFO("\033[1;32m----> Lidar Feature Extraction Started.\033[0m");
   
    ros::spin();
 
    return 0;
}

一看主函数优势只有一个实例化类对象，C++学的菜如我一下子蒙了，再往后看才知道在哪用了吧，然后也来看一下这个类函数，一上来定义了两个小结构体smoothness_t（在下面定义了一个容器std::vector<smoothness_t> cloudSmoothness）和by_value，

FeatureExtraction类中一上来也定义了雷达点云的订阅者和雷达点云、角点面点的发布者。构造函数里调用了自身类函数laserCloudInfoHandler处理点云，订阅者subLaserCloudInfo处理完点云后，将相关数据通过上述三个发布者向后续节点发布数据，然后初始化相关容器。重点来看laserCloudInfoHandler函数，首先将点云格式从ros转换到pcl,然后：

1.使用calculateSmoothness()计算点云中各点曲率，为提取特征点左准备，进入到calculateSmoothness函数中，某一点云i的曲率利用i左右各取五个点辅助算出，曲率计算公式 = 当前点前5个点距离和 - 当前点距离*10 + 当前点后5个点距离和；最终采用平方的形式（x、y、z距离分别平方）记下算出的曲率。cloudNeighborPicked[i]为点i的标记位，若该点可能被遮挡则不用。然后根据曲率对点云进行分类，分为大曲率点、较平坦点、平坦点。前面结构体里定义的cloudSmoothness这里使用，cloudSmoothness[i].value用来存曲率值，cloudSmoothness[i].ind用来存索引，避免后面对曲率排序时候乱掉。

2、使用markOccludedPoints()屏蔽点云中被遮挡点或平行点，相邻两帧做差，差小，水平角比较近，认为容易遮挡，不参与特征点提取，这部分比较简单分析网上到处都有（果然简单的事做的人多啊hhh），认为可能会遮挡的点设置标志位cloudNeighborPicked[i] = 1。

3、使用extractFeatures()提取角特征和面特征。首先将存放角点和面点的容器cornerCloud、surfaceCloud清空，然后定义了两个指针surfaceCloudScan、surfaceCloudScanDS。接着遍历每个scan，将scan六等分

int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
    continue;

这一段没看懂，听到课回来补。曲率大于阈值算一个合格角点，小于设定阈值算合适的面点，然后就是一些tricks了，比如避免特征点过于集中，特征点周围五个点给屏蔽掉，面点数量很大，为节约计算时长，进行降采样等。

4、最后用publishFeatureCloud()把角点云和面点云发布出去。

3、imuPreintegration()

先看主函数和前两个一样

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar");
    
    IMUPreintegration ImuP;
 
    ROS_INFO("\033[1;32m----> Lidar IMU Preintegration Started.\033[0m");
 
    ros::spin();
    
    return 0;
}

来看IMUPreintegration类，开始订阅和转换的东西有点多，订阅了IMU和subOdometry（这个是什么的里程计？看流程图是mapOptimization里的publishOdometry来自mapOptimization的激光里程计），然后发布了IMU里程计和IMU路径，使用tf完成了map到odom、odom到base_link的转换，然后定义了协方差矩阵、imu预积分器、imu数据队列、因子图优化中要用到的状态变量等。构造函数中的两个订阅者分别订阅了话题名为imutopic和PROJECT_NAME +  /lidar/mapping/odometry对应的话题，然后对应的回调函数分别为imuHandler和odometryHandler，然后定义协方差矩阵、预积分器、噪声先验等。

        // 订阅imu原始数据，用下面因子图优化的结果，施加两帧之间的imu预积分量，预测每一个时刻（imu频率）的imu里程计
        subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）
        subOdometry = nh.subscribe<nav_msgs::Odometry>(PROJECT_NAME + "/lidar/mapping/odometry", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

进入到imu的回调函数，主要功能是一个imu里程计，就是用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，再把imu里程计的位姿转到雷达坐标系，然后发布出去。vins_estimator下的odom_callback订阅了这个里程计。odometryHandler回调函数的操作多很多，整体上首先每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化，然后计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态，最后优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿。

        具体来看，首先从mapOptimization中发布的激光里程计（scan to map匹配得到）中订阅当前帧的位姿，然后设置里程计先验prevPose_（转到imu坐标系下的位姿），速度先验prevVel_（设为0）和imu bias先验prevBias_（设为0），分别赋值给第一次优化变量节点作初值X(0)，V(0)，B(0)。然后用optimizer.update(graphFactors, graphValues)进行一次优化，graphValues就是X、V、B，的值，节点就是priorPose、priorVel和priorBias。如priorPose，是用gtsam::PriorFactor定义的，有priorBias(B(0), prevBias_, priorBiasNoise)，即初值、先验和噪声项，然后用graphFactors.add(priorPose)添加到节点中。优化一次后完成初始化。初始化标志位systemInitialized设为true。然后每隔100帧激光里程计，通过resetOptimization()重置ISAM2优化器，保证优化效率，然后再通过上面的那些操作重新进行优化当imuQueOpt非空的时候，提取前一帧和当前帧之间的imu数据，利用gtsam计算预积分，然后删除处理过的imu数据。然后添加imu预积分因子，用前一帧的状态和bias，施加imu预积分量，得到当前帧的状态，预测量作为初始值给到X(key)、V（key）和B（key）插入到因子图当中。进行两次优化，优化后的当前状态作为当前帧的最佳估计，更新当前帧位姿、速度、偏置。 优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿，再有一个失效检测，速度超过30的时候认为失效，这一块基本就结束了。

4、mapOptmization（）

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar");
 
    mapOptimization MO;
 
    ROS_INFO("\033[1;32m----> Lidar Map Optimization Started.\033[0m");
    //前面那个是vins的回环 这里是里程计的检测
    std::thread loopDetectionthread(&mapOptimization::loopClosureThread, &MO);
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
 
    ros::spin();
 
    loopDetectionthread.join();
    visualizeMapThread.join();
 
    return 0;
}

这个是激光雷达主要处理的一个类了，两千多行代码也是有点东西的，主函数除了初始化了一个mapOptimization类MO以外，还有两个回环的检测，视觉的回环用词袋，然后里程计用kd树搜索，分别会给一个判断回环的标志位，判断中了就进行全局位姿优化。然后进到类MO里面，首先定义了里程计、路径和一堆点云的发布者，订阅了点云、GPS和回环信息，分别对应回调函数：&mapOptimization::laserCloudInfoHandler、&mapOptimization::gpsHandler和&mapOptimization::loopHandler。

当接收到提取好特征的点云消息“PROJECT_NAME + "/lidar/feature/cloud_info" ” 就会调用该回调函数laserCloudInfoHandler，传入的是点云信息格式msgIn，用timeLaserInfoStamp获取传入点云消息的时间戳，用timeLaserInfoCur获取传入点云消息的时间点，然后将点云信息指针->点云信息cloudInfo = *msgIn，将角点面点格式从ROS转换到PCL后，加线程锁。设定条件，如果两帧点云的时间戳离得太近，则不进行处理。满足时间要求后进入以下几个函数处理：

1.updateInitialGuess()求解当前帧位姿的初值估计，存至transformTobeMapped。首先，如果是cloudKeyPoses3D->points为空，即没有关键帧的情况，应该是对应刚开始第一帧还没有里程计的情况，初始位姿由IMU磁力计提供（9轴IMU的用处），然后用pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit)转成Eigen::Affine3f格式的静态变量，lastImuTransformation就有了第一帧的位姿，可以退出了。如果里程计是可以用的，而且是第一次收到VINS来的里程计消息（lastVinsTransAvailable == false），则需要通过getTransformation将里程计的位姿格式转化为Eigen::Affine3f的lastVinsTransformation进行后续操作。

                lastVinsTransformation = pcl::getTransformation(cloudInfo.odomX,    cloudInfo.odomY,     cloudInfo.odomZ, 
                                                                cloudInfo.odomRoll, cloudInfo.odomPitch, cloudInfo.odomYaw);
                // 已经收到过VINS IMU递推的里程计，则将lastVinsTransAvailable标志变量置为true
                lastVinsTransAvailable = true;

不是第一次接受就直接开始处理，首先也要进行格式转换，将VINS IMU递推的里程计转换为Eigen::Affine3f的transBack，然后lastVinsTransformation.inverse() * transBack获取VINS里程计相对于上一次的里程计增量transIncre，将上一帧优化结束获得的关键帧估计结果transformTobeMapped转换为Eigen::Affine3f格式的transTobe（注意这里的transformTobeMapped是上一帧优化结束获得的关键帧估计结果，在全局变量里定义transformTobeMapped[6]），前面九轴IMU给的也是这个。

                // Twlk^ = Twlk-1 * Tlk-1lk
                Eigen::Affine3f transFinal = transTobe * transIncre;

用上一帧的位姿乘上增量得到最后的结果。然后进行更新，将当前VINS+IMU递推的里程计 -> lastVinsTransformation（新旧交替）。如果 VINS IMU里程计不可用，如果没有里程记信息，就是用imu的旋转信息来更新，因为单纯使用imu无法得到靠谱的平移信息，因此，平移直接置0。

2、extractSurroundingKeyFrames()，提取与【当前帧】相邻的关键帧surroundingKeyPosesDS，组成局部地图laserCloudCornerFromMap、laserCloudSurfFromMap。先判断cloudKeyPoses3D->points.empty() == true，如果是则代表还没有关键帧，即当前是第一帧，直接return。不为空，进入extractNearby()，这块指针什么的太多了，为了节约时间直接看代码注释了解一个大概了，即定义存储【当前帧】附近(指定半径范围内)的【关键帧位置信息】的点云指针(集合)，然后定义索引的vector，用存储关键帧位置信息的点云/集合，用以构建KD-Tree，根据最新关键帧位置cloudKeyPoses3D->back()提取关键帧位置集合cloudKeyPoses3D中距离最后一个关键帧指定半径范围(50.0m)内的所有关键帧，cloudKeyPoses3D存的是关键帧位置，然后降采样，总之最后得到了筛选过后的近邻关键帧位置集合surroundingKeyPosesDS，然后要整合其关键帧对应的点云，得到局部地图。用extractCloud(surroundingKeyPosesDS)，输入的是一个点云类型指针（不懂为什么要吧关键帧放在点云类型的指针里），首先设置两个大小为局部地图包含点云帧数的容器laserCloudCornerSurroundingVec和laserCloudSurfSurroundingVec，遍历筛选出来的关键帧集合cloudToExtract，把强度值索引给到关键帧索引thisKeyInd，

            // cornerCloudKeyFrames是在 saveKeyFramesAndFactor中被赋值
            // 将遍历到的关键帧按照其【位姿】转到世界坐标系，加入到点云vector中
            //cornercloud里面存的才是关键帧对应的点云的指针
            //存到局部地图角点云、面点云对应的容器？中
            laserCloudCornerSurroundingVec[i]  = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
            laserCloudSurfSurroundingVec[i]    = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);

先来看一下参数cornerCloudKeyFrames，注释说是在saveKeyFramesAndFactor()里赋值，跳转过去，先进行了addOdomFactor()和addLoopFactor()，再进去看一下，addOdomFactor中如果是第一帧，贼增加一个先验约束，如果不是则获取上一帧的关键位姿pclPointTogtsamPose3(cloudKeyPoses6D->points.back())和当前最新关键帧的位姿trans2gtsamPose(transformTobeMapped)，用图优化模型gtSAMgraph.add来添加一个帧间约束（？这个最新帧是怎么来的这个是在saveKeyFramesAndFactor的下面，获取的是优化完的位姿，下文中也标黄了），回环也差不多，用新帧和匹配到的历史帧做一个约束。跳回saveKeyFramesAndFactor，用isam->update(gtSAMgraph, initialEstimate)把前面构建的约束的缓存都压进去，isamCurrentEstimate = isam->calculateEstimate()拿到全部的关键帧位姿，然后latestEstimate是在优化结果isamCurrentEstimate中往里面添加索引得到最新的关键帧的优化位姿估计结果。thisPose3D是xyz位置信息+强度，thisPose6D多了三个欧拉角信息和时间戳，把latestEstimate的位姿信息分别给到thisPose3D和thisPose6D。cloudKeyPoses6D是点云结构但存的是位姿。获取最新位姿估计的协方差poseCovariance， 获取最新位姿估计结果存至transformTobeMapped。这里逻辑有点没懂，我拿最新位姿去构建帧间约束，然后优化获得最新位姿？？那这里的scan-to-map匹配去哪了？？transformTobeMapped就是在scan2MapOptimization()里求解出来的最新位姿，然后这里将最新位姿和上一帧的增量做一个约束就是里程计约束了。将当前帧降采样后的角点和面点存至thisCornerKeyFrame、thisSurfKeyFrame，存入存储点云指针的容器cornerCloudKeyFrames和surfCloudKeyFrames中，遇到关键帧后进行全局位姿图优化，之后updatePath(thisPose6D)对globalPath进行更新。（laserCloudCornerLastDS是降采样后的点云，这个初始点云是从哪里发布的？回到sub中发现订阅的是featrueExtraction节点laserCloudInfoHandler发布的点云特征信息）。

跳出saveKeyFramesAndFactor，回到上一个代码段中，我们通过前述阅读明白了cornerCloudKeyFrames和surfCloudKeyFrames是储存点云指针的容器，再来看另外一个参数cloudKeyPoses6D->points[thisKeyInd]是位姿、强度、时间戳，参数明确了，看下这个转换的函数transformPointCloud，嗯没有注释，灰溜溜的出来了，总之这个函数干了这么一件事：将关键帧对应的点云指针存到局部地图角点云、面点云对应的容器laserCloudCornerSurroundingVec、laserCloudSurfSurroundingVec中。角点云面点云基本操作一样的，我就只写一个了。然后首先首先移除角点云、面点云局部地图中的所有点laserCloudCornerFromMap->clear()，遍历【筛选出来的关键帧集合】cloudToExtract，进行*laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i]，即将转到世界坐标系的角点云、面点云加入到各自局部地图，注意这里就拿到局部地图laserCloudCornerFromMap啦，然后进行降采样等操作完事。然后就要跳回到主线程laserCloudInfoHandler了，进入下一个函数。

3、downsampleCurrentScan()对当前帧收到的角点云、面点云进行降采样，获取laserCloudCornerLastDS、laserCloudSurfLastDS，这个地方怎么感觉已经很多地方用到了，就一个降采样操作也没几行就过了。

4、scan2MapOptimization()求解当前帧位姿transformTobeMapped;怎么这个transformTobeMapped是在这里求解的，那个顺序可能还得理一下，不是完全的顺序想得太简单了。先来看这个函数，同样的，如果是第一帧，那就没有办法scan-to-map，直接退出，然后当当前帧角点云、面点云特征分别大于设定的阈值，这里是10、100的时候，才能进行scan-to-map优化，用刚构建的角、面局部地图分别构建KD-Tree然后每两帧优化一次。setInputCloud是pcl的库函数，大概就是处理下读入进来的点云，然后以下有一个循环：

 // 以下操作迭代计算30次
            for (int iterCount = 0; iterCount < 30; iterCount++)
            {
                laserCloudOri->clear();
                coeffSel->clear();
 
                // 求解遍历到的角点对应的残差的反方向向量与残差大小
                cornerOptimization();
 
                // 得到了平面法向量以及残差(无绝对值)
                surfOptimization();
 
                /* 将符合条件的角点以及面点
                   在LiDAR系坐标push入laserCloudOri
                   带有权重的残差push入coeffSel
                */
                combineOptimizationCoeffs();
 
                // 构建H△x=b方程，求解△x，并更新到待优化变量transformTobeMapped上
                if (LMOptimization(iterCount) == true)
                    break;              
            }

laserCloudOri是存当前遍历到的只有呈线性分布的角点和拟合结果合理的平面的面点的容器，coeffSel是带有权重信息的残差push入coeffSel，cornerOptimization()中，前面在updateInitialGuess()函数中已经得到当前帧Twl的预测值transformTobeMapped了，现在用updatePointAssociateToMap()将其转换为Eigen::Affine3f格式的transPointAssociateToMap，然后进入一个for循环，这个for循环是被多个线程同时运行的，遍历当前帧降采样后的角点云，然后令pointOri = laserCloudCornerLastDS->points[i]，pointOri是激光系下遍历到的第i个点pointOri，然后按照当前帧预测的位姿transPointAssociateToMap将pointOri转换到W'系，得到pointSel，然后kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis)在构建的角点局部地图laserCloudCornerFromMapDS中找到距离pointSel最近的5个点，然后是要拟合一条直线吧，PCA方式判断如果五个点中最远的距离也小于1m，那么首先计算角点局部地图laserCloudCornerFromMapDS中这5个最近点的均值，然后计算这5个点的协方差矩阵matA1，再求解这5个点协方差矩阵的特征值matD1和特征向量matV1，目的是为了判断这五个点是不是呈线性分布，判断方法是：如果最大特征值 > 3*次大特征值，则认为这5个点呈线性分布，然后计算点到直线的距离，当做残差。如果残差小于1m，将当前点LiDAR坐标系的坐标pointOri保存至laserCloudOriCornerVec，同时将带有残差信息的coeff存至coeffSelCornerVec（coeffSelCornerVec[i] = coeff）。surfOptimization()则是拟合平面，求点到面的残差，最后将带有残差的信息存在coeffSelSurfVec[i] = coeff中。combineOptimizationCoeffs()则将符合条件的角点以及面点在LiDAR系坐标push入laserCloudOri，带有权重的残差push入coeffSel。然后通过bool型函数LMOptimization(int iterCount)构建H△x=b方程，求解△x，并更新到待优化变量transformTobeMapped上，LMOptimization()构建增量方程用到的约束个数就是由laserCloudOri的数量决定的。而只有在laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]为true才会将点塞入laserCloudOri。而laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]是在cornerOptimization()和surfOptimization()中判断s > 0.1时才置为true。迭代优化30次后，再对优化结果transformTobeMapped做一个限制，（如果测力计pitch角度小于阈值就把权重调成怎么样？虽然只是一个小tricks但感觉蛮重要的slam比较有效的操作我感觉就是加权），然后如果特征不足，输出特征不足的警告。scan2MapOptimization就结束了

5、对于关键帧，用saveKeyFramesAndFactor()加入里程计因子、回环因子，进行全局位姿图优化，最新帧位姿保存至transformTobeMapped，更新globalPath中的最新位姿，这里的频率比5hz还要低，因为只对关键帧优化，且全局位姿图优化很耗时。这个函数在extractSurroundingKeyFrames()用到赋值的时候简单看过了。

6、遇到回环，就对所有关键帧位姿进行调整，在correctPoses()内完成：检测到回环后，更新所有历史关键帧位姿到cloudkeyPoses3D中，顺便也updatePath(cloudKeyPoses6D->points[i])更新globalPath，然后将回环标志位aLoopIsClosed = false

7、最后将相对高频的LO（5Hz）(transformTobeMapped)和每一帧发送出去。

激光部分先到这里，最近有点别的事情可能要久一点再更了，希望别忘光了....待续.....

三、VIO（视觉里程计部分）代码阅读

        这一部分对我而言不是重点，就简略的看了。

        先进入到visual_feature中的feature_tracker.h文件中，这里主要完成特征点的提取和光流跟踪，以及视觉深度点和激光雷达深度点的匹配，首先构建一个存储点云信息的容器，tf工具查询当前图像帧时刻的变换，监听从世界坐标系到body坐标系的变换，用xCur = transform.getOrigin().x()和m.getRPY(rollCur, pitchCur, yawCur)得到平移、旋转，构建出特征xCur, yCur, zCur, rollCur, pitchCur, yawCur，投影去畸变归一化的2d特征点到单位球面，再投影深度图至离散化表格range image进行同一区域点云滤波，并从中选取距离有效的点，形成新的深度点云图，将深度点云图投影至单位球面;用单位球面点云图构建kd树，使用KD树寻找距离图像特征最近的三个点,并求解相机中心经图像特征的向量到这三个点平面的距离，并将深度超过3m的用depth_of_point.values[i] = features_3d_sphere->points[i].intensity返回（为什么是超过3m的？待回看原理补充），最后将投影点画在图像上，以可视化。feature_tracker.cpp中主要是光流法的相关操作，调用opencv的函数进行提取特征点的相关操作，并进行光流追踪，通过opencv光流追踪给的状态位剔除outlier，再进入到feature_tracker_node.cpp中，看名字应该是和ros相关的操作，首先定义了一个img_callback，计算图像特征点的回调函数，并发布，这里还需要将ros图像格式转化为opencv格式，还有一个lidar_callback雷达回调函数，获取雷达数据后首先进行降采样并进行滤波，只保留相机视野内的雷达点，并将雷达点云转换至相机坐标系，将雷达点云转换为全局里程计坐标系（全局里程计坐标系是什么？）， 保存最后转换完的点云和对应时间戳，叠加5s内的点云数据，最后融合全局点云并进行全局下采样。主函数竟然在这里，真是太久不看了都生疏了，main里首先初始化ros节点，然后读取文件配置和获取相机参数，初始化深度记录器后，在这里订阅图像和雷达数据，进入回调函数：

    ros::Subscriber sub_img   = n.subscribe(IMAGE_TOPIC,       5,    img_callback);
    ros::Subscriber sub_lidar = n.subscribe(POINT_CLOUD_TOPIC, 5,    lidar_callback);

最后 将获取的特征pub_feature、pub_match、pub_restart发布给估计器estimator。最后用多线程雷达和图像并行处理。这里要考虑下给estimator的特征都是什么，后面才能方便对齐。

进入estimator.cpp中，void Estimator::processImage是边缘化关键帧的，marginalization_flag是边缘化哪一帧的标志位，MARGIN_OLD是最老帧，MARGIN_SECOND_NEW是最新帧。且进行系统初始化，如果激光雷达信息可使用，则直接使用激光惯性子系统的数据，激光雷达不可用就用视觉的信息进行初始化，imu要通过加速度标准差判断是否有充分的运动激励。然后是一些三角化和滑窗的操作，以及故障检测，还有一个优化函数void Estimator::optimization()，应该是vins+imu的视觉优化模块，这里把原来的回环检测给删了。estimator_node.cpp中，main函数里依次发布了sub_imu、sub_odom、sub_image和sub_restart，分别对应回调函数imu_callback、odom_callback、feature_callback和restart_callback。依次来看，imu回调函数中predict(imu_msg)递推得到IMU的当前时刻的PQV（初值），然后发布最新的由IMU直接递推得到的PQV （用于RVIZ可视化），odom回调函数中从激光惯性子系统获取的相关参数，odomQueue.push_back(*odom_msg)压入队列中。feature回调也是把之前处理的图像压入队列，restart回调应该就是看看有没有重启发给后面进行处理。

feature_manager.h中，class FeaturePerId中如果lidar测量的深度在图像前面 则为有效，用lidar的深度estimated_depth = _measured_depth，标志位置turelidar_depth_flag = true，否则estimated_depth = -1；class FeatureManager中有一堆操作，先不看了。

进入到feature_manager.cpp中，一个比较关心的地方是如果雷达估计的深度没有给到特征点，怎么办，看这里是直接剔除了，相关的函数是：

//如果估计失败则剔除
void FeatureManager::removeFailures()
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        // 估计失败则剔除该特征点
        if (it->solve_flag == 2)
            feature.erase(it);
    }
}
//清除存储的深度
void FeatureManager::clearDepth(const VectorXd &x)
{
    int feature_index = -1;
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
 
        it_per_id.estimated_depth = 1.0 / x(++feature_index);
        it_per_id.lidar_depth_flag = false;
    }
}

还有一些三角化和去除外点的操作。

还有一个大块是visual_loop，这也不是重点，只考虑怎么迁移就行，先看到这里。

---

总结