LIO-SAM源码解析（五）：mapOptmization.cpp

瞻邈

已于 2024-06-08 18:07:22 修改

阅读量2.6k

点赞数 19

分类专栏： # 激光SLAM 文章标签：人工智能算法机器人自动驾驶

于 2024-05-21 12:56:38 首次发布

本文链接：https://blog.csdn.net/xhtchina/article/details/128160103

版权

1. 代码流程

1.1. extractSurroundingKeyFrames()

1.2. scan2MapOptimization()

这个函数主要就是进行帧到地图的匹配，通过点到面、点到线的距离距离最小作为优化目标。

LOAM中雅阁比矩阵推导其实还是过于复杂了，可以使用进行误差扰动来计算雅阁比矩阵，姿态使用失准角（李代数）模型推导更为简单

1.3. saveKeyFramesAndFactor()

在这个函数一直维持着一个关键帧节点构建的图，将GPS位置、关键帧间相对位姿、闭环关键帧间相对位姿（两帧的点云匹配得到）作为观测值，对关键帧位姿进行优化，在没有GPS情况下，精度主要靠回环。

利用当前关键帧scan2map匹配位姿和上个关键帧优化后的位姿（这一次优化的初值）来计算关键帧间相对位姿观测量，实际上这个观测量对位姿起不到优化的作用。

2. 功能说明

2.1. 回调函数：lasercloudinfoHandler

这个函数监听的是/feature/cloud_info，关于这个话题里面包含的内容，我已经在featureExtraction.cpp的总结部分说过了。这里回忆一下，cloud_info是作者自定义的一个特殊的msg类型，包含了当前激光帧的角点集合，平面点集合，一维数组，每根线开始的点和结束点在一维数组中的索引。

那么收到数据，先保存时间戳，然后提取已经在featureExtraction.cpp中被提取的角点和平面点信息，保存到*laserCloudCornerLast和*laserCloudSurfLast中。请记住这两个命名。

频率控制：当前时刻-上一时刻>=0.15s时，才开始调用下列函数：

updateInitialGuess();

extractSurroundingKeyFrames();

downsampleCurrentScan();

scan2MapOptimization();

saveKeyFramesAndFactor();

correctPoses();

publishOdometry();

publishFrames();

2.2. UpdateInitGuess：当前帧初始化

当关键帧集合为空时，用激光帧的imu原始数据角度数据初始化，并且把数据用lastImuTransformation保存（记住这个命名），返回。

假如cloudInfo.odomAvailable=true时，那么就用一个transBack来记录cloudInfo.initialGuessX等信息，（这个信息其实来自于imupreintegration.cpp中发布的imu里程计数据），然后记录增量，在之前系统状态transformTobeMapped的基础上，加上这个增量，再次存入transformTobeMapped。注意这个transformTobeMapped，这个数据结构，在这个cpp里，我们可以理解为就是它在存储着激光里程计部分的系统状态，包括位置与姿态。

注意，这里有一个lastImuPreTransformation，这个用来保存上一时刻imu里程计的数据，根据它和当前的imu里程计来算增量。不要和lastImuTransformation变量混起来，虽然这俩变量名字长的很像。

然后覆盖lastImuTransformation（在这个case里没用到），返回；

假如cloudInfo.imuAvailable=true，那么进入这个case：

注意，lastImuTransformation在1.1.1和1.1.2中并未用到，只是不断的在替换成最新数据。当cloudInfo.odomAvailable一直是true的时候，程序压根也不会进入到这个case。

但是，凡事总有例外，万一哪里没有衔接好，imu里程计没有及时算出来，那么就导致此时激光帧的位姿没有一个初始化的数据（我们之后是要在初始化的基础上进行优化的），那么之后的优化就无从进行。因此，就要用到这个case。

这里主要思路是用imuRollInit数据来初始化，如果你回顾过imageProjection.cpp部分的总结3.2 那么你应该就会懂，这里的数据来源是原始imu数据的角度信息。那么如果这里有数据，就用lastImuTransformation当成最新的上一时刻数据，当前数据transBack和它算一个增量，然后累积到系统值transformTobeMapped上面去。最后更新覆盖lastImuTransformation，返回。

2.3. extractSurroundingKeyFrames

如果没有关键帧，那就算了，返回；

如果有，就调用extractNearby函数。

关键帧是啥？cloudKeyPoses3D，我们要记住这个变量，虽然到现在为止，我们还不知道它是怎么来的，但是这个东西是怎么获取的，我们在后续必须弄明白。

在这里我先剧透一下：它里面装的是历史的“关键帧”的位置，即x，y，z三个值。需要明确：这里装的绝不是历史的关键帧位置处的点云。而是历史关键帧记录时刻机器人所在的位置。

同理还有一个cloudKeyPoses6D，它比这个3D还多了三个角度信息。之所以要用一个6D一个3D分别来装关键帧，我现在直接揭晓答案：用3D装，是因为我们要根据这个来构建KD树，搜索历史上最近的节点。“最近”指的是距离上最近，即xyz空间坐标最近，和角度无关。而cloudKeyPoses6D，是用来投影点云的，把当前帧点云投影到世界坐标系下，那么投影就必须要用角度信息了，所以作者分别用了一个3D和一个6D来装数据。

2.4. extractNearby函数

使用kd树，搜索当前最近的关键帧，在给定半径内（默认是50m）所有关键帧最近的位置，并把结果返回到pointSearchInd，把距离返回到pointSearchSqDis中。

根据索引pointSearchInd，把相邻关键帧存入到surroundingKeyPoses中。

下采样，装进surroundingKeyPosesDS中，并在原始的surroundingKeyPoses其中找到最近的一个点，替换掉索引。（关于这个，我的理解是，下采样后不太准确了，好几个不同的关键帧可能因为下采样的原因混成了一个，所以要用原始数据对索引进行一个修正，这样以后才方便根据索引投影点云）

顺手把10s内的关键帧cloudKeyPoses3D中的位置也加入到surroundingKeyPosesDS中。

extractCloud：提取边缘和平面点对应的localmap，把surroundingKeyPosesDS传入到函数中：

对输入的surroundingKeyPosesDS进行遍历，找到50m之内的位置，然后用transformPointCloud把对应位置的点云，进行变换到世界坐标系下。

如何变换呢？根据上面提到的cloudKeyPoses6D的位姿，然后把cornerCloudKeyFrame和surfCloudKeyFrame中根据索引找到点云，投影到世界坐标系下。

那么在这里，cornerCloudKeyFrame和surfCloudKeyFrame是什么？之前从来没有出现过。我这里同样进行剧透，它里面存放的是下采样的点云。注意总结1中的*laserCloudCornerLast和*laserCloudSurfLast这两个东西，这是瞬时点云，这个东西会在之后被下采样，然后装入cornerCloudKeyFrame中。

在角点点云和平面点点云被投影到世界坐标系下后，会被加入到laserCloudCornerFromMap和laserCloudSurfFromMap等数据结构中，然后再合出一个pair类型的Container<关键帧号，<角点集合，平面点集合>>。

2.5. downsampleCurrentScan

这部分比较简单，就是对最外层的回调函数中的laserCloudCornerLast之类的东西，进行一个下采样，保存到laserCloudCornerLastDS这些以DS结尾的数据结构中，并且把数目存到laserCloudCornerLastDSNum这种以DSNUM结尾的数据结构中。其实就是代表了当前帧点云的角点/平面点的下采样集合，和数目值。

2.6. Scan2MapOptimization

首先，没有关键帧保存，那就返回，不处理；

如果DSNUM这种记录角点和平面点的数据结构中，发现数目不够多，也不处理；只有在数目足够多的时候才进行处理，默认最少要10个角点，100个平面点。

迭代30次：

边缘点匹配优化：CornerOptimization

平面点匹配优化：SurfOptimization

组合优化多项式系数：combineOptimizationCoeffs

LMOptimization：判断迭代误差是否足够小，如果是true则认为迭代完成，返回；

transformUpdate：原始的imu的rpy，在这里和优化后的激光里程计位姿进行一个加权融合。

接下来，我们依次展开这些函数：

2.7. 边缘点匹配优化：CornerOptimization

把系统状态transformTobeMapped做一个数据格式转换，变成transPointAssociateToMap形式
从当前角点下采样集合laserCloudCornerLastDS进行遍历，找到世界坐标系下最近的5个点，要求小于1m。

求5个样本的均值，协方差矩阵。对协方差矩阵进行特征值分解，如果最大特征值大于次大特征值的3倍，那么就认为构成线。

一旦发现构成线，那么就在均值沿着最大特征向量方向（把它看成线的方向）前后各取一个点（+-0.1 x 方向）。

X0为当前点，X1和X2为“X0附近的5个点一起算出的均值沿方向前后各取的一点”，叉乘计算三点面积a012，x1x2底边长度l12。然后再做一次叉乘，得到X0距离x1，x2连线的垂线段的单位方向向量（la，lb，lc）。并计算点到直线的距离ld2=a012/l12。

用一个鲁棒和函数，使得距离ld2越大，s越小。然后用coeff来保存“鲁棒后”的距离，和“鲁棒后”的点到线的垂线的方向向量。

如果点到直线的距离小于1m，那么存入数据结构，laserCloudOriCornerVec为原始点云，coeffSelCornerVec为鲁棒距离和鲁棒向量，laserCloudOriCornerFlag代表当前点X0 已经找到对应的线。

思考：为什么要加入方向向量呢？是因为这个在优化的偏导数中会被用到。

2.8. 平面点匹配优化：SurfOptimization

和上面的同理，对系统状态量transformTobeMapped进行数据格式转换；

从当前角点下采样集合laserCloudSurfLastDS进行遍历，找到世界坐标系下最近的5个点，要求小于1m。

直接用matA0存储这个5个点，求解Ax+By+Cz+1=0的ABC系数（用QR分解），然后对ABCD，代码中为pa，pb，pc，pd=1进行单位化。

根据点x0到平面的距离 $d=\frac{|Ax_0+By_0+Cz_0+D|}{\sqrt{A^2+B^2+C^2}}$ (分母为1）判断是否构成平面。如果有一个大于0.2m则不构成。

pd2为点到平面的距离，也用鲁棒和函数处理，并且比上两次开方（这点我不理解，我猜就是用来鲁棒的，换成1次开方可能也差不多，意义或许不大），然后和角点部分类似，得到s，存入数据结构。

2.9. 组合优化多项式系数：combineOptimizationCoeffs

这个比较简单，就是把CornerOptimization和SurfOptimization中已经确定匹配关系的点提取出来，laserCloudOri统一把角点和平面点装在一起，coeffSel统一装之前计算得到的“鲁棒优化向量”（角点就是点到直线的“鲁棒垂线”，平面点就是点到平面的“鲁棒法线”）。

优化向量会在LMOptimization中进行优化。

LMOptimization判断迭代误差是否足够小，如果是true则认为迭代完成，返回。

构建JtJ*delt_x=-JTf，然后构建MatAtA，matAtB，利用cv:solve提供的QR分解，得到matX，即delta_x。

当特征点缺乏时，状态估计方法会发生退化。特征值小于阈值，则视为退化方向向量。这块的理论，可以参考LOAM SLAM原理之防止非线性优化解退化

更新位姿，判断收敛与否。那么真正的雷达里程计系统状态transformTobeMapped，就是在这里被更新。

2.10. transformUpdate

原始的imu的rpy，在这里和优化后的激光里程计位姿进行一个加权融合。

当imuAvailable=True的时候，并且俯仰角<1.4，那么对位姿和原始imu的rpy做一个加权平均，（权重在配置文件中可以被设置为0.01）。主要是对roll，pitch仅加权平均，并且对z进行一个高度约束（也就是clip，不得超过配置文件中的z_tollerance，这个主要是一个小trick，应对不能飞起来的无人小车用的），更新transformTobeMapped。

2.11. saveKeyFramesAndFactor

之前函数二话不说就用了一些并没有出现过的数据结构，例如什么cloudKeyPoses3D，cornerCloudKeyFrame之类的东西，看完这个函数将明白这些变量是怎么来的。

2.12. saveFrame

计算当前帧和前一帧位姿变换，如果太小不设关键帧。默认是角度<0.2，距离<1m，两者不满足其一就不保存；

2.13. addOdomFactor

这个是要加入激光里程计因子，给图优化的模型gtSAMgraph。在1.5之前别的函数里，如果没有关键帧，直接就跳过了。但是这里不能跳过。

如果暂时还没有关键帧，就把当前激光系统状态transformTobeMapped，打包成一个PriorFactor加入到gtSAMgraph里。如果目前已经有关键帧了，就把最后一个关键帧，和当前状态transformTobeMapped计算一个增量，把这个增量打包成一个BetweenFactor，加入到gtSAMgraph里头去。

initialEstimate代表变量初值，用transformTobeMapped赋值。

2.14. addGpsFactor

GPS的筛选规则为：如果没有GPS信息，没有关键帧信息，首尾关键帧小于5m，或者位姿的协方差很小（x，y的协方差阈值小于25），就不添加GPS。

否则，遍历GPS列表，当前帧0.2s以前的不要，或者GPS的协方差太大了也不要，无效数据也不要。找到正常数据，打包成一个gps_factor，加入gtSAMgraph里面。

2.15. addLoopFactor

这个其实和当前的回调函数无关，因为当前回调函数监听的是/feature/cloud_info信息，回环是由其他线程监控和检测的。那么在这里，它查询回环队列，加入回环因子，就是一个顺手的事情，反正现在要更新优化，那么查一下，如果有候选的等在那里，就顺手加入优化。如果用做饭来比喻这件事，那么另外的回环检测的线程就是相当于另一个人在备菜，这里addLoopFactor相当于是在炒菜，备好了就先炒，没有备好就算了。

gtsam正常更新。如果有回环那就多更新几次。

把cloudKeyPoses3D，cloudKeyPoses6D，分别装上信息，cloudKeyPoses3D代表关键帧的位置，cloudKeyPoses6D代表关键帧的位置+姿态，为什么要有一个3D一个6D呢？6D里不已经包含了3D信息吗？这个问题我在1.2处已经解释过了。

用优化结果更新transformTobeMapped。

cornerCloudKeyFrames，surfCloudKeyFrames装入信息，回顾一下，回调函数开头收到的点云数据为laserCloudCornerLast，laserCloudSurfLast，然后在downsampleCurrentScan处这俩信息被下采样，加上了DS后缀。在这里把它装到cornerCloudKeyFrames和surfCloudKeyFrames中。

回顾：cornerCloudKeyFrames代表关键帧位置处的角点点云，surfCloudKeyFrames代表关键帧位置处的平面点点云。这俩东西就是上面1.2处extractSurroundingKeyFrames用到的内容，cornerCloudKeyFrames通过cloudKeyPoses6D变换到世界系下，被存到laserCloudCornerFromMap里面，这个FromMap又在scan2MapOptimization函数中被设置到kdtreeCornerFromMap这个Kd树里，在cornerOptimization函数里，就是把当前帧的激光点云依据1.1的初值transformTobeMapped，变换到世界坐标系下，再用kdtreeCornerFromMap进行kd搜索，建立匹配关系，优化transformTobeMapped。

updatePath，更新里程计轨迹。把cloudKeyPoses6D传入，保存在globalPath中。不过暂时还没有进行发布。

2.16. correctPoses

如果发现回环的话，就把历史关键帧通通更新一遍。刚刚虽然更新过了，但是结果都是保存在gtsam里面的，cloudKeyPoses3D和cloudKeyPoses6D，这俩保存位置和位姿的变量仍然保留着更新前的关键帧位姿。所以就根据更新结果，把他俩更新一遍。

为什么不更新cornerCloudKeyFrames和surfCloudKeyFrames呢？因为没有必要更新，这俩存的是机器人坐标系下的点云，和机器人在世界系下的位姿是无关的。

2.17. publishOdometry

到此为止，激光里程计部分的transformTobeMapped就不再更新了。

回顾一下transformTobeMapped经历了哪些变换：在1.1部分用imu角度初值或是imu里程计初值赋值，然后在scan2mapOptimization里面根据点到线、点到面的方程进行更新，再在transformUpdate里和原始imu的rpy信息进行一个很小的加权融合（不过这一步我觉得没啥大用），最后在saveKeyFrameAndFactor里面再加入GPS因子和回环因子进行一轮优化。

最后把transformTobeMapped发布出去，其他cpp文件里，接收的“激光里程计”就是这么个东西。也就是lio_sam/mapping/odometry_incremental。

2.18 publishFrames

这个纯粹就是把乱七八糟东西都发布出去，不管有没有用。如果用户需要就可以监听它。

发布关键帧位姿集合，把cloudKeyPoses3D发布成lio_sam/mapping/trajectory

发布局部降采样平面点，把laserCloudSurfFromMapDS（历史默认50m内的点，在extractCloud中被设置），发布为lio_sam/mapping/map_local

发布当前帧的下采样角点和平面点，用优化后的激光里程计位姿transformTobeMapped投影到世界系下发布，/lio_sam/mapping/cloud_registered

发布原始点云经过配准的点云：输入的/feature/cloud_info的cloud_deskewed字段是由featureExtraction.cpp发布的，其cloud_deskewed是源于imageProjection.cpp发布的原始去畸变点云。把它发布到世界坐标系下，然后以/lio_sam/mapping/cloud_registered_raw的形式发布。

发布轨迹，把之前装好在globalPath里面但是还没有发布的轨迹发布出去，名为/lio_sam/mapping/path。

那么到现在，基本上mapOptimization.cpp的内容就结束了，但是还有一些尾巴：

2.14. gpshandle

监听GPS数据，保存到GPS队列里。

2.15. loopinfohandle

监听"lio_loop/loop_closure_detection"，订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。

2.16. loopClosureThread

这个线程在主函数里单独开了一个线程，简要说一下：

读取配置文件中是否开启回环检测。

开始无限循环performLoopClosure：

在历史帧中搜索距离关键帧最近，时间间各较远的关键帧（默认是30s以外，15m以内）没找到就返回，如果找到了，结果就放在loopKeyPre当中，loopKeyCur保存最近一个关键帧。
把最近一个关键帧的特征点提出来，放入cureKeyframeCloud里；回环候选帧前后各25帧也提取出来，放入prevKeyframeCloud里。
把prevKeyframeCloud发布出去，名为lio_sam/mapping/icp_loop_closure_history/cloud
调用pcl库的icp轮子，设定阈值，参数，用setInputSource，setInputTarget传入两个点云，用align对齐。成功阈值设定为0.3，成功则存在icp.getFinalTransformation里面。把当前关键帧的点云，用这个结果icp.getFinalTransformation，转换以后，以lio_sam/mapping/icp_loop_closure_corrected_cloud发布出去。
把当前帧的的位姿用icp.getFinalTransformation结果校正一下，把pair<当前，回环>，间隔位姿，噪声用队列存起来，等待addLoopFactor来调用，即上面的部分。

2.17. visualizeLoopClosure

这部分内容没啥好说的，就是用于rviz展示，把关键帧节点和二者的约束用点和线连起来，以lio_sam/mapping/loop_closure_constraints发布出去。

2.18. visualizeGlobalMapThread

这个主要是两块内容：

publishGlobalmap：

把当前关键帧附近1000m（默认）的关键帧找出来（其实也就是全局的了），降采样，变换到世界系下，然后发布为lio_sam/mapping/map_global.

saveMapService：

这个用来保存pcd格式的点云地图。在配置文件中可以设置开启与否，和存储位置。注意，当程序结束时，ctrl+c以后，才会启动保存任务。这个部分的代码，和发布globalmap部分的核心内容基本一致，反正就是把cornerCloudKeyFrames，surfCloudKeyFrames用cloudKeyPoses6D变换到世界系下，分别保存角点pcd和平面点pcd，以及全局（合起来）的pcd文件。

3. 代码

#include "utility.h"
#include "lio_sam/cloud_info.h"
#include "lio_sam/save_map.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>

using namespace gtsam;

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


/**
 * 6D位姿点云结构定义
*/
struct PointXYZIRPYT
{
    PCL_ADD_POINT4D     
    PCL_ADD_INTENSITY;  
    float roll;         
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
} EIGEN_ALIGN16;                    

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;


class mapOptimization : public ParamServer
{

public:

    // gtsam
    NonlinearFactorGraph gtSAMgraph;
    Values initialEstimate;
    Values optimizedEstimate;
    ISAM2 *isam;
    Values isamCurrentEstimate;
    Eigen::MatrixXd poseCovariance;

    ros::Publisher pubLaserCloudSurround;
    ros::Publisher pubLaserOdometryGlobal;
    ros::Publisher pubLaserOdometryIncremental;
    ros::Publisher pubKeyPoses;
    ros::Publisher pubPath;

    ros::Publisher pubHistoryKeyFrames;
    ros::Publisher pubIcpKeyFrames;
    ros::Publisher pubRecentKeyFrames;
    ros::Publisher pubRecentKeyFrame;
    ros::Publisher pubCloudRegisteredRaw;
    ros::Publisher pubLoopConstraintEdge;

    ros::Subscriber subCloud;
    ros::Subscriber subGPS;
    ros::Subscriber subLoop;

    ros::ServiceServer srvSaveMap;

    std::deque<nav_msgs::Odometry> gpsQueue;
    lio_sam::cloud_info cloudInfo;

    // 历史所有关键帧的角点集合（降采样）
    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 copy_cloudKeyPoses3D;
    pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;

    // 当前激光帧角点集合
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; 
    // 当前激光帧平面点集合
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; 
    // 当前激光帧角点集合，降采样，DS: DownSize
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; 
    // 当前激光帧平面点集合，降采样
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; 

    // 当前帧与局部map匹配上了的角点、平面点，加入同一集合；后面是对应点的参数
    pcl::PointCloud<PointType>::Ptr laserCloudOri;
    pcl::PointCloud<PointType>::Ptr coeffSel;

    // 当前帧与局部map匹配上了的角点、参数、标记
    std::vector<PointType> laserCloudOriCornerVec; 
    std::vector<PointType> coeffSelCornerVec;
    std::vector<bool> laserCloudOriCornerFlag;
    // 当前帧与局部map匹配上了的平面点、参数、标记
    std::vector<PointType> laserCloudOriSurfVec; 
    std::vector<PointType> coeffSelSurfVec;
    std::vector<bool> laserCloudOriSurfFlag;

    map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
    // 局部map的角点集合
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
    // 局部map的平面点集合
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
    // 局部map的角点集合，降采样
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
    // 局部map的平面点集合，降采样
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;

    // 局部关键帧构建的map点云，对应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::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;
    std::mutex mtxLoopInfo;

    bool isDegenerate = false;
    cv::Mat matP;

    // 局部map角点数量
    int laserCloudCornerFromMapDSNum = 0;
    // 局部map平面点数量
    int laserCloudSurfFromMapDSNum = 0;
    // 当前激光帧角点数量
    int laserCloudCornerLastDSNum = 0;      
    // 当前激光帧面点数量
    int laserCloudSurfLastDSNum = 0;

    bool aLoopIsClosed = false;
    map<int, int> loopIndexContainer; // from new to old
    vector<pair<int, int>> loopIndexQueue;
    vector<gtsam::Pose3> loopPoseQueue;
    vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;
    deque<std_msgs::Float64MultiArray> loopInfoVec;

    nav_msgs::Path globalPath;

    // 当前帧位姿
    Eigen::Affine3f transPointAssociateToMap;
    // 前一帧位姿
    Eigen::Affine3f incrementalOdometryAffineFront;
    // 当前帧位姿
    Eigen::Affine3f incrementalOdometryAffineBack;

    /**
     * 构造函数
    */
    mapOptimization()
    {
        // ISM2参数
        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1;
        isam = new ISAM2(parameters);

        // 发布历史关键帧里程计
        pubKeyPoses                 = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
        // 发布局部关键帧map的特征点云
        pubLaserCloudSurround       = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
        // 发布激光里程计，rviz中表现为坐标轴
        pubLaserOdometryGlobal      = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
        // 发布激光里程计，它与上面的激光里程计基本

最低0.47元/天解锁文章