LVI-SAM mapOptimization 代码解析_using symbol_shorthand::x;

本篇文章来对图优化内容进行解析。如今，图优化和卡尔曼滤波已经成为了主流的后端优化方法。

头文件

// 头文件，和imuPreintegration差不多
#include "utility.h"
#include "lvi_sam/cloud_info.h"
 
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
 
#include <gtsam/nonlinear/ISAM2.h>
 
// 命名空间，这个作用就是不用写gtsam::了，就直接用就可以
using namespace gtsam;
 
// 这个意思就是X表示位姿（位置 + 姿态） 然后V表示速度，这个后面有赋值，例如V(0)这种的
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::G; // GPS pose

首先这个头文件和imu预积分的内容差不多，其中有几个库是imu预积分没有的，当然imu预积分没有用上那些库，所以才没有include进去。然后下面是使用了gtsam命名空间，这样的话就可以不用谢gtsam::然后啥啥啥变量了，就直接写变量名即可。然后是XVBG，这些就是起了个名字，具体的用法的话，在我的印象里X(0)，类似这种，进行赋值。

变量

/*
 * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
 * 这里非常贴心的告诉阅读者 intensity里面装的是时间戳
 */
struct PointXYZIRPYT
{
    PCL_ADD_POINT4D // xyz + padding(填充的首选方法)
    PCL_ADD_INTENSITY;
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 这个是eigen字节对齐用的，前面搜过
} EIGEN_ALIGN16;
 
// 注册为PCL点云格式 （PCL内有自己定义的一系列点云格式，用结构体扩展后需要注册）
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
                                   (double, time, time))
 
typedef PointXYZIRPYT  PointTypePose; // 重命名
 
// 定义的类，还是公有继承ParamServer
class mapOptimization : public ParamServer
{
 
public:
 
    // gtsam
    NonlinearFactorGraph gtSAMgraph; // 包含非线性因子的因子图
    // Values 用于指定因子图中一组变量的值
    Values initialEstimate;
    Values optimizedEstimate;
    // 优化器
    ISAM2 *isam;
    Values isamCurrentEstimate;
    Eigen::MatrixXd poseCovariance; // 位置协方差
 
    // 发布
    // 下面是在ros作用域下面，也就是可能会显示到rviz里面
    ros::Publisher pubLaserCloudSurround; // 点云信息 发布关键帧的map的特征点云
    ros::Publisher pubOdomAftMappedROS; // odometry信息 发布激光里程计
    ros::Publisher pubKeyPoses; // 点云信息 发布关键位姿信息
    ros::Publisher pubPath; // 路径信息 发布路径，主要是给rivz用于展示
 
    ros::Publisher pubHistoryKeyFrames; // 历史点云信息 发布历史关键帧
    ros::Publisher pubIcpKeyFrames; // 点云信息 icp方法 配准的 对齐的 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云
    ros::Publisher pubRecentKeyFrames; // 点云信息 发布局部map的降采样平面边重合
    ros::Publisher pubRecentKeyFrame; // 点云信息 发布历史帧的角点、平面点降采样集合
    ros::Publisher pubCloudRegisteredRaw; // 点云信息 校准之后的点云信息
    ros::Publisher pubLoopConstraintEdge; // MarkerArray信息 发布闭环边信息
 
    // 输入：激光点云信息、GPS信息、闭环信息
    ros::Subscriber subLaserCloudInfo; // 订阅当前激光帧点云信息，来自FeatureExtraction
    ros::Subscriber subGPS; // 订阅GPS里程计
    ros::Subscriber subLoopInfo; // 订阅来自外部闭环检测程序提供的闭环数据。（本程序没有提供）
 
    std::deque<nav_msgs::Odometry> gpsQueue;
    lvi_sam::cloud_info cloudInfo; // 这个是作者团队自己定义的，可以在lvi_sam/cloud_info.h里面看一下
 
    // 历史所有关键帧的角点集合（降采样）
    vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
    // 历史所有关键帧的平面点集合（降采样）
    vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
    
    // 历史关键帧位姿
    pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
    pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
 
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization 来自优化里面的角点
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
    // 后面DS是downsampled 降采样的简称 是上面经过降采样后的数据
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
 
    pcl::PointCloud<PointType>::Ptr laserCloudOri;
    pcl::PointCloud<PointType>::Ptr coeffSel; // coeffSel ： 系数选择
 
    std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation 用于并行计算的角点支架 
    std::vector<PointType> coeffSelCornerVec;
    std::vector<bool> laserCloudOriCornerFlag;
    std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
    std::vector<PointType> coeffSelSurfVec;
    std::vector<bool> laserCloudOriSurfFlag;
 
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
 
    //局部关键帧构建的地图点云，对应kdtree，用于scan-to-map找相邻
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
 
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
 
    pcl::PointCloud<PointType>::Ptr latestKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr nearHistoryKeyFrameCloud;
 
    //降采样
    pcl::VoxelGrid<PointType> downSizeFilterCorner;
    pcl::VoxelGrid<PointType> downSizeFilterSurf;
    pcl::VoxelGrid<PointType> downSizeFilterICP;
    pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
    
    ros::Time timeLaserInfoStamp;
    double timeLaserInfoCur;
 
    float transformTobeMapped[6];
 
    std::mutex mtx; // 这个是锁，用于共享内存数据读取的
 
    bool isDegenerate = false; // Degenerate：退化
    cv::Mat matP; // 这个是矩阵
 
    // 当前激光帧角点数量 还得是降采样的
    int laserCloudCornerLastDSNum = 0;
    // 当前激光帧平面点数量 降采样之后的
    int laserCloudSurfLastDSNum = 0;
 
    bool aLoopIsClosed = false;
    int imuPreintegrationResetId = 0;
 
    //一个关于运动路径的消息
    nav_msgs::Path globalPath;
 
    //当前帧位姿
    Eigen::Affine3f transPointAssociateToMap;
 
    map<int, int> loopIndexContainer; // from new to old
    vector<pair<int, int>> loopIndexQueue;
    vector<gtsam::Pose3> loopPoseQueue;
    vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;

首先是定义了一个结构体，这个结构体里面包含了xyz RPY 还有时间戳这些非常基本的内容。然后这个EIGEN_MAKE_ALIGNED_OPERATOR_NEW是EIGEN字节对齐用的，这个不再重述了。然后下面是注册为点云格式，然后这个的话，就看看记一下格式模板即可。最后把定义的结构体重新命名一下，这样显得更加高端一些！下面就是mapOptimization类中定义的变量，首先它公有继承自ParamServer。然后下面的话就是一些列的值，这里就不过多讲解了，自己看注释。这里的话，分清作用域即可。然后下面有锁std::mutex mtx，还有一些下面函数需要单独使用的变量，哲理的话不讲解，没啥意思。

函数

    //构造函数
    mapOptimization()
    {
        ISAM2Params parameters; // ISAM2优化器的参数
        // 设置参数
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1;
        // 初始化优化器
        isam = new ISAM2(parameters);
 
        // 发布历史关键帧里程计
        pubKeyPoses           = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/trajectory", 1); // 后面这个1是消息接受的数量，就看成能接受几条就可以
        // 发布局部关键帧地图的特征点云
        pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/map_global", 1);
        // 发布激光里程计，rviz中表现为坐标轴
        pubOdomAftMappedROS   = nh.advertise<nav_msgs::Odometry>      (PROJECT_NAME + "/lidar/mapping/odometry", 1);
        // 发布路径
        pubPath               = nh.advertise<nav_msgs::Path>          (PROJECT_NAME + "/lidar/mapping/path", 1);
 
        // 订阅当前激光帧点云信息，来自featureExtraction
        subLaserCloudInfo     = nh.subscribe<lvi_sam::cloud_info>     (PROJECT_NAME + "/lidar/feature/cloud_info", 5, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅GPS里程计 这个GPS在图优化部分可以先不考虑，这个加入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());
 
        // 发布闭环匹配关键帧局部地图
        pubHistoryKeyFrames   = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/loop_closure_history_cloud", 1);
        // 发布当前关键帧经过闭环优化后的位姿变换以后的特征点云
        pubIcpKeyFrames       = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/loop_closure_corrected_cloud", 1);
        // 发布闭环边，rviz中变现为闭环帧之间的连线
        pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>(PROJECT_NAME + "/lidar/mapping/loop_closure_constraints", 1);
 
        // 发布局部地图的降采样平面点集合
        pubRecentKeyFrames    = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/map_local", 1);
        // 发布历史帧的角点、平面点降采样集合
        pubRecentKeyFrame     = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/cloud_registered", 1);
        // 发布当前帧原始点云配准之后的点云
        pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/cloud_registered_raw", 1);
 
        // 设置每个体素的大小，分别为长宽高
        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
 
        // 初始化变量
        allocateMemory();
    }

构造函数，设置了阈值的数值，然后下面的话就是订阅和发布的内容，一个advertise 一个subscribe。当然subscribe里面有函数句柄，在调用这个函数的时候，这个句柄就会自动启动，然后它们做的事情，下面来讲解。然后设置体素大小，这个没啥意思，我的理解的话还是你先搞个正方体，然后把装在正方体里面的点云用一个点来表示，这样实现降采样。开始第一个函数，分配内存。

// 分配内存
    void allocateMemory()
    {
        // 对定义的一系列关键帧变量进行初始化
        // reset 括号里面的内容 和前面的类型要对齐
        cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
        cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
 
        kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
        kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
 
        laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
        laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
        laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
        laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
 
        laserCloudOri.reset(new pcl::PointCloud<PointType>());
        coeffSel.reset(new pcl::PointCloud<PointType>()); // 这个是系数选择，现在还不清楚是啥系数
 
        laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
        // 这里的N_SCAN 我的理解是横着的扫描线 总共有16条，然后运行代码的时候，可以找一个树的点云，然后发现他是一层一层的，感觉是验证了这个N_SCAN是横着的
 
        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false); // 这里应该是都填充为false
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
 
        laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
 
        kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
        kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
 
        latestKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
        nearHistoryKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
 
        for (int i = 0; i < 6; ++i)
        {
            transformTobeMapped[i] = 0;
        }
 
        matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
    }

这个没啥好说的 reset resize fill 等函数操作 都是给这个板块里的变量在清除里面可能存在的数值。哲理的话，我对N_SCAN * Horizon_SCAN的理解，就是行×列，这个雷达的扫描线就是一行行的，横着扫的。这个是我的理解，同时这个的话，也是转为一维坐标的关键，知道他是按行来一个个存储的就ok。还有这个fill函数，第一次遇到，一看名字就知道他是干啥的，但是里面具体的逻辑，再读一遍的时候，细细研究。这里没啥好说的，就后面括号里面的内容 类型，和前面的变量对齐就可以了。

    // 订阅激光帧点云信息 这个就是构造函数里面订阅函数里面的一个参数，就是一用到构造函数的那个变量，这个就开始启动了
    void laserCloudInfoHandler(const lvi_sam::cloud_infoConstPtr& msgIn)
    {
        // extract time stamp
        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec(); // 这两个都是一样的内容，不过形式不一样而已，一个是转换成秒的，上面那个是没转换的
 
        // extract info ana(and 我猜测的) feature cloud 提取当前激光帧角点、平面点集合
        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;
        // mapping执行频率控制
        // 如果当前激光帧与上一帧激光帧之间的差值大于某个值
        // mappingProcessInterval在utility.h中被初始化为0.15
        if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
        {
            // 更新timeLastProcessing的值
            timeLastProcessing = timeLaserInfoCur;
 
            // 当前帧初始化
            // 1.如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
            // 2.后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
            // 我的理解：要不使用vins来更新，要不使用imu来更新，然后一系列的转换矩阵相乘，得到坐标
            updateInitialGuess();
 
            // 提取局部角点、平面点云集合，加入局部地图
            // 1.对最近的一帧关键帧，搜索时空纬度上相邻的关键帧集合，降采样一下
            // 2.对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部地图中
            // 先nearby 再extractcloud 最后结束，这个就是对周围的关键帧做一个处理
            extractSurroundingKeyFrames();
 
            // 当前激光帧角点、平面点集合降采样
            // 重复了两边的降采样操作，这个没啥多解析的地方
            downsampleCurrentScan();
 
            // 1、要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
            
            // 2、迭代30次优化
 
            // 1）当前激光帧角点寻找局部map匹配点
            // a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线，
            //  则认为匹配上了（用距离中心点的协方差矩阵，特征值进行判断）
            // b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
 
            // 2）当前激光帧平面点寻找局部map匹配点
            // a.更新当前帧位姿，将当前帧平面点坐标交换到map系下，在局部map中查找5个最近点，距离小于1米，且5个点构成平面
            // （最小二乘拟合平面），则认为匹配上了
            // b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
 
            // 3）提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
 
            // 4）对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
 
            // 3、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll pirch 约束z坐标
            scan2MapOptimization();
 
            // 因子图优化，更新所有关键帧位姿
            // 设置当前帧为关键帧并执行因子图优化
            // 计算当前帧与前一帧位姿变化，如果变化太小，不设为关键帧，反之设为关键帧
            // 添加激光里程计因子、GPS因子、闭环因子
            // 执行因子图优化
            // 得到当前帧优化后位姿，位姿协方差
            // 添加cloudKeyPoses3D,cloudKeyPoses6D,更新transformTobeMapped,添加当前关键帧的角点、平面点集合
            // 该优化是独立于scan-map的另一个优化
            saveKeyFramesAndFactor();
 
            // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
            correctPoses();
 
            // 发布激光里程计
            publishOdometry();
 
            // 发布里程计、点云、轨迹
            // 1、发布历史关键帧位姿集合
            // 2、发布局部地图降采样平面点集合
            // 3、发布历史帧（累加的）的角点、平面点降采样集合
            // 4、发布里程计轨迹
            publishFrames();
        }
    }

雷达点云的句柄，当然是subcribe雷达点云的时候启动的，一开始获取时间，然后从ROS作用域下获得点云的数据，然后上锁，下面的话，就是一系列函数，对于这些函数能干啥，都在代码里面注释了。当然这是学习了网上各路神仙以后总结下来的代码。

laserCloudInfoHandler：

1、updateInitialGuess();首先初始化 这个Guess可以想成高斯分布，完了这个我不确定，反正是初始化

2、extractSurroundingKeyFrames();这个提取周围的关键帧

3、downsampleCurrentScan(); 降采样，就是上一步可能提取的数据太多，计算机一下处理不了这么多

4、scan2MapOptimization(); 这个部分还不是很理解，大体内容的话就是cornerOptimization、surfOptimization然后combineOptimization，里面还有LMOptimization，这些优化的内容，前面的话感觉就是要找到角点和平面点，然后你算出来个coeff用在后面的优化中，后面两个的话就是不断地优化，然后他最后判断一个delta啥东西的，是两个！然后不停地迭代优化，找到最优解。就是里面的话一些变量不是很明白，我推测，应该论文原文会有，或者是一些基础的知识，但是我没看到。

5、saveKeyFramesAndFactor();这部分的话就是添加因子，然后保存关键帧的一些操作

6、correctPoses();这部分的话，没看懂的是aLoopIsClosed是啥意思，然后这个东西在if条件中用到，这个地方不是很理解，这个模块的话就是在修正位姿。

7、publishOdometry(); 发布激光里程计

8、publishFrames(); 发布帧 这两个的话可以具体到函数里面看看它到底发布了啥，这里不多讲了

    void gpsHandler(const nav_msgs::Odometry::ConstPtr& gpsMsg)
    {
        std::lock_guard<std::mutex> lock(mtx);
        gpsQueue.push_back(*gpsMsg);
    }
 
    void pointAssociateToMap(PointType const * const pi, PointType * const po)
    {
        po->x = transPointAssociateToMap(0,0) * pi->x 
                +transPointAssociateToMap(0,1) * pi->y 
                +transPointAssociateToMap(0,2) * pi->z 
                +transPointAssociateToMap(0,3);
        
        po->y = transPointAssociateToMap(1,0) * pi->x 
                +transPointAssociateToMap(1,1) * pi->y 
                +transPointAssociateToMap(1,2) * pi->z 
                +transPointAssociateToMap(1,3);
        
        po->z = transPointAssociateToMap(2,0) * pi->x 
                +transPointAssociateToMap(2,1) * pi->y 
                +transPointAssociateToMap(2,2) * pi->z 
                +transPointAssociateToMap(2,3);
        
        po->intensity = pi->intensity;
    }
 
    pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn)
    {
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
 
        PointType *pointFrom;
 
        int cloudSize = cloudIn->size();
        cloudOut->resize(cloudSize);
 
        Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
        
        for (int i = 0; i < cloudSize; ++i)
        {
            pointFrom = &cloudIn->points[i];
            cloudOut->points[i].x = transCur(0,0) * pointFrom->x 
                                    +transCur(0,1) * pointFrom->y 
                                    +transCur(0,2) * pointFrom->z 
                                    +transCur(0,3);
            
            cloudOut->points[i].y = transCur(1,0) * pointFrom->x 
                                    +transCur(1,1) * pointFrom->y 
                                    +transCur(1,2) * pointFrom->z 
                                    +transCur(1,3);
            
            cloudOut->points[i].z = transCur(2,0) * pointFrom->x 
                                    +transCur(2,1) * pointFrom->y 
                                    +transCur(2,2) * pointFrom->z 
                                    +transCur(2,3);
            
            cloudOut->points[i].intensity = pointFrom->intensity;
        }
        return cloudOut;
    }
 
    gtsam::Pose3 affine3fTogtsamPose3(const Eigen::Affine3f& thisPose)
    {
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(thisPose, x, y, z, roll, pitch, yaw);
        return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(roll), double(pitch), double(yaw)),
                                  gtsam::Point3(double(x),    double(y),     double(z)));
    }
 
    gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
    {
        return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
                                  gtsam::Point3(double(thisPoint.x),    double(thisPoint.y),     double(thisPoint.z)));
    }
 
    gtsam::Pose3 trans2gtsamPose(float transformIn[])
    {
        return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]), 
                                  gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
    }
 
    Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
    { 
        return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
    }
 
    Eigen::Affine3f trans2Affine3f(float transformIn[])
    {
        return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
    }
 
    PointTypePose trans2PointTypePose(float transformIn[])
    {
        PointTypePose thisPose6D;
        thisPose6D.x = transformIn[3];
        thisPose6D.y = transformIn[4];
        thisPose6D.z = transformIn[5];
        thisPose6D.roll  = transformIn[0];
        thisPose6D.pitch = transformIn[1];
        thisPose6D.yaw   = transformIn[2];
        return thisPose6D;
    }

一些转换的函数，可以看看里面到底谁赋值给了谁，这里我还没看，再看一遍的时候，再来注意这个地方。

    void visualizeGlobalMapThread()
    {
        ros::Rate rate(0.2); // 调整ros系统运行的快慢的
        while (ros::ok())
        {
            rate.sleep();
            publishGlobalMap();
        }
 
        if (savePCD == false)
            return;
 
        cout << "****************************************************" << endl;
        cout << "Saving map to pcd files ..." << endl;
        // create directory and remove old files;
        savePCDDirectory = std::getenv("HOME") + savePCDDirectory;
        int unused = system((std::string("exec rm -r ") + savePCDDirectory).c_str());
        unused = system((std::string("mkdir ") + savePCDDirectory).c_str()); ++unused;
        // save key frame transformations
        pcl::io::savePCDFileASCII(savePCDDirectory + "trajectory.pcd", *cloudKeyPoses3D);
        pcl::io::savePCDFileBinary(savePCDDirectory + "transformations.pcd", *cloudKeyPoses6D);
        // extract global point cloud map        
        pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
        for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) 
        {
            // clip cloud
            pcl::PointCloud<PointType>::Ptr cornerTemp(new pcl::PointCloud<PointType>());
            pcl::PointCloud<PointType>::Ptr cornerTemp2(new pcl::PointCloud<PointType>());
            *cornerTemp = *transformPointCloud(cornerCloudKeyFrames[i],  &cloudKeyPoses6D->points[i]);
            for (int j = 0; j < (int)cornerTemp->size(); ++j)
            {
                if (cornerTemp->points[j].z > cloudKeyPoses6D->points[i].z && cornerTemp->points[j].z < cloudKeyPoses6D->points[i].z + 5)
                    cornerTemp2->push_back(cornerTemp->points[j]);
            }
            pcl::PointCloud<PointType>::Ptr surfTemp(new pcl::PointCloud<PointType>());
            pcl::PointCloud<PointType>::Ptr surfTemp2(new pcl::PointCloud<PointType>());
            *surfTemp = *transformPointCloud(surfCloudKeyFrames[i],  &cloudKeyPoses6D->points[i]);
            for (int j = 0; j < (int)surfTemp->size(); ++j)
            {
                if (surfTemp->points[j].z > cloudKeyPoses6D->points[i].z && surfTemp->points[j].z < cloudKeyPoses6D->points[i].z + 5)
                    surfTemp2->push_back(surfTemp->points[j]);
            }
            *globalCornerCloud += *cornerTemp2;
            *globalSurfCloud   += *surfTemp2;
 
            // origin cloud
            *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i],  &cloudKeyPoses6D->points[i]);
            *globalSurfCloud   += *transformPointCloud(surfCloudKeyFrames[i],    &cloudKeyPoses6D->points[i]);
            cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
        }
        // down-sample and save corner cloud
        downSizeFilterCorner.setInputCloud(globalCornerCloud);
        pcl::io::savePCDFileASCII(savePCDDirectory + "cloudCorner.pcd", *globalCornerCloud);
        // down-sample and save surf cloud
        downSizeFilterSurf.setInputCloud(globalSurfCloud);
        pcl::io::savePCDFileASCII(savePCDDirectory + "cloudSurf.pcd", *globalSurfCloud);
        // down-sample and save global point cloud map
        *globalMapCloud += *globalCornerCloud;
        *globalMapCloud += *globalSurfCloud;
        downSizeFilterSurf.setInputCloud(globalMapCloud);
        pcl::io::savePCDFileASCII(savePCDDirectory + "cloudGlobal.pcd", *globalMapCloud);
        cout << "****************************************************" << endl;
        cout << "Saving map to pcd files completed" << endl;
    }
 
    void publishGlobalMap()
    {
        if (pubLaserCloudSurround.getNumSubscribers() == 0)
            return;
 
        if (cloudKeyPoses3D->points.empty() == true)
            return;
 
        pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;
        pcl::PointCloud<PointType> ::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType> ::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType> ::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType> ::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());
 
        // kd-tree to find near key frames to visualize
        std::vector<int> pointSearchIndGlobalMap;
        std::vector<float> pointSearchSqDisGlobalMap;
        // search near key frames to visualize
        mtx.lock();
        kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
        kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
        mtx.unlock();
 
        for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
            globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
        // downsample near selected key frames
        pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
        downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
        downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
        downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
 
        // extract visualized and downsampled key frames
        for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i){
            if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
                continue;
            int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
            *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
            *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
        }
        // downsample visualized points
        pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
        downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
        publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, "odom");    
    }
 
    void loopHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg)
    {
        // control loop closure frequency
        static double last_loop_closure_time = -1;
        {
            std::lock_guard<std::mutex> lock(mtx);
            if (timeLaserInfoCur - last_loop_closure_time < 5.0)
                return;
            else
                last_loop_closure_time = timeLaserInfoCur;
        }
 
        performLoopClosure(*loopMsg);
    }
 
    void performLoopClosure(const std_msgs::Float64MultiArray& loopMsg)
    {
        pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D(new pcl::PointCloud<PointTypePose>());
        {
            std::lock_guard<std::mutex> lock(mtx);
            *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
        }
 
        // get lidar keyframe id
        int key_cur = -1; // latest lidar keyframe id
        int key_pre = -1; // previous lidar keyframe id
        {
            loopFindKey(loopMsg, copy_cloudKeyPoses6D, key_cur, key_pre);
            if (key_cur == -1 || key_pre == -1 || key_cur == key_pre) || abs(key_cur - key_pre) < 25)
                return;
        }
 
        // check if loop added before
        {
            // if image loop closure comes at high frequency, many image loop may point to the same key_cur
            auto it = loopIndexContainer.find(key_cur);
            if (it != loopIndexContainer.end())
                return;
        }
        
        // get lidar keyframe cloud
        pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
        {
            loopFindNearKeyframes(copy_cloudKeyPoses6D, cureKeyframeCloud, key_cur, 0);
            loopFindNearKeyframes(copy_cloudKeyPoses6D, prevKeyframeCloud, key_pre, historyKeyframeSearchNum);
            if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
                return;
            if (pubHistoryKeyFrames.getNumSubscribers() != 0)
                publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, "odom");
        }
 
        // get keyframe pose
        Eigen::Affine3f pose_cur;
        Eigen::Affine3f pose_pre;
        Eigen::Affine3f pose_diff_t; // serves as initial guess
        {
            pose_cur = pclPointToAffine3f(copy_cloudKeyPoses6D->points[key_cur]);
            pose_pre = pclPointToAffine3f(copy_cloudKeyPoses6D->points[key_pre]);
 
            Eigen::Vector3f t_diff;
            t_diff.x() = - (pose_cur.translation().x() - pose_pre.translation().x());
            t_diff.y() = - (pose_cur.translation().y() - pose_pre.translation().y());
            t_diff.z() = - (pose_cur.translation().z() - pose_pre.translation().z());
            if (t_diff.norm() < historyKeyframeSearchRadius)
                t_diff.setZero();
            pose_diff_t = pcl::getTransformation(t_diff.x(), t_diff.y(), t_diff.z(), 0, 0, 0);
        }
 
        // transform and rotate cloud for matching
        pcl::IterativeClosestPoint<PointType, PointType> icp;
        // pcl::GeneralizedIterativeClosestPoint<PointType, PointType> icp;
        icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius * 2);
        icp.setMaximumIterations(100);
        icp.setRANSACIterations(0);
        icp.setTransformationEpsilon(1e-3);
        icp.setEuclideanFitnessEpsilon(1e-3);
 
        // initial guess cloud
        pcl::PointCloud<PointType>::Ptr cureKeyframeCloud_new(new pcl::PointCloud<PointType>());
        pcl::transformPointCloud(*cureKeyframeCloud, *cureKeyframeCloud_new, pose_diff_t);
 
        // match using icp
        icp.setInputSource(cureKeyframeCloud_new);
        icp.setInputTarget(prevKeyframeCloud);
        pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
        icp.align(*unused_result);
 
        if (pubIcpKeyFrames.getNumSubscribers() != 0)
        {
            pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
            pcl::transformPointCloud(*cureKeyframeCloud_new, *closed_cloud, icp.getFinalTransformation());
            publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, "odom");
        }
 
        // add graph factor
        if (icp.getFitnessScore() < historyKeyframeFitnessScore && icp.hasConverged() == true)
        {
            // get gtsam pose
            gtsam::Pose3 poseFrom = affine3fTogtsamPose3(Eigen::Affine3f(icp.getFinalTransformation()) * pose_diff_t * pose_cur);
            gtsam::Pose3 poseTo   = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[key_pre]);
            // get noise
            float noise = icp.getFitnessScore();
            gtsam::Vector Vector6(6);
            Vector6 << noise, noise, noise, noise, noise, noise;
            noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
            // save pose constraint
            mtx.lock();
            loopIndexQueue.push_back(make_pair(key_cur, key_pre));
            loopPoseQueue.push_back(poseFrom.between(poseTo));
            loopNoiseQueue.push_back(constraintNoise);
            mtx.unlock();
            // add loop pair to container
            loopIndexContainer[key_cur] = key_pre;
        }
 
        // visualize loop constraints
        if (!loopIndexContainer.empty())
        {
            visualization_msgs::MarkerArray markerArray;
            // loop nodes
            visualization_msgs::Marker markerNode;
            markerNode.header.frame_id = "odom";
            markerNode.header.stamp = timeLaserInfoStamp;
            markerNode.action = visualization_msgs::Marker::ADD;
            markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
            markerNode.ns = "loop_nodes";
            markerNode.id = 0;
            markerNode.pose.orientation.w = 1;
            markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3; 
            markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1;
            markerNode.color.a = 1;
            // loop edges
            visualization_msgs::Marker markerEdge;
            markerEdge.header.frame_id = "odom";
            markerEdge.header.stamp = timeLaserInfoStamp;
            markerEdge.action = visualization_msgs::Marker::ADD;
            markerEdge.type = visualization_msgs::Marker::LINE_LIST;
            markerEdge.ns = "loop_edges";
            markerEdge.id = 1;
            markerEdge.pose.orientation.w = 1;
            markerEdge.scale.x = 0.1;
            markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;
            markerEdge.color.a = 1;
 
            for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
            {
                int key_cur = it->first;
                int key_pre = it->second;
                geometry_msgs::Point p;
                p.x = copy_cloudKeyPoses6D->points[key_cur].x;
                p.y = copy_cloudKeyPoses6D->points[key_cur].y;
                p.z = copy_cloudKeyPoses6D->points[key_cur].z;
                markerNode.points.push_back(p);
                markerEdge.points.push_back(p);
                p.x = copy_cloudKeyPoses6D->points[key_pre].x;
                p.y = copy_cloudKeyPoses6D->points[key_pre].y;
                p.z = copy_cloudKeyPoses6D->points[key_pre].z;
                markerNode.points.push_back(p);
                markerEdge.points.push_back(p);
            }
 
            markerArray.markers.push_back(markerNode);
            markerArray.markers.push_back(markerEdge);
            pubLoopConstraintEdge.publish(markerArray);
        }
    }
 
    void loopFindNearKeyframes(const pcl::PointCloud<PointTypePose>::Ptr& copy_cloudKeyPoses6D,
                               pcl::PointCloud<PointType>::Ptr& nearKeyframes, 
                               const int& key, const int& searchNum)
    {
        // extract near keyframes
        nearKeyframes->clear();
        int cloudSize = copy_cloudKeyPoses6D->size();
        for (int i = -searchNum; i <= searchNum; ++i)
        {
            int key_near = key + i;
            if (key_near < 0 || key_near >= cloudSize )
                continue;
            *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[key_near], &copy_cloudKeyPoses6D->points[key_near]);
            *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[key_near],   &copy_cloudKeyPoses6D->points[key_near]);
        }
 
        if (nearKeyframes->empty())
            return;
 
        // downsample near keyframes
        pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
        downSizeFilterICP.setInputCloud(nearKeyframes);
        downSizeFilterICP.filter(*cloud_temp);
        *nearKeyframes = *cloud_temp;
    }
 
    void loopFindKey(const std_msgs::Float64MultiArray& loopMsg, 
                     const pcl::PointCloud<PointTypePose>::Ptr& copy_cloudKeyPoses6D,
                     int& key_cur, int& key_pre)
    {
        if (loopMsg.data.size() != 2)
            return;
 
        double loop_time_cur = loopMsg.data[0];
        double loop_time_pre = loopMsg.data[1];
 
        if (abs(loop_time_cur - loop_time_pre) < historyKeyframeSearchTimeDiff)
            return;
 
        int cloudSize = copy_cloudKeyPoses6D->size();
        if (cloudSize < 2)
            return;
 
        // latest key
        key_cur = cloudSize - 1;
        for (int i = cloudSize - 1; i >= 0; --i)
        {
            if (copy_cloudKeyPoses6D->points[i].time > loop_time_cur)
                key_cur = round(copy_cloudKeyPoses6D->points[i].intensity);
            else
                break;
        }
 
        // previous key
        key_pre = 0;
        for (int i = 0; i < cloudSize; ++i)
        {
            if (copy_cloudKeyPoses6D->points[i].time < loop_time_pre)
                key_pre = round(copy_cloudKeyPoses6D->points[i].intensity);
            else
                break;
        }
    }
 
    void loopClosureThread()
    {
        if (loopClosureEnableFlag == false)
            return;
 
        ros::Rate rate(0.5);
        while (ros::ok())
        {
            rate.sleep();
            performLoopClosureDetection();
        }
    }
 
    void performLoopClosureDetection()
    {
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
 
        int key_cur = -1;
        int key_pre = -1;
 
        double loop_time_cur = -1;
        double loop_time_pre = -1;
 
        // find latest key and time 对于latest的理解就是最新的那个，就是当前的，感觉差不多
        {
            std::lock_guard<std::mutex> lock(mtx); // 先上锁
 
            if (cloudKeyPoses3D->empty())
                return;
            
            kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
            kdtreeHistoryKeyPoses->radiusSearch(cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
 
            key_cur = cloudKeyPoses3D->size() - 1;
            loop_time_cur = cloudKeyPoses6D->points[key_cur].time;
        }
 
        // find previous key and time 从小到大去循环
        {
            for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
            {
                int id = pointSearchIndLoop[i];
                if (abs(cloudKeyPoses6D->points[id].time - loop_time_cur) > historyKeyframeSearchTimeDiff)
                {
                    key_pre = id;
                    loop_time_pre = cloudKeyPoses6D->points[key_pre].time;
                    break;
                }
            }
        }
 
        // 下面if条件中 都是不正常的情况
        if (key_cur == -1 || key_pre == -1 || key_pre == key_cur || loop_time_cur < 0 || loop_time_pre < 0)
            return;
 
        std_msgs::Float64MultiArray match_msg;
        match_msg.data.push_back(loop_time_cur);
        match_msg.data.push_back(loop_time_pre);
        performLoopClosure(match_msg);
    }

上面是一些关于线程的内容，这个是在最下面的main函数里面出现的，这个main函数相比其他的话，多了两个线程。

    void updateInitialGuess()
    {        
        // 前一帧的位姿，这里指lidar的位姿
        // Affine3f 仿射变换矩阵（移向量 + 旋转变换组合而成，可以同时实现旋转，缩放，平移等空间变换）
        static Eigen::Affine3f lastImuTransformation;
        // system initialization 系统初始化
        // 如果关键帧集合为空，继续进行初始化
        if (cloudKeyPoses3D->points.empty())
        {
            // 大小为6的数组，当前帧位姿的旋转部分，用激光帧信息中的RPY（来自IMU原始数据）初始化
            // 这里前三个变量分别记录了roll pitch yaw三个初始化的值
            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;
        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
                // 这里的0 1 2位置代表了RPY 3 4 5 位置代表了xyz，这里的顺序换了一下 小细节
                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 (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;
        }
    }

一个初始化的操作，没啥好讲的

    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就是点云里的最后一个点
        // 这个函数的第一个参数 传入一个点在kd树中，搜索这个所有到这个点的距离小于某个值的点
        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 同时提取一些最新的关键帧，防止机器人在一个位置旋转
        // 加入时间上相邻的一些关键帧，比如当载体在原地转圈，这些帧加进来是合理的
        // Question : 原地旋转为什么不算作空间上相邻的关键帧
        /*
        暂时的答案（3.7）
        3D点云集合中只保存位置信息（xyz），6D点云中保存位置 + 旋转信息（xyz + rpy）
        当原地旋转时，关键帧的xyz应该是保持不变的，所以连续的若干帧会在3D点云集合中只保存一个坐标（是不是这样需要后续看cloudKeyPose3D是怎么添加帧的）
        所以使用3D点云集合搜索空间领域时，会丢失旋转前后的帧（xyz相同，rpy不同），所以需要重新对事件领域进行添加
        这便解释下面if语句中使用的是6D的时间戳
        */
        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;
        }
 
        // 将相邻关键帧集合对应的角点、平面点，加入到局部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) //并行编程
        // #pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {
            // 相邻关键帧索引
            // intensity也可以放索引：index
            int thisKeyInd = (int)cloudToExtract->points[i].intensity;
            // 提取50米范围内的点，transformPointCloud作用是返回输入点云乘以输入的transform
            // 自定义函数，计算两个点之间的距离
            if (pointDistance(cloudKeyPoses3D->points[thisKeyInd], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
                continue;
            // 相邻关键帧对应的角点、平面点云，通过6D位姿变换到世界坐标系下
            laserCloudCornerSurroundingVec[i]  = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
            laserCloudSurfSurroundingVec[i]    = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
        }
 
        // fuse the map
        // 赋值线和面特征点击，并且进行下采样
        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) 降采样 DS后缀downsample
        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        // Downsample the surrounding surf key frames (or map)
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
        // downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        // downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
    }
    void extractSurroundingKeyFrames()
    {
        // 关键帧集合为空，直接返回
        if (cloudKeyPoses3D->points.empty() == true)
            return; 
        
        extractNearby();
    }