devbox
花生_TL007
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

LIO-SAM_based_relocalization运行kitti回环序列并保存轨迹评估（一）——————源码的分析_liosam轨迹输出

作者：花生_TL007 | 2024-03-13 01:43:57

赞

踩

liosam轨迹输出

LIO-SAM_based_relocalization是由哈尔滨工业大学GaoChao开发的基于lio-sam改版的机器人重定位系统，依赖于先验地图信息。github链接如下：

https://github.com/Gaochao-hit/LIO-SAM_based_relocalization

LIO-SAM源码包含imageProjection、imuPreintegration、featureExtraction、mapOptmization四个cpp文件。GaoChao在此基础上增加了globalLocalize代码，并修改简化了源代码。

LIO-SAM源码的分析网络上比比皆是，但对于LIO-SAM_based_relocalization的分析确没有。笔者花费大半个月的时间分析并且测试了LIO-SAM_based_relocalization，从网上下载kitti原始数据并制作成bag包，整个过程较为曲折，踩了不少坑，特此记录。

该算法中imageProjection主要进行数据预处理，移动机器人小车在一帧lidar数据获取过程中也在不断的运动，导致lidar获取的数据不在同一个坐标系下，所以需要除去雷达帧的平移和旋转畸变，即这部分代码的作用就是把雷达旋转一周时，所有的点云都转换到同一个位姿下，也就是该帧初始时刻的lidar坐标系下；featureExtraction提取特征点云；imuPreintegration用来imu预积分。

最为核心的部分就是mapOptmization模块，代码量也是其余之和。Gao对其进行了修改和简化，貌似是去掉了回环检测的功能（笔者的bag包测试也证实了这一点）。下文详细注释了代码的过程，并增加了tum格式的轨迹输出（此处踩坑较多：

  1. #include "utility.h"
  2. #include "lio_sam/cloud_info.h"
  3.  
  4. #include <gtsam/geometry/Rot3.h>
  5. #include <gtsam/geometry/Pose3.h>
  6. #include <gtsam/slam/PriorFactor.h>
  7. #include <gtsam/slam/BetweenFactor.h>
  8. #include <gtsam/navigation/GPSFactor.h>
  9. #include <gtsam/navigation/ImuFactor.h>
  10. #include <gtsam/navigation/CombinedImuFactor.h>
  11. #include <gtsam/nonlinear/NonlinearFactorGraph.h>
  12. #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
  13. #include <gtsam/nonlinear/Marginals.h>
  14. #include <gtsam/nonlinear/Values.h>
  15. #include <gtsam/inference/Symbol.h>
  16.  
  17. #include <gtsam/nonlinear/ISAM2.h>
  18.  
  19. using namespace gtsam;
  20.  
  21. using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
  22. using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
  23. using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
  24. using symbol_shorthand::G; // GPS pose
  25.  
  26. /*
  27.     * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
  28.     */
  29. struct PointXYZIRPYT                    //6D位姿点云结构体定义
  30. {
  31.     PCL_ADD_POINT4D
  32.     PCL_ADD_INTENSITY;                  // preferred way of adding a XYZ+padding
  33.     float roll;
  34.     float pitch;
  35.     float yaw;
  36.     double time;
  37.     EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
  38. } EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment
  39.  
  40. POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
  41.                                    (float, x, x) (float, y, y)
  42.                                    (float, z, z) (float, intensity, intensity)
  43.                                    (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
  44.                                    (double, time, time))
  45.  
  46. typedef PointXYZIRPYT  PointTypePose;
  47.  
  48.  
  49. class mapOptimization : public ParamServer
  50. {
  51.  
  52. public:
  53.  
  54.     // gtsam
  55.     NonlinearFactorGraph gtSAMgraph;
  56.     Values initialEstimate;
  57.     Values optimizedEstimate;
  58.     ISAM2 *isam;
  59.     Values isamCurrentEstimate;
  60.     Eigen::MatrixXd poseCovariance;
  61.  
  62.     ros::Publisher pubLaserCloudSurround;
  63.     ros::Publisher pubOdomAftMappedROS;
  64.     ros::Publisher pubKeyPoses;
  65.     ros::Publisher pubPath;
  66.  
  67.     ros::Publisher pubHistoryKeyFrames;
  68.     ros::Publisher pubIcpKeyFrames;
  69.     ros::Publisher pubRecentKeyFrames;
  70.     ros::Publisher pubRecentKeyFrame;
  71.     ros::Publisher pubCloudRegisteredRaw;
  72.  
  73.     ros::Subscriber subLaserCloudInfo;
  74.     ros::Subscriber subGPS;
  75.  
  76.     std::deque<nav_msgs::Odometry> gpsQueue;
  77.     lio_sam::cloud_info cloudInfo;
  78.  
  79.     //历史所有关键帧的角点集合（降采样）
  80.     vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
  81.     //历史所有关键帧的平面点集合（降采样）
  82.     vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
  83.     //历史关键帧位姿（位置）
  84.     pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
  85.     //历史关键帧位姿
  86.     pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
  87.     //  * pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;
  88.     //  * pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;
  89.     //当前激光帧角点集合
  90.     pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization
  91.     //当前激光帧平面点集合
  92.     pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
  93.     //当前激光帧角点集合，降采样 DS：down sample
  94.     pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
  95.     //当前降采样平面点集合 
  96.     pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
  97.  
  98.     // 当前帧与局部map匹配上的角点，平面点加入同一集合；后面是对应的参数
  99.     pcl::PointCloud<PointType>::Ptr laserCloudOri;
  100.     pcl::PointCloud<PointType>::Ptr coeffSel;
  101.  
  102.     // 当前帧与局部map匹配上的角点、参数、标记;
  103.     std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
  104.     std::vector<PointType> coeffSelCornerVec;
  105.     std::vector<bool> laserCloudOriCornerFlag;
  106.     // 当前帧与局部map匹配上的平面点、参数、标记
  107.     std::vector<PointType> laserCloudOriSurfVec; // surfstampMapDS;
  108.     std::vector<PointType> coeffSelSurfVec;
  109.     std::vector<bool> laserCloudOriSurfFlag;
  110.  
  111.     // 局部角点集合
  112.     pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
  113.     // 局部平面点集合
  114.     pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
  115.     // 局部map角点集合，降采样
  116.     pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
  117.     // 局部map平面点集合，降采样
  118.     pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
  119.  
  120.     // 局部关键帧构建的map点云，对应kd-tree，用于scan-to-map找相邻点
  121.     pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
  122.     pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
  123.  
  124.     pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
  125.     pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
  126.  
  127.     pcl::PointCloud<PointType>::Ptr latestKeyFrameCloud;
  128.     pcl::PointCloud<PointType>::Ptr nearHistoryKeyFrameCloud;
  129.  
  130.     // 降采样
  131.     pcl::VoxelGrid<PointType> downSizeFilterCorner;
  132.     pcl::VoxelGrid<PointType> downSizeFilterSurf;
  133.     pcl::VoxelGrid<PointType> downSizeFilterICP;
  134.     pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
  135.     
  136.     ros::Time timeLaserInfoStamp;
  137.     double timeLaserCloudInfoLast;
  138.  
  139.     float transformTobeMapped[6];
  140.  
  141.     std::mutex mtx;
  142.     double timeLastProcessing = -1;
  143.     // * std::mutex mtxLoopInfo
  144.  
  145.     bool isDegenerate = false;                //退化  
  146.     Eigen::Matrix<float, 6, 6> matP;
  147.  
  148.     // 局部map降采样角点数量
  149.     int laserCloudCornerFromMapDSNum = 0;
  150.     // 局部map降采样平面点数量
  151.     int laserCloudSurfFromMapDSNum = 0;
  152.     // 当前激光帧降采样角点数量
  153.     int laserCloudCornerLastDSNum = 0;
  154.     // 当前激光帧降采样平面点数量
  155.     int laserCloudSurfLastDSNum = 0;
  156.  
  157.     bool aLoopIsClosed = false;
  158.     int imuPreintegrationResetId = 0;
  159.     // * vector<pair<int, int>> loopIndexQueue;
  160.     // * vector<gtsam::Pose3> loopPoseQueue;
  161.     // * vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;
  162.     // * deque<std_msgs::Float64MultiArray> loopInfoVec;
  163.  
  164.     nav_msgs::Path globalPath;
  165.     // 当前帧位姿
  166.     Eigen::Affine3f transPointAssociateToMap;
  167.     // 前一帧位姿
  168.     // * Eigen::Affine3f incrementalOdometryAffineFront;
  169.     // 当前帧位姿
  170.     // * Eigen::Affine3f incrementalOdometryAffineBack;
  171.  
  172.     mapOptimization()
  173.     {
  174.         // ISM2参数
  175.         ISAM2Params parameters;
  176.         parameters.relinearizeThreshold = 0.1;
  177.         parameters.relinearizeSkip = 1;
  178.         isam = new ISAM2(parameters);
  179.  
  180.         // 发布历史关键帧里程计
  181.         pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
  182.         // 发布局部关键帧map的特征点云
  183.         pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
  184.         // 发布激光里程计，rviz中表现为坐标轴
  185.         // pubLaserOdometryGlobal      = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
  186.         // 发布激光里程计，它与上面的激光里程计基本一样，只是roll、pitch用imu数据加权平均了一下，z做了限制
  187.         // pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1);
  188.         pubOdomAftMappedROS = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
  189.         // 发布激光里程及路径，rviz中表现为小车的运行轨迹
  190.         pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
  191.  
  192.         subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 10, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
  193.         // 订阅gps里程计
  194.         subGPS = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
  195.         // 订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上
  196.         // subLoop  = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());
  197.         // 发布地图保存服务
  198.         // srvSaveMap  = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);
  199.  
  200.         pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
  201.         pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
  202.  
  203.         // 发布局部map的降采样平面点集合
  204.         pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
  205.         // 发布历史帧（累加的）的角点、平面点降采样集合
  206.         pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
  207.         // 发布当前帧原始点云配准之后的点云
  208.         pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);
  209.  
  210.         downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
  211.         downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
  212.         downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
  213.         downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
  214.  
  215.         allocateMemory();
  216.     }
  217.  
  218.     // 内存分配
  219.     void allocateMemory()                           
  220.     {
  221.         cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
  222.         cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
  223.  
  224.         kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
  225.         kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
  226.  
  227.         laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
  228.         laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
  229.         laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
  230.         laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
  231.  
  232.         laserCloudOri.reset(new pcl::PointCloud<PointType>());
  233.         coeffSel.reset(new pcl::PointCloud<PointType>());
  234.  
  235.         laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
  236.         coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
  237.         laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
  238.         laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
  239.         coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
  240.         laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
  241.  
  242.         std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
  243.         std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
  244.  
  245.         laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
  246.         laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
  247.         laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
  248.         laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
  249.  
  250.         kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
  251.         kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
  252.  
  253.         latestKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
  254.         nearHistoryKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
  255.  
  256.         for (int i = 0; i < 6; ++i){
  257.             transformTobeMapped[i] = 0;
  258.         }
  259.  
  260.         matP.setZero();
  261.     }
  262.  
  263.  
  264.  
  265.     void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)            
  266.     {
  267.         // extract time stamp
  268.         //获取当前激光帧时间戳
  269.         timeLaserInfoStamp = msgIn->header.stamp;
  270.         timeLaserCloudInfoLast = msgIn->header.stamp.toSec();
  271.  
  272.         // extract info and feature cloud
  273.         // 提取来自featureExtraction的当前激光帧角点、平面点集合
  274.         cloudInfo = *msgIn;
  275.         pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);   //将ros的角点点云消息数据转换成pcl数据
  276.         pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);     //将ros的平面点点云消息转换成pcl数据
  277.  
  278.         std::lock_guard<std::mutex> lock(mtx);
  279.  
  280.         // mapping执行频率控制
  281.         // static double timeLastProcessing = -1;
  282.         if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval) {//gc:control the rate of mapping process
  283.  
  284.             timeLastProcessing = timeLaserCloudInfoLast;
  285.             // 当前帧位姿初始化
  286.             // 1、如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
  287.             // 2、后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光初始位姿
  288.             updateInitialGuess();//gc: update initial value for states
  289.             // 获得当前帧的点云，降采样角点和平面点
  290.             downsampleCurrentScan();//gc:down sample the current corner points and surface points
  291.             // 然后和全局地图去匹配（scan to map），得到当前帧的一个更好的位姿。
  292.             scan2MapOptimization();//gc: calculate the tranformtion using lidar measurement with the Imu preintegration as initial values
  293.                                     //and then interpolate roll and pitch angle using IMU measurement and above measurement
  294.             //但不可能拿全局地图来匹配，所以选取和当前帧比较近的点云，获取局部点云图
  295.             extractSurroundingKeyFrames();//gc:
  296.             //但上述匹配仍不够完美，需要一个后端融合所有信息获取真正准确的位姿。设置当前帧为关键帧，并执行因子图优化
  297.             saveKeyFramesAndFactor();//gc: save corner cloud and surface cloud of this scan, and add odom and GPS factors
  298.             //随后更新所有的状态量，即更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
  299.             correctPoses();
  300.             //最后把结果给下游模块发布
  301.             publishOdometry();
  302.             // 发布里程计、点云、轨迹
  303.             // 1、发布历史关键帧位姿集合
  304.             // 2、发布局部map的降采样平面点集合
  305.             // 3、发布历史帧（累加的）的角点、平面点降采样集合
  306.             // 4、发布里程计轨迹
  307.             publishFrames();
  308.         }
  309.     }
  310.  
  311.     void gpsHandler(const nav_msgs::Odometry::ConstPtr& gpsMsg)
  312.     {
  313.         gpsQueue.push_back(*gpsMsg);
  314.     }
  315.  
  316.     // 根据当前帧位姿，变换到世界坐标系（map）下
  317.     void pointAssociateToMap(PointType const * const pi, PointType * const po)
  318.     {
  319.         po->x = transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z + transPointAssociateToMap(0,3);
  320.         po->y = transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z + transPointAssociateToMap(1,3);
  321.         po->z = transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z + transPointAssociateToMap(2,3);
  322.         po->intensity = pi->intensity;
  323.     }
  324.  
  325.     pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn)        //输入两个形参，分别为点云和变换，返回变换到世界坐标系下的点
  326.     {
  327.         pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
  328.  
  329.         PointType *pointFrom;
  330.  
  331.         int cloudSize = cloudIn->size();
  332.         cloudOut->resize(cloudSize);
  333.  
  334.         Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
  335.         
  336.         for (int i = 0; i < cloudSize; ++i){
  337.  
  338.             pointFrom = &cloudIn->points[i];
  339.             cloudOut->points[i].x = transCur(0,0) * pointFrom->x + transCur(0,1) * pointFrom->y + transCur(0,2) * pointFrom->z + transCur(0,3);
  340.             cloudOut->points[i].y = transCur(1,0) * pointFrom->x + transCur(1,1) * pointFrom->y + transCur(1,2) * pointFrom->z + transCur(1,3);
  341.             cloudOut->points[i].z = transCur(2,0) * pointFrom->x + transCur(2,1) * pointFrom->y + transCur(2,2) * pointFrom->z + transCur(2,3);
  342.             cloudOut->points[i].intensity = pointFrom->intensity;
  343.         }
  344.         return cloudOut;
  345.     }
  346.  
  347.     gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
  348.     {
  349.         return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
  350.                                   gtsam::Point3(double(thisPoint.x),    double(thisPoint.y),     double(thisPoint.z)));
  351.     }
  352.  
  353.     gtsam::Pose3 trans2gtsamPose(float transformIn[])
  354.     {
  355.         return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]), 
  356.                                   gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
  357.     }
  358.  
  359.     Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
  360.     { 
  361.         return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
  362.     }
  363.  
  364.     // Eigen格式的位姿变换
  365.     Eigen::Affine3f trans2Affine3f(float transformIn[])
  366.     {
  367.         return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
  368.     }
  369.  
  370.     PointTypePose trans2PointTypePose(float transformIn[])
  371.     {
  372.         PointTypePose thisPose6D;
  373.         thisPose6D.x = transformIn[3];
  374.         thisPose6D.y = transformIn[4];
  375.         thisPose6D.z = transformIn[5];
  376.         thisPose6D.roll  = transformIn[0];
  377.         thisPose6D.pitch = transformIn[1];
  378.         thisPose6D.yaw   = transformIn[2];
  379.         return thisPose6D;
  380.     }
  381.  
  382.  
  383.     void visualizeGlobalMapThread()
  384.     {
  385.         ros::Rate rate(0.2);
  386.         while (ros::ok()){
  387.             rate.sleep();
  388.             publishGlobalMap();
  389.         }
  390.  
  391.         if (savePCD == false)
  392.             return;
  393.  
  394.         cout << "****************************************************" << endl;
  395.         cout << "Saving map to pcd files ..." << endl;
  396.         // create directory and remove old files;
  397.         savePCDDirectory = std::getenv("HOME") + savePCDDirectory;
  398.         int unused = system((std::string("exec rm -r ") + savePCDDirectory).c_str());
  399.         unused = system((std::string("mkdir ") + savePCDDirectory).c_str());
  400.         // save key frame transformations
  401.         pcl::io::savePCDFileASCII(savePCDDirectory + "trajectory.pcd", *cloudKeyPoses3D);
  402.         pcl::io::savePCDFileASCII(savePCDDirectory + "transformations.pcd", *cloudKeyPoses6D);
  403.         // extract global point cloud map    
  404.         //提取历史关键帧角点平面点集合    
  405.         pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
  406.         pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());
  407.         pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
  408.         pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());
  409.         pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
  410.         for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {
  411.             *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i],  &cloudKeyPoses6D->points[i]);
  412.             *globalSurfCloud   += *transformPointCloud(surfCloudKeyFrames[i],    &cloudKeyPoses6D->points[i]);
  413.             cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
  414.         }
  415.         // down-sample and save corner cloud
  416.         downSizeFilterCorner.setInputCloud(globalCornerCloud);
  417.         downSizeFilterCorner.filter(*globalCornerCloudDS);
  418.         pcl::io::savePCDFileASCII(savePCDDirectory + "cloudCorner.pcd", *globalCornerCloudDS);
  419.         // down-sample and save surf cloud
  420.         downSizeFilterSurf.setInputCloud(globalSurfCloud);
  421.         downSizeFilterSurf.filter(*globalSurfCloudDS);
  422.         pcl::io::savePCDFileASCII(savePCDDirectory + "cloudSurf.pcd", *globalSurfCloudDS);
  423.         // down-sample and save global point cloud map
  424.         *globalMapCloud += *globalCornerCloud;
  425.         *globalMapCloud += *globalSurfCloud;
  426.         pcl::io::savePCDFileASCII(savePCDDirectory + "cloudGlobal.pcd", *globalMapCloud);
  427.         cout << "****************************************************" << endl;
  428.         cout << "Saving map to pcd files completed" << endl;
  429.     }
  430.  
  431.     void publishGlobalMap()
  432.     {
  433.         if (pubLaserCloudSurround.getNumSubscribers() == 0)
  434.             return;
  435.  
  436.         if (cloudKeyPoses3D->points.empty() == true)
  437.             return;
  438.  
  439.         pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;
  440.         pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
  441.         pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
  442.         pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
  443.         pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());
  444.  
  445.         // kd-tree to find near key frames to visualize
  446.         // kdtree查找最近一帧关键帧相邻的关键帧集合
  447.         std::vector<int> pointSearchIndGlobalMap;
  448.         std::vector<float> pointSearchSqDisGlobalMap;
  449.         // search near key frames to visualize
  450.         mtx.lock();
  451.         kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
  452.         kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
  453.         mtx.unlock();
  454.  
  455.         for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
  456.             globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
  457.         // downsample near selected key frames
  458.         pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
  459.         // 降采样全局地图关键帧
  460.         downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
  461.         downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
  462.         downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
  463.  
  464.         // extract visualized and downsampled key frames
  465.         // 提取局部关键帧特征点云
  466.         for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i){
  467.             // 剔除过远点
  468.             if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
  469.                 continue;
  470.             int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
  471.             *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
  472.             *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
  473.         }
  474.         // downsample visualized points
  475.         // 体素降采样，发布
  476.         pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
  477.         downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
  478.         downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
  479.         downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
  480.         publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, "odom");    
  481.     }
  482.  
  483.  
  484.     void loopClosureThread()
  485.     {
  486.         if (loopClosureEnableFlag == false)
  487.             return;
  488.  
  489.         ros::Rate rate(0.1);
  490.         while (ros::ok())
  491.         {
  492.             rate.sleep();
  493.             performLoopClosure();
  494.         }
  495.     }
  496.  
  497.     bool detectLoopClosure(int *latestID, int *closestID)
  498.     {
  499.         int latestFrameIDLoopCloure;
  500.         int closestHistoryFrameID;
  501.  
  502.         latestKeyFrameCloud->clear();       //当前帧点云集合
  503.         nearHistoryKeyFrameCloud->clear();  //回环帧点云集合
  504.  
  505.         std::lock_guard<std::mutex> lock(mtx);
  506.  
  507.         // find the closest history key frame
  508.         std::vector<int> pointSearchIndLoop;
  509.         std::vector<float> pointSearchSqDisLoop;
  510.         kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
  511.         //gc:search the last keyframe's nearest keyframes，输入：索要查询的点，搜索距离  输出：搜索完领域内对应点索引、欧式距离，0表示返回全部查询结果
  512.         kdtreeHistoryKeyPoses->radiusSearch(cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
  513.         
  514.         closestHistoryFrameID = -1;
  515.         for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
  516.         {
  517.             int id = pointSearchIndLoop[i];
  518.             //gc: find the nearest keyframe whose time is not close
  519.             if (abs(cloudKeyPoses6D->points[id].time - timeLaserCloudInfoLast) > historyKeyframeSearchTimeDiff)
  520.             {
  521.                 closestHistoryFrameID = id;
  522.                 break;          //结束整个for循环
  523.             }
  524.         }
  525.  
  526.         if (closestHistoryFrameID == -1)
  527.             return false;
  528.         //gc: if the closest keyframe is 当前帧
  529.         if ((int)cloudKeyPoses3D->size() - 1 == closestHistoryFrameID)
  530.             return false;
  531.  
  532.         // save latest key frames  将当前帧保存进最新关键帧集合
  533.         latestFrameIDLoopCloure = cloudKeyPoses3D->size() - 1;
  534.         *latestKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
  535.         *latestKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure],   &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
  536.  
  537.         // save history near key frames
  538.         //gc: combine the time_close frame of the nearset frame
  539.         // 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合
  540.         bool nearFrameAvailable = false;
  541.         for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j)
  542.         {
  543.             // 防止回环帧过早或者过晚
  544.             if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)
  545.                 continue;
  546.             *nearHistoryKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
  547.             *nearHistoryKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j],   &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
  548.             nearFrameAvailable = true;
  549.         }
  550.  
  551.         if (nearFrameAvailable == false)
  552.             return false;
  553.         
  554.         // 找到两个回环帧的id后
  555.         *latestID = latestFrameIDLoopCloure;
  556.         *closestID = closestHistoryFrameID;
  557.  
  558.         return true;
  559.     }
  560.  
  561.     
  562.     void performLoopClosure()
  563.     {
  564.         // 找到两帧回环帧点云后，开始icp匹配
  565.         if (cloudKeyPoses3D->points.empty() == true)
  566.             return;
  567.  
  568.         int latestFrameIDLoopCloure;
  569.         int closestHistoryFrameID;
  570.         if (detectLoopClosure(&latestFrameIDLoopCloure, &closestHistoryFrameID) == false)
  571.             return;
  572.  
  573.  
  574.         // ICP Settings
  575.         // ICP参数设置
  576.         pcl::IterativeClosestPoint<PointType, PointType> icp;   //创建icp实例类
  577.         icp.setMaxCorrespondenceDistance(100);      //设置对应点对之间的最大距离（此值对配准结果影响较大）。  
  578.         icp.setMaximumIterations(100);              //最大迭代次数
  579.         icp.setTransformationEpsilon(1e-6);         // 设置两次变化矩阵之间的差值（一般设置为1e-10即可）；
  580.         icp.setEuclideanFitnessEpsilon(1e-6);       //设置收敛条件是均方误差和小于阈值， 停止迭代；
  581.         icp.setRANSACIterations(0);
  582.  
  583.         // Downsample map cloud
  584.         pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
  585.         downSizeFilterICP.setInputCloud(nearHistoryKeyFrameCloud);      //回环帧点云降采样
  586.         downSizeFilterICP.filter(*cloud_temp);
  587.         *nearHistoryKeyFrameCloud = *cloud_temp;
  588.         // publish history near key frames
  589.         publishCloud(&pubHistoryKeyFrames, nearHistoryKeyFrameCloud, timeLaserInfoStamp, "odom");
  590.  
  591.         // Align clouds
  592.         //icp.setin
  593.         icp.setInputSource(latestKeyFrameCloud);
  594.         icp.setInputTarget(nearHistoryKeyFrameCloud);
  595.         pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
  596.         icp.align(*unused_result);      //经过配准后的点云，保存经过icp算法应用过后的结果
  597.  
  598.         // std::cout << "ICP converg flag:" << icp.hasConverged() << ". Fitness score: " << icp.getFitnessScore() << std::endl;
  599.         // icp输入和目标点云对齐正确，hasConverged为true
  600.         // getFitnessScore用来表示两份点云之间的误差，小于阈值表示匹配成功
  601.         if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
  602.             return;
  603.  
  604.         // publish corrected cloud
  605.         // getNumSubscribers判断订阅者是否连接
  606.         if (pubIcpKeyFrames.getNumSubscribers() != 0){
  607.             pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
  608.             // 输入点云，输出点云，变换矩阵
  609.             pcl::transformPointCloud(*latestKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());
  610.             // 发布当前帧经过回环优化后的位姿变换的点云
  611.             publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, "odom");
  612.         }
  613.  
  614.         // 匹配成功的话，更新位姿
  615.         // Get pose transformation
  616.         // 闭环优化得到的当前关键帧
  617.         float x, y, z, roll, pitch, yaw;
  618.         Eigen::Affine3f correctionLidarFrame;
  619.         //gc: realtive transformation
  620.         // 闭环关键帧之间的位姿变换
  621.         correctionLidarFrame = icp.getFinalTransformation();
  622.         // transform from world origin to wrong pose
  623.         // 闭环优化前当前帧位姿
  624.         Eigen::Affine3f tWrong = pclPointToAffine3f(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
  625.         // transform from world origin to corrected pose
  626.         // 闭环优化后当前帧位姿
  627.         Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
  628.         pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
  629.         gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
  630.         // 闭环匹配帧的位姿
  631.         gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);
  632.  
  633.         gtsam::Vector Vector6(6);
  634.         // 误差
  635.         float noiseScore = icp.getFitnessScore();//gc: noise right??
  636.         Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
  637.         // 使用icp的得分作为他们的约束噪声项
  638.         noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
  639.         // Add pose constraint
  640.         std::lock_guard<std::mutex> lock(mtx);
  641.         // 添加闭环因子需要的数据
  642.         gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
  643.         isam->update(gtSAMgraph);
  644.         isam->update();
  645.         gtSAMgraph.resize(0);
  646.  
  647.         isamCurrentEstimate = isam->calculateEstimate();
  648.         Pose3 latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
  649.  
  650.         transformTobeMapped[0] = latestEstimate.rotation().roll();
  651.         transformTobeMapped[1] = latestEstimate.rotation().pitch();
  652.         transformTobeMapped[2] = latestEstimate.rotation().yaw();
  653.         transformTobeMapped[3] = latestEstimate.translation().x();
  654.         transformTobeMapped[4] = latestEstimate.translation().y();
  655.         transformTobeMapped[5] = latestEstimate.translation().z();
  656.  
  657.         correctPoses();
  658.  
  659.         aLoopIsClosed = true;
  660.     }
  661.  
  662.  
  663.  
  664.     // 当前帧位姿初始化
  665.     // 2、后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光初始位姿
  666.     void updateInitialGuess()
  667.     {
  668.         // save current transformation before any processing
  669.         // 前一帧的位姿，注：这里指lidar的位姿，后面都简写成位姿
  670.         // incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
  671.  
  672.         // 前一帧的初始化姿态角（来自cloudInfo的IMU数据）
  673.         static Eigen::Affine3f lastImuTransformation;//gc: note that this is static type
  674.         // initialization
  675.         // 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
  676.         if (cloudKeyPoses3D->points.empty())//gc: there is no key pose 初始化
  677.         {
  678.             transformTobeMapped[0] = cloudInfo.imuRollInit;
  679.             transformTobeMapped[1] = cloudInfo.imuPitchInit;
  680.             transformTobeMapped[2] = cloudInfo.imuYawInit;
  681.  
  682.             if (!useImuHeadingInitialization)//gc: if not use the heading of init_IMU as Initialization   //GPS
  683.                 transformTobeMapped[2] = 0;
  684.  
  685.             lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
  686.             return;
  687.         }
  688.  
  689.  
  690.         // 用当前帧和前一帧对应的imu里程计计算相对位姿变换，再用前一帧的位姿与相对变换，计算当前帧的位姿，存transformTobeMapped
  691.         // * static bool lastImuPreTransAvailable = false;
  692.         // * static Eigen::Affine3f lastImuPreTransformation;
  693.         //odomAvailable和imuAvailable均来源于imageProjection.cpp中赋值，
  694.         //imuAvailable是遍历激光帧前后起止时刻0.01s之内的imu数据，
  695.         //如果都没有那就是false，因为imu频率一般比激光帧快，因此这里应该是都有的。
  696.         //odomAvailable同理，是监听imu里程计的位姿，如果没有紧挨着激光帧的imu里程计数据，那么就是false；
  697.         //这俩应该一般都有
  698.         // use imu pre-integration estimation for pose guess
  699.         // 
  700.         if (cloudInfo.odomAvailable == true && cloudInfo.imuPreintegrationResetId == imuPreintegrationResetId)     //该段代码与原文相差较大
  701.         {
  702.             // cloudInfo来自featureExtraction.cpp发布的liox-sam/feature/cloud_info 
  703.             // 直接使用imu里程计数据
  704.             transformTobeMapped[0] = cloudInfo.initialGuessRoll;
  705.             transformTobeMapped[1] = cloudInfo.initialGuessPitch;
  706.             transformTobeMapped[2] = cloudInfo.initialGuessYaw;
  707.  
  708.             transformTobeMapped[3] = cloudInfo.initialGuessX;
  709.             transformTobeMapped[4] = cloudInfo.initialGuessY;
  710.             transformTobeMapped[5] = cloudInfo.initialGuessZ;
  711.  
  712.             lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
  713.             return;
  714.         }
  715.  
  716.         // use previous pose for pose guess
  717.         // if (cloudKeyPoses6D->points.size() >= 2)
  718.         // {
  719.         //     int oldId = cloudKeyPoses6D->points.size() - 2;
  720.         //     int preId = cloudKeyPoses6D->points.size() - 1;
  721.         //     Eigen::Affine3f transOld = pclPointToAffine3f(cloudKeyPoses6D->points[oldId]);
  722.         //     Eigen::Affine3f transPre = pclPointToAffine3f(cloudKeyPoses6D->points[preId]);
  723.         //     double deltaTimePre = cloudKeyPoses6D->points[preId].time - cloudKeyPoses6D->points[oldId].time;
  724.         //     double deltaTimeNow = timeLaserCloudInfoLast - cloudKeyPoses6D->points[preId].time;
  725.         //     double alpha = deltaTimeNow / deltaTimePre;
  726.  
  727.         //     Eigen::Affine3f transIncPre = transOld.inverse() * transPre;
  728.         //     float x, y, z, roll, pitch, yaw;
  729.         //     pcl::getTranslationAndEulerAngles (transIncPre, x, y, z, roll, pitch, yaw);
  730.         //     Eigen::Affine3f transIncNow = pcl::getTransformation(alpha*x, alpha*y, alpha*z, alpha*roll, alpha*pitch, alpha*yaw);
  731.  
  732.         //     Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
  733.         //     Eigen::Affine3f transFinal = transTobe * transIncNow;
  734.         //     pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
  735.         //                                                   transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  736.  
  737.         //     lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
  738.         //     return;
  739.         // }    
  740.  
  741.         // use imu incremental estimation for pose guess (only rotation)
  742.         // 如果是第二帧
  743.         if (cloudInfo.imuAvailable == true)
  744.         {
  745.             // transBack是这一帧时刻的imu位姿
  746.             Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);            
  747.             // lastImuTransFormation是最新的上一刻数据，求逆后与transback相乘才是增量
  748.             Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;//gc: the transform of IMU between two scans
  749.             //前一帧的位姿
  750.             Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
  751.             // 当前帧姿态
  752.             Eigen::Affine3f transFinal = transTobe * transIncre;
  753.             //将transFinal传入，结果输出至transformTobeMapped
  754.             pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
  755.                                                           transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  756.  
  757.             lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
  758.             return;
  759.         }
  760.     }
  761.  
  762.     void extractForLoopClosure()
  763.     {
  764.         pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
  765.         int numPoses = cloudKeyPoses3D->size();
  766.         for (int i = numPoses-1; i >= 0; --i)
  767.         {
  768.             if ((int)cloudToExtract->size() <= surroundingKeyframeSize)
  769.                 cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
  770.             else
  771.                 break;
  772.         }
  773.  
  774.         extractCloud(cloudToExtract);
  775.     }
  776.  
  777.  
  778.  
  779.     //gc:search nearby key poses and downsample them and then extract the local map points
  780.     void extractNearby()
  781.     {
  782.         pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
  783.         // 降采样后的关键帧
  784.         pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());//gc: the key poses after downsample
  785.         std::vector<int> pointSearchInd;
  786.         std::vector<float> pointSearchSqDis;
  787.  
  788.         // extract all the nearby key poses and downsample them
  789.         // kd-tree的输入，全局关键帧位姿集合（历史所有关键帧）
  790.         kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
  791.         //gc: kd-tree is used for nearby key poses
  792.         // 创建kd-tree。固定半径近邻搜索，pointSearchInd返回index，pointSearchSqDis依次距离中心点距离
  793.         kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
  794.         for (int i = 0; i < (int)pointSearchInd.size(); ++i)
  795.         {
  796.             int id = pointSearchInd[i];
  797.             surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
  798.         }
  799.  
  800.         // 降采样，再滤波，存入surroundingKeyPosesDS
  801.         downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
  802.         downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
  803.  
  804.         // also extract some latest key frames in case the robot rotates in one position
  805.         // 提取一些最新的关键帧以防机器人原地旋转，即和当前帧时间近的帧
  806.         int numPoses = cloudKeyPoses3D->size();
  807.         // 把10s内的关键帧也加到surroundingKeyPosesDS中,注意是“也”，原先已经装了下采样的位姿(位置)
  808.         for (int i = numPoses-1; i >= 0; --i)
  809.         {
  810.             if (timeLaserCloudInfoLast - cloudKeyPoses6D->points[i].time < 10.0)
  811.                 surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
  812.             else
  813.                 break;
  814.         }
  815.         //对降采样后的点云提取出角点和平面点，加入局部map中，作为scan-to-map的局部点云地图
  816.         extractCloud(surroundingKeyPosesDS);
  817.     }
  818.  
  819.  
  820.     //gc: extract the nearby Map points
  821.     /*
  822.       * 将相邻关键帧集合对应的角点、平面点，加入到局部map中，作为scan-to-map匹配的局部点云地图
  823.     */
  824.     void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
  825.     {
  826.         std::vector<pcl::PointCloud<PointType>> laserCloudCornerSurroundingVec;       //新加变量，降采样后附近角点
  827.         std::vector<pcl::PointCloud<PointType>> laserCloudSurfSurroundingVec;
  828.  
  829.         laserCloudCornerSurroundingVec.resize(cloudToExtract->size());
  830.         laserCloudSurfSurroundingVec.resize(cloudToExtract->size());
  831.  
  832.         // extract surrounding map
  833.         #pragma omp parallel for num_threads(numberOfCores)             //4个线程共同执行循环
  834.         for (int i = 0; i < (int)cloudToExtract->size(); ++i)
  835.         {
  836.             // 距离超过50m，丢弃
  837.             if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
  838.                 continue;
  839.             //相邻关键帧索引 
  840.             int thisKeyInd = (int)cloudToExtract->points[i].intensity;//gc: the index of this key frame
  841.             //gc: tranform the corner points and surfpoints of the nearby keyFrames into the world frame
  842.             //相邻关键帧对应的角点、平面点云，通过6D位姿变换到世界坐标系下
  843.             //transformPointCloud输入的两个形参，分别为点云和变换，返回变换位姿后的点            
  844.             laserCloudCornerSurroundingVec[i]  = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
  845.             laserCloudSurfSurroundingVec[i]    = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
  846.         }
  847.  
  848.         // fuse the map，角点面点清空
  849.         laserCloudCornerFromMap->clear();
  850.         laserCloudSurfFromMap->clear(); 
  851.         // 遍历当前帧（实际是取最近的一个关键帧来找它相邻的关键帧集合）时空维度上相邻的关键帧集合
  852.         for (int i = 0; i < (int)cloudToExtract->size(); ++i)
  853.         {
  854.             // 加入局部map
  855.             *laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i];
  856.             *laserCloudSurfFromMap   += laserCloudSurfSurroundingVec[i];
  857.         }
  858.  
  859.         // Downsample the surrounding corner key frames (or map)
  860.         // 降采样局部角点map
  861.         downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
  862.         downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
  863.         laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
  864.         // Downsample the surrounding surf key frames (or map)
  865.         // 降采样局部平面点map
  866.         downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
  867.         downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
  868.         laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
  869.  
  870.         // 太大了，清空一下内存
  871.         // if (laserCloudMapContainer.size() > 1000)
  872.         //     laserCloudMapContainer.clear();
  873.  
  874.     }
  875.  
  876.     /*
  877.      * 提取局部角点、平面点云集合，加入局部map
  878.      * 1、对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
  879.      * 2、对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
  880.     */
  881.     void extractSurroundingKeyFrames()
  882.     {
  883.         if (cloudKeyPoses3D->points.empty() == true)
  884.             return; 
  885.         
  886.         if (loopClosureEnableFlag == true)//gc:TODO: a little weired: Loop closure should search the whole map while
  887.         {
  888.             extractForLoopClosure(); //gc: the name is misleading
  889.         } else {
  890.             extractNearby();
  891.         }
  892.     }
  893.  
  894.     void downsampleCurrentScan()
  895.     {
  896.         // Downsample cloud from current scan
  897.         //对当前帧点云降采样  刚刚完成了周围关键帧的降采样  
  898.         //大量的降采样工作无非是为了使点云稀疏化 加快匹配以及实时性要求
  899.         laserCloudCornerLastDS->clear();
  900.         downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
  901.         downSizeFilterCorner.filter(*laserCloudCornerLastDS);
  902.         // 降采样角点数量
  903.         laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
  904.         // 降采样面点数量
  905.         laserCloudSurfLastDS->clear();
  906.         downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
  907.         downSizeFilterSurf.filter(*laserCloudSurfLastDS);
  908.         laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
  909.     }
  910.  
  911.  
  912.     /*
  913.      * 当前激光帧角点寻找局部map匹配点
  914.      * 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
  915.      * 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
  916.     */
  917.     void cornerOptimization()
  918.     {
  919.         //实现transformTobeMapped的矩阵形式转换
  920.         // 把结果存入transPointAssociateToMap中
  921.         updatePointAssociateToMap();
  922.  
  923.         #pragma omp parallel for num_threads(numberOfCores)
  924.         //gc: for every corner point
  925.         for (int i = 0; i < laserCloudCornerLastDSNum; i++)
  926.         {
  927.             PointType pointOri, pointSel, coeff;
  928.             std::vector<int> pointSearchInd;
  929.             std::vector<float> pointSearchSqDis;
  930.  
  931.             pointOri = laserCloudCornerLastDS->points[i];
  932.             //gc: calculate its location in the map using the prediction pose
  933.             //第i帧的点转换到第一帧坐标系下
  934.             //这里就调用了第一步中updatePointAssociateToMap中实现的transPointAssociateToMap，
  935.             //然后利用这个函数，把pointOri的点转换到pointSel下，pointSel作为输出
  936.             pointAssociateToMap(&pointOri, &pointSel);
  937.             // kd-tree最近搜索
  938.             kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
  939.  
  940.             cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));     //mat(int rows行数, int cols列数, int type类型);
  941.             cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
  942.             cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
  943.                     
  944.             if (pointSearchSqDis[4] < 1.0) {
  945.                 float cx = 0, cy = 0, cz = 0;
  946.                 // 5个样本的均值
  947.                 for (int j = 0; j < 5; j++) {
  948.                     cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
  949.                     cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
  950.                     cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
  951.                 }
  952.                 //gc: the average coordinate of the most nearest points
  953.                 cx /= 5; cy /= 5;  cz /= 5;
  954.                 // 下面求矩阵matA1=[ax,ay,az]^t*[ax,ay,az]
  955.                 // 更准确地说应该是在求协方差matA1
  956.                 float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
  957.                 for (int j = 0; j < 5; j++) {
  958.                     float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
  959.                     float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
  960.                     float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
  961.  
  962.                     a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
  963.                     a22 += ay * ay; a23 += ay * az;
  964.                     a33 += az * az;
  965.                 }
  966.                 a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
  967.  
  968.                 matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
  969.                 matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
  970.                 matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
  971.                 // 求正交阵的特征值和特征向量
  972.                 // 特征值：matD1，特征向量：matV1中  对应于LOAM论文里雷达建图 特征值与特征向量那块
  973.                 cv::eigen(matA1, matD1, matV1);
  974.                 // 边缘：与较大特征值相对应的特征向量代表边缘线的方向（一大两小，大方向）
  975.                 // 以下这一大块是在计算点到边缘的距离，最后通过系数s来判断是否距离很近
  976.                 // 如果距离很近就认为这个点在边缘上，需要放到laserCloudOri中
  977.                 // 如果最大的特征值相比次大特征值，大很多，认为构成了线，角点是合格的
  978.                 if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
  979.  
  980.                     // 当前帧角点坐标（map系下）
  981.                     float x0 = pointSel.x;
  982.                     float y0 = pointSel.y;
  983.                     float z0 = pointSel.z;
  984.                     // 局部map对应中心角点，沿着特征向量（直线方向）方向，前后各取一个点
  985.                     float x1 = cx + 0.1 * matV1.at<float>(0, 0);
  986.                     float y1 = cy + 0.1 * matV1.at<float>(0, 1);
  987.                     float z1 = cz + 0.1 * matV1.at<float>(0, 2);
  988.                     float x2 = cx - 0.1 * matV1.at<float>(0, 0);
  989.                     float y2 = cy - 0.1 * matV1.at<float>(0, 1);
  990.                     float z2 = cz - 0.1 * matV1.at<float>(0, 2);
  991.  
  992.                     // 这边是在求[(x0-x1),(y0-y1),(z0-z1)]与[(x0-x2),(y0-y2),(z0-z2)]叉乘得到的向量的模长
  993.                     // 这个模长是由0.2*V1[0]和点[x0,y0,z0]构成的平行四边形的面积
  994.                     // 因为[(x0-x1),(y0-y1),(z0-z1)]x[(x0-x2),(y0-y2),(z0-z2)]=[XXX,YYY,ZZZ],
  995.                     // [XXX,YYY,ZZZ]=[(y0-y1)(z0-z2)-(y0-y2)(z0-z1),-(x0-x1)(z0-z2)+(x0-x2)(z0-z1),(x0-x1)(y0-y2)-(x0-x2)(y0-y1)]
  996.  
  997.                     // area_012，也就是三个点组成的三角形面积*2，叉积的模|axb|=a*b*sin(theta)
  998.  
  999.                     float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
  1000.                                     + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
  1001.                                     + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
  1002.  
  1003.                     // line_12，底边边长
  1004.                     float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
  1005.  
  1006.                     // 两次叉积，得到点到直线的垂线段单位向量，x分量，下面同理
  1007.                     // 求叉乘结果[la',lb',lc']=[(x1-x2),(y1-y2),(z1-z2)]x[XXX,YYY,ZZZ]
  1008.                     // [la,lb,lc]=[la',lb',lc']/a012/l12
  1009.                     // 得到底边上的高的方向向量[la,lb,lc]
  1010.                     // LLL=[la,lb,lc]是V1[0]这条高上的单位法向量。||LLL||=1；
  1011.  
  1012.                     //如不理解则看图：
  1013.                     //        A
  1014.                     //   B        C
  1015.                     // 这里ABxAC，代表垂直于ABC面的法向量，其模长为平行四边形面积
  1016.                     //因此BCx(ABxAC),代表了BC和（ABC平面的法向量）的叉乘，那么其实这个向量就是A到BC的垂线的方向向量
  1017.                     //那么(ABxAC)/|ABxAC|,代表着ABC平面的单位法向量
  1018.                     //BCxABC平面单位法向量，即为一个长度为|BC|的（A到BC垂线的方向向量），因此再除以|BC|，得到A到BC垂线的单位方向向量
  1019.                     float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
  1020.                               + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
  1021.  
  1022.                     float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
  1023.                                - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
  1024.  
  1025.                     float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
  1026.                                + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
  1027.  
  1028.                     // 三角形的高，也就是点到直线距离
  1029.                     // 计算点pointSel到直线的距离
  1030.                     // 这里需要特别说明的是ld2代表的是点pointSel到过点[cx,cy,cz]的方向向量直线的距离
  1031.                     float ld2 = a012 / l12;
  1032.                     // 下面涉及到一个鲁棒核函数，作者简单地设计了这个核函数。
  1033.                     // 距离越大，s越小，是个距离惩罚因子（权重）
  1034.                     float s = 1 - 0.9 * fabs(ld2);
  1035.                     // coeff代表系数的意思
  1036.                     // coff用于保存距离的方向向量
  1037.                     coeff.x = s * la;
  1038.                     coeff.y = s * lb;
  1039.                     coeff.z = s * lc;
  1040.                     // intensity本质上构成了一个核函数，ld2越接近于1，增长越慢
  1041.                     // intensity=(1-0.9*ld2)*ld2=ld2-0.9*ld2*ld2
  1042.                     coeff.intensity = s * ld2;
  1043.                     // 程序末尾根据s的值来判断是否将点云点放入点云集合laserCloudOri以及coeffSel中。
  1044.                     // 所以就应该认为这个点是边缘点
  1045.                     // s>0.1 也就是要求点到直线的距离ld2要小于1m
  1046.                     // s越大说明ld2越小(离边缘线越近)，这样就说明点pointOri在直线上
  1047.                     if (s > 0.1) {
  1048.                         laserCloudOriCornerVec[i] = pointOri;
  1049.                         coeffSelCornerVec[i] = coeff;
  1050.                         laserCloudOriCornerFlag[i] = true;
  1051.                     }
  1052.                 }
  1053.             }
  1054.         }
  1055.     }
  1056.     /*
  1057.      * 当前激光帧平面点寻找局部map匹配点
  1058.      * 1、更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
  1059.      * 2、计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
  1060.     */
  1061.     void surfOptimization()
  1062.     {
  1063.         updatePointAssociateToMap();
  1064.         // 遍历当前帧平面点集合
  1065.         #pragma omp parallel for num_threads(numberOfCores)
  1066.         for (int i = 0; i < laserCloudSurfLastDSNum; i++)
  1067.         {
  1068.             PointType pointOri, pointSel, coeff;
  1069.             std::vector<int> pointSearchInd;
  1070.             std::vector<float> pointSearchSqDis;
  1071.             // 寻找5个紧邻点, 计算其特征值和特征向量
  1072.             // 平面点（坐标还是lidar系）
  1073.             pointOri = laserCloudSurfLastDS->points[i];
  1074.             // 根据当前帧位姿，变换到世界坐标系（map系）下
  1075.             pointAssociateToMap(&pointOri, &pointSel); 
  1076.             // 在局部平面点map中查找当前平面点相邻的5个平面点
  1077.             kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
  1078.  
  1079.             Eigen::Matrix<float, 5, 3> matA0;
  1080.             Eigen::Matrix<float, 5, 1> matB0;
  1081.             Eigen::Vector3f matX0;
  1082.  
  1083.             matA0.setZero();            // 5*3 存储5个紧邻点
  1084.             matB0.fill(-1);
  1085.             matX0.setZero();
  1086.             // 只考虑附近1.0m内
  1087.             if (pointSearchSqDis[4] < 1.0) {
  1088.                 for (int j = 0; j < 5; j++) {
  1089.                     matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
  1090.                     matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
  1091.                     matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
  1092.                 }
  1093.                 // 求maxA0中点构成的平面法向量
  1094.                 //matB0是-1，这个函数用来求解AX=B的X，
  1095.                 //也就是AX+BY+CZ+1=0
  1096.                 matX0 = matA0.colPivHouseholderQr().solve(matB0);
  1097.                 // 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1
  1098.                 float pa = matX0(0, 0);
  1099.                 float pb = matX0(1, 0);
  1100.                 float pc = matX0(2, 0);
  1101.                 float pd = 1;
  1102.                 // 单位法向量
  1103.                 // 对[pa,pb,pc,pd]进行单位化
  1104.                 float ps = sqrt(pa * pa + pb * pb + pc * pc);
  1105.                 pa /= ps; pb /= ps; pc /= ps; pd /= ps;
  1106.  
  1107.  
  1108.                 // 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面
  1109.                 bool planeValid = true;
  1110.                 for (int j = 0; j < 5; j++) {
  1111.                     if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
  1112.                              pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
  1113.                              pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
  1114.                         planeValid = false;
  1115.                         break;
  1116.                     }
  1117.                 }
  1118.  
  1119.                 if (planeValid) {
  1120.                     // 当前激光帧点到平面距离
  1121.                     //点(x0,y0,z0)到了平面Ax+By+Cz+D=0的距离为：d=|Ax0+By0+Cz0+D|/√(A^2+B^2+C^2)
  1122.                     //但是会发现下面的分母开了两次方，不知道为什么，分母多开一次方会更小，这因此求出的距离会更大
  1123.                     float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
  1124.                     // 距离越大，s越小，是个距离惩罚因子（权重）
  1125.                     // 后面部分相除求的是[pa,pb,pc,pd]与pointSel的夹角余弦值(两个sqrt，其实并不是余弦值)
  1126.                     // 这个夹角余弦值越小越好，越小证明所求的[pa,pb,pc,pd]与平面越垂直
  1127.                     float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
  1128.                             + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
  1129.                     // 点到平面垂线单位法向量（其实等价于平面法向量）
  1130.                     coeff.x = s * pa;
  1131.                     coeff.y = s * pb;
  1132.                     coeff.z = s * pc;
  1133.                     coeff.intensity = s * pd2;
  1134.  
  1135.                     if (s > 0.1) {
  1136.                         // 当前激光帧平面点，加入匹配集合中.
  1137.                         //如果s>0.1,代表fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z))这一项<1,即"伪距离"<1
  1138.                         laserCloudOriSurfVec[i] = pointOri;
  1139.                         coeffSelSurfVec[i] = coeff;
  1140.                         laserCloudOriSurfFlag[i] = true;
  1141.                     }
  1142.                 }
  1143.             }
  1144.         }
  1145.     }
  1146.  
  1147.     /*
  1148.      * 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
  1149.     */
  1150.     void combineOptimizationCoeffs()
  1151.     {
  1152.         // combine corner coeffs
  1153.         // 遍历当前帧角点集合，提取出与局部map匹配上了的角点
  1154.         for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
  1155.             if (laserCloudOriCornerFlag[i] == true){
  1156.                 laserCloudOri->push_back(laserCloudOriCornerVec[i]);
  1157.                 coeffSel->push_back(coeffSelCornerVec[i]);
  1158.             }
  1159.         }
  1160.         // combine surf coeffs
  1161.         // 遍历当前帧平面点集合，提取出与局部map匹配上了的平面点
  1162.         for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
  1163.             if (laserCloudOriSurfFlag[i] == true){
  1164.                 laserCloudOri->push_back(laserCloudOriSurfVec[i]);
  1165.                 coeffSel->push_back(coeffSelSurfVec[i]);
  1166.             }
  1167.         }
  1168.         // reset flag for next iteration
  1169.         // 清空标记
  1170.         std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
  1171.         std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
  1172.     }
  1173.  
  1174.  
  1175.     void scan2MapOptimization()
  1176.     {
  1177.         //根据现有地图与最新点云数据进行配准从而更新机器人精确位姿与融合建图，
  1178.         //它分为角点优化、平面点优化、配准与更新等部分。
  1179.         //优化的过程与里程计的计算类似，是通过计算点到直线或平面的距离，构建优化公式再用LM法求解。
  1180.         if (cloudKeyPoses3D->points.empty())
  1181.             return;
  1182.  
  1183.         //降采样后角点和平面点数量如果大于最小合理角点数量10和面点数量10
  1184.         if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
  1185.         {
  1186.             // 构建kd-tree
  1187.             kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
  1188.             kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
  1189.  
  1190.             // 迭代30次
  1191.             for (int iterCount = 0; iterCount < 30; iterCount++)
  1192.             {
  1193.                 laserCloudOri->clear();
  1194.                 coeffSel->clear();
  1195.                 //gc: calculate some coeff and judge whether tho point is valid corner point
  1196.                 // 当前激光帧与局部地图之间的：角点-角点 特征匹配
  1197.                 cornerOptimization();
  1198.                 //gc: calculate some coeff and judge whether tho point is valid surface point
  1199.                 // 当前激光帧与局部地图之间的：面点-面点 特征匹配
  1200.                 surfOptimization();
  1201.                 // 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
  1202.                 combineOptimizationCoeffs();
  1203.  
  1204.                 //gc: the true iteration steps, calculate the transform
  1205.                 // scan-to-map优化
  1206.                 if (LMOptimization(iterCount) == true)
  1207.                     break;              
  1208.             }
  1209.             //gc: interpolate the roll and pitch angle using the IMU measurement and Lidar calculation
  1210.             //使用了9轴imu的orientation与做transformTobeMapped插值，并且roll和pitch收到常量阈值约束（权重）
  1211.             // 更新当前位姿
  1212.             transformUpdate();
  1213.         } else {
  1214.             ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
  1215.         }
  1216.     }
  1217.  
  1218.     /*
  1219.      * 用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
  1220.     */
  1221.     //gc: interpolate the roll and pitch angle using the IMU measurement and Lidar calculation
  1222.     void transformUpdate()
  1223.     {
  1224.         if (cloudInfo.imuAvailable == true)
  1225.         {
  1226.             // 俯仰角小于1.4
  1227.             if (std::abs(cloudInfo.imuPitchInit) < 1.4)
  1228.             {
  1229.                 double imuWeight = 0.01;
  1230.                 tf::Quaternion imuQuaternion;
  1231.                 tf::Quaternion transformQuaternion;
  1232.                 double rollMid, pitchMid, yawMid;
  1233.  
  1234.                 // slerp roll
  1235.                 transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
  1236.                 imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
  1237.                 //gc: interpolate between Imu roll measurement and angle from lidar calculation
  1238.                 tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
  1239.                 transformTobeMapped[0] = rollMid;
  1240.  
  1241.                 // slerp pitch
  1242.                 transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
  1243.                 imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
  1244.                 //gc: interpolate between Imu roll measurement and angle from lidar calculation
  1245.                 tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
  1246.                 transformTobeMapped[1] = pitchMid;
  1247.             }
  1248.         }
  1249.         // 更新当前帧位姿的roll, pitch, z坐标；因为是小车，roll、pitch是相对稳定的，
  1250.         // 不会有很大变动，一定程度上可以信赖imu的数据，z是进行高度约束
  1251.         transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
  1252.         transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
  1253.         transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
  1254.     }
  1255.  
  1256.  
  1257.  
  1258.     //相当于clib函数 
  1259.     float constraintTransformation(float value, float limit)
  1260.     {
  1261.         if (value < -limit)
  1262.             value = -limit;
  1263.         if (value > limit)
  1264.             value = limit;
  1265.  
  1266.         return value;
  1267.     }
  1268.  
  1269.     /*
  1270.      * 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
  1271.     */
  1272.     bool saveFrame()
  1273.     {
  1274.         if (cloudKeyPoses3D->points.empty())
  1275.             return true;
  1276.         // 前一帧位姿
  1277.         //注：最开始没有的时候，在函数extractCloud里面有
  1278.         Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
  1279.         // 当前帧位姿
  1280.         Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
  1281.                                                             transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  1282.         // 位姿变换增量
  1283.         Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
  1284.         float x, y, z, roll, pitch, yaw;
  1285.         pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
  1286.         //gc: judge whther should generate key pose
  1287.         // 旋转和平移量都较小，当前帧不设为关键帧
  1288.         if (abs(roll)  < surroundingkeyframeAddingAngleThreshold &&
  1289.             abs(pitch) < surroundingkeyframeAddingAngleThreshold && 
  1290.             abs(yaw)   < surroundingkeyframeAddingAngleThreshold &&
  1291.             sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
  1292.             return false;
  1293.         return true;
  1294.     }
  1295.  
  1296.     void addOdomFactor()
  1297.     {
  1298.         //gc: the first key pose
  1299.         if (cloudKeyPoses3D->points.empty())
  1300.         {
  1301.             noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
  1302.             gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
  1303.             initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
  1304.         }else{
  1305.             noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
  1306.             gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
  1307.             gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);
  1308.             //gc: add constraint between current pose and previous pose
  1309.             gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
  1310.             initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
  1311.         }
  1312.     }
  1313.  
  1314.     void addGPSFactor()
  1315.     {
  1316.         if (gpsQueue.empty())
  1317.             return;
  1318.  
  1319.         // wait for system initialized and settles down
  1320.         if (cloudKeyPoses3D->points.empty())
  1321.             return;
  1322.         else
  1323.         {
  1324.             //gc: the translation between the first and last pose is very small
  1325.             if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
  1326.                 return;
  1327.         }
  1328.  
  1329.         // pose covariance small, no need to correct
  1330.         //gc: the odom covariance is small TODO: although small maybe some interpolation
  1331.         if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
  1332.             return;
  1333.  
  1334.         // last gps position
  1335.         static PointType lastGPSPoint;
  1336.  
  1337.         while (!gpsQueue.empty())
  1338.         {
  1339.             if (gpsQueue.front().header.stamp.toSec() < timeLaserCloudInfoLast - 0.2)
  1340.             {
  1341.                 // message too old
  1342.                 gpsQueue.pop_front();
  1343.             }
  1344.             else if (gpsQueue.front().header.stamp.toSec() > timeLaserCloudInfoLast + 0.2)
  1345.             {
  1346.                 // message too new
  1347.                 break;
  1348.             }
  1349.             else
  1350.             {
  1351.                 nav_msgs::Odometry thisGPS = gpsQueue.front();
  1352.                 gpsQueue.pop_front();
  1353.  
  1354.                 // GPS too noisy, skip
  1355.                 float noise_x = thisGPS.pose.covariance[0];
  1356.                 float noise_y = thisGPS.pose.covariance[7];
  1357.                 float noise_z = thisGPS.pose.covariance[14];
  1358.                 if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
  1359.                     continue;
  1360.  
  1361.                 float gps_x = thisGPS.pose.pose.position.x;
  1362.                 float gps_y = thisGPS.pose.pose.position.y;
  1363.                 float gps_z = thisGPS.pose.pose.position.z;
  1364.                 if (!useGpsElevation)
  1365.                 {
  1366.                     gps_z = transformTobeMapped[5];
  1367.                     noise_z = 0.01;
  1368.                 }
  1369.  
  1370.                 // GPS not properly initialized (0,0,0)
  1371.                 if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
  1372.                     continue;
  1373.  
  1374.                 // Add GPS every a few meters
  1375.                 PointType curGPSPoint;
  1376.                 curGPSPoint.x = gps_x;
  1377.                 curGPSPoint.y = gps_y;
  1378.                 curGPSPoint.z = gps_z;
  1379.                 if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
  1380.                     continue;
  1381.                 else
  1382.                     lastGPSPoint = curGPSPoint;
  1383.  
  1384.                 gtsam::Vector Vector3(3);
  1385.                 Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
  1386.                 noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
  1387.                 gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
  1388.                 gtSAMgraph.add(gps_factor);
  1389.  
  1390.                 aLoopIsClosed = true;
  1391.                 break;
  1392.             }
  1393.         }
  1394.     }
  1395.  
  1396.  
  1397.     /*
  1398.      * 设置当前帧为关键帧并执行因子图优化
  1399.      * 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
  1400.      * 2、添加激光里程计因子、GPS因子、闭环因子
  1401.      * 3、执行因子图优化
  1402.      * 4、得到当前帧优化后位姿，位姿协方差
  1403.      * 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
  1404.     */   
  1405.     void saveKeyFramesAndFactor()
  1406.     {
  1407.         //gc: judge whther should generate key pose
  1408.         // 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
  1409.         if (saveFrame() == false)
  1410.             return;
  1411.  
  1412.         // 往GTSAM里添加因子都是添加的是序号和相对间的位姿变换和noise（一般设置为固定值），
  1413.         // odom factor
  1414.         //gc: add odom factor in the graph
  1415.         addOdomFactor();
  1416.  
  1417.         // gps factor
  1418.         addGPSFactor();
  1419.  
  1420.         // cout << "****************************************************" << endl;
  1421.         // gtSAMgraph.print("GTSAM Graph:\n");
  1422.  
  1423.         // update iSAM
  1424.         // 执行优化
  1425.         isam->update(gtSAMgraph, initialEstimate);
  1426.         isam->update();
  1427.         // update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了
  1428.         gtSAMgraph.resize(0);
  1429.         initialEstimate.clear();
  1430.  
  1431.         //save key poses
  1432.         PointType thisPose3D;
  1433.         PointTypePose thisPose6D;
  1434.         Pose3 latestEstimate;
  1435.  
  1436.         // 优化结果
  1437.         isamCurrentEstimate = isam->calculateEstimate();
  1438.         // 当前位姿结果
  1439.         latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
  1440.         // cout << "****************************************************" << endl;
  1441.         // isamCurrentEstimate.print("Current estimate: ");
  1442.  
  1443.         //gc:cloudKeyPoses3D can be used to calculate the nearest key frames
  1444.         // cloudKeyPoses3D加入当前帧位姿
  1445.         thisPose3D.x = latestEstimate.translation().x();
  1446.         thisPose3D.y = latestEstimate.translation().y();
  1447.         thisPose3D.z = latestEstimate.translation().z();
  1448.         // 索引
  1449.         thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
  1450.         cloudKeyPoses3D->push_back(thisPose3D);
  1451.         // cloudKeyPoses6D加入当前帧位姿
  1452.         thisPose6D.x = thisPose3D.x;
  1453.         thisPose6D.y = thisPose3D.y;
  1454.         thisPose6D.z = thisPose3D.z;
  1455.         thisPose6D.intensity = thisPose3D.intensity ; // this can be used as index
  1456.         thisPose6D.roll  = latestEstimate.rotation().roll();
  1457.         thisPose6D.pitch = latestEstimate.rotation().pitch();
  1458.         thisPose6D.yaw   = latestEstimate.rotation().yaw();
  1459.         thisPose6D.time = timeLaserCloudInfoLast;
  1460.         cloudKeyPoses6D->push_back(thisPose6D);
  1461.  
  1462.         // cout << "****************************************************" << endl;
  1463.         // cout << "Pose covariance:" << endl;
  1464.         // cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
  1465.         // 位姿协方差
  1466.         poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);
  1467.  
  1468.         // save updated transform
  1469.         // transformTobeMapped更新当前帧位姿
  1470.         transformTobeMapped[0] = latestEstimate.rotation().roll();
  1471.         transformTobeMapped[1] = latestEstimate.rotation().pitch();
  1472.         transformTobeMapped[2] = latestEstimate.rotation().yaw();
  1473.         transformTobeMapped[3] = latestEstimate.translation().x();
  1474.         transformTobeMapped[4] = latestEstimate.translation().y();
  1475.         transformTobeMapped[5] = latestEstimate.translation().z();
  1476.  
  1477.         // save all the received edge and surf points
  1478.         // 当前帧激光角点、面点，降采样集合
  1479.         // 把当前帧点云放进全局地图里
  1480.         pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
  1481.         pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
  1482.         //gc:
  1483.         pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
  1484.         pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);
  1485.  
  1486.         // save key frame cloud
  1487.         // 保存特征点降采样集合
  1488.         cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
  1489.         surfCloudKeyFrames.push_back(thisSurfKeyFrame);
  1490.  
  1491.         // save path for visualization
  1492.         // 更新里程计轨迹
  1493.         updatePath(thisPose6D);
  1494.     }
  1495.  
  1496.     void updatePointAssociateToMap()
  1497.     {
  1498.         transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
  1499.     }
  1500.  
  1501.  
  1502.     // 进行完一次图优化之后，改变的不单单是当前帧的位姿，其它节点的位姿也可能变化，所以都更新一下
  1503.     // 更新因子图所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
  1504.     void correctPoses()
  1505.     {
  1506.         if (cloudKeyPoses3D->points.empty())
  1507.             return;
  1508.  
  1509.         if (aLoopIsClosed == true)
  1510.         {
  1511.             // clear path
  1512.             // 清空里程计轨迹
  1513.             globalPath.poses.clear();
  1514.             // update key poses
  1515.             //更新因子图所有变量节点的位姿，也就是所有历史关键帧的位姿，
  1516.             int numPoses = isamCurrentEstimate.size();
  1517.             for (int i = 0; i < numPoses; ++i)
  1518.             {
  1519.                 cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
  1520.                 cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
  1521.                 cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
  1522.  
  1523.                 cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
  1524.                 cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
  1525.                 cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
  1526.                 cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
  1527.                 cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
  1528.                 cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
  1529.  
  1530.                 updatePath(cloudKeyPoses6D->points[i]);
  1531.             }
  1532.  
  1533.             aLoopIsClosed = false;
  1534.             // ID for reseting IMU pre-integration
  1535.             ++imuPreintegrationResetId;
  1536.         }
  1537.     }
  1538.  
  1539.     // 更新里程计轨迹。输入：thisPose6D
  1540.     void updatePath(const PointTypePose& pose_in)
  1541.     {
  1542.         geometry_msgs::PoseStamped pose_stamped;
  1543.         pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
  1544.         pose_stamped.header.frame_id = "odom";
  1545.         pose_stamped.pose.position.x = pose_in.x;
  1546.         pose_stamped.pose.position.y = pose_in.y;
  1547.         pose_stamped.pose.position.z = pose_in.z;
  1548.         tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
  1549.         pose_stamped.pose.orientation.x = q.x();
  1550.         pose_stamped.pose.orientation.y = q.y();
  1551.         pose_stamped.pose.orientation.z = q.z();
  1552.         pose_stamped.pose.orientation.w = q.w();
  1553.  
  1554.         globalPath.poses.push_back(pose_stamped);
  1555.  
  1556.     }
  1557.  
  1558.     // 较原码修改、删除较多
  1559.     void publishOdometry()
  1560.     {
  1561.         // Publish odometry for ROS
  1562.         nav_msgs::Odometry laserOdometryROS;
  1563.         laserOdometryROS.header.stamp = timeLaserInfoStamp;
  1564.         laserOdometryROS.header.frame_id = "odom";
  1565.         laserOdometryROS.child_frame_id = "odom_mapping";
  1566.         laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
  1567.         laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
  1568.         laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
  1569.         laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  1570.         laserOdometryROS.pose.covariance[0] = double(imuPreintegrationResetId);
  1571.         // 没有经过IMU数据融合，单纯由mapOptimization经过GTSAM优化后的位姿是由pubOdomAftMappedROS发布的
  1572.         // 它的话题是"lio_sam/mapping/odometry"，这个是由transformFusion订阅的，用于与IMU里程计进行融合
  1573.         pubOdomAftMappedROS.publish(laserOdometryROS);
  1574.  
  1575.         // 在publishOdometry()里，删除了pubLaserOdometryIncremental，它的话题是"lio_sam/mapping/odometry_incremental"
  1576.         // 它发布的laserOdomIncremental位姿是经过IMU数据加权融合的，这个是由IMUPreintegration订阅的
  1577.         // 用于制作高频的IMU里程计；
  1578.         // 代码段如下：
  1579.         // Publish odometry for ROS (incremental)
  1580.     //     static bool lastIncreOdomPubFlag = false;
  1581.     //     static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
  1582.     //     static Eigen::Affine3f increOdomAffine; // incremental odometry in affine
  1583.     //     //第一次数据直接用全局里程计初始化
  1584.     //     if (lastIncreOdomPubFlag == false)
  1585.     //     {
  1586.     //         lastIncreOdomPubFlag = true;
  1587.     //         laserOdomIncremental = laserOdometryROS;
  1588.     //         increOdomAffine = trans2Affine3f(transformTobeMapped);
  1589.     //     } else {
  1590.     //         // 当前帧与前一帧之间的位姿变换
  1591.     //         Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
  1592.     //         increOdomAffine = increOdomAffine * affineIncre;
  1593.     //         float x, y, z, roll, pitch, yaw;
  1594.     //         pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw);
  1595.     //         if (cloudInfo.imuAvailable == true)
  1596.     //         {
  1597.     //             if (std::abs(cloudInfo.imuPitchInit) < 1.4)
  1598.     //             {
  1599.     //                 double imuWeight = 0.1;
  1600.     //                 tf::Quaternion imuQuaternion;
  1601.     //                 tf::Quaternion transformQuaternion;
  1602.     //                 double rollMid, pitchMid, yawMid;
  1603.  
  1604.     //                 // slerp roll
  1605.     //                 // roll姿态角加权平均
  1606.     //                 transformQuaternion.setRPY(roll, 0, 0);
  1607.     //                 imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
  1608.     //                 tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
  1609.     //                 roll = rollMid;
  1610.  
  1611.     //                 // slerp pitch
  1612.     //                 // pitch姿态角加权平均
  1613.     //                 transformQuaternion.setRPY(0, pitch, 0);
  1614.     //                 imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
  1615.     //                 tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
  1616.     //                 pitch = pitchMid;
  1617.     //             }
  1618.     //         }
  1619.     //         laserOdomIncremental.header.stamp = timeLaserInfoStamp;
  1620.     //         laserOdomIncremental.header.frame_id = odometryFrame;
  1621.     //         laserOdomIncremental.child_frame_id = "odom_mapping";
  1622.     //         laserOdomIncremental.pose.pose.position.x = x;
  1623.     //         laserOdomIncremental.pose.pose.position.y = y;
  1624.     //         laserOdomIncremental.pose.pose.position.z = z;
  1625.     //         laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
  1626.     //         if (isDegenerate)
  1627.     //             laserOdomIncremental.pose.covariance[0] = 1;
  1628.     //         else
  1629.     //             laserOdomIncremental.pose.covariance[0] = 0;
  1630.     //     }
  1631.     //     pubLaserOdometryIncremental.publish(laserOdomIncremental);
  1632.     // }
  1633.  
  1634.         // 较原码还删除了isDegenerate
  1635.         // 这个判断是在scan2map的LMOptimization那里计算匹配的J矩阵时获得的
  1636.         // 如果发生退化，那么标志位covariance[0] = 1，在IMUPreintegration那里的图优化会给当前位姿的计算加一个大的方差。
  1637.         // if (isDegenerate)
  1638.         //     laserOdomIncremental.pose.covariance[0] = 1;
  1639.         // else
  1640.         //     laserOdomIncremental.pose.covariance[0] = 0;
  1641.     
  1642.  
  1643.                 // kitti位姿输出到txt文档
  1644.         std::ofstream pose1("/home/clf/trajectory/lio-sam-tum/test.txt", std::ios::app);
  1645.         std::ofstream pose2("/home/clf/trajectory/lio-sam-tum/test_time.txt", std::ios::app);
  1646.         pose1.setf(std::ios::scientific, std::ios::floatfield);
  1647.         pose2.setf(std::ios::scientific, std::ios::floatfield);
  1648.         // pose1.precision(15);
  1649.  
  1650.         //save final trajectory in the left camera coordinate system.
  1651.         Eigen::Matrix3d rotation_matrix;
  1652.         // rotation_matrix = q_w_curr.toRotationMatrix();
  1653.         rotation_matrix = Eigen::AngleAxisd(transformTobeMapped[2], Eigen::Vector3d::UnitZ()) * 
  1654.                         Eigen::AngleAxisd(transformTobeMapped[1], Eigen::Vector3d::UnitY()) * 
  1655.                         Eigen::AngleAxisd(transformTobeMapped[0], Eigen::Vector3d::UnitX());
  1656.         Eigen::Matrix<double, 4, 4> myaloam_pose;
  1657.         myaloam_pose.topLeftCorner(3,3) = rotation_matrix;
  1658.  
  1659.         myaloam_pose(0,3) = laserOdometryROS.pose.pose.position.x;
  1660.         myaloam_pose(1,3) = laserOdometryROS.pose.pose.position.y;
  1661.         myaloam_pose(2,3) = laserOdometryROS.pose.pose.position.z;
  1662.         Eigen::Matrix<double, 4, 4> cali_paremeter;
  1663.         // cali_paremeter << 4.276802385584e-04, -9.999672484946e-01, -8.084491683471e-03, -1.198459927713e-02,  //00-02
  1664.         //                	 -7.210626507497e-03, 8.081198471645e-03, -9.999413164504e-01, -5.403984729748e-02, 
  1665.         //                   9.999738645903e-01, 4.859485810390e-04, -7.206933692422e-03, -2.921968648686e-01,
  1666.         //                   0,                    0,                   0,                          1;
  1667.         cali_paremeter << -1.857739385241e-03,-9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03,     //04-10
  1668.                         -6.481465826011e-03, 8.051860151134e-03, -9.999466081774e-01, -7.337429464231e-02,
  1669.                         9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01,
  1670.                         0,                   0,                   0,                          1;
  1671.         /*cali_paremeter << 2.347736981471e-04, -9.999441545438e-01, -1.056347781105e-02, -2.796816941295e-03,    // 03
  1672.                             1.044940741659e-02, 1.056535364138e-02, -9.998895741176e-01, -7.510879138296e-02, 
  1673.                             9.999453885620e-01, 1.243653783865e-04, 1.045130299567e-02, -2.721327964059e-01,
  1674.                             0,                     0,                   0,                          1;*/
  1675.                 
  1676.         Eigen::Matrix<double, 4, 4> myloam_pose_f;
  1677.         myloam_pose_f = cali_paremeter * myaloam_pose * cali_paremeter.inverse();
  1678.  
  1679.  
  1680.         pose1 << myloam_pose_f(0,0) << " " << myloam_pose_f(0,1) << " " << myloam_pose_f(0,2) << " " << myloam_pose_f(0,3) << " "
  1681.             << myloam_pose_f(1,0) << " " << myloam_pose_f(1,1) << " " << myloam_pose_f(1,2) << " " << myloam_pose_f(1,3) << " "
  1682.             << myloam_pose_f(2,0) << " " << myloam_pose_f(2,1) << " " << myloam_pose_f(2,2) << " " << myloam_pose_f(2,3) << std::endl;
  1683.  
  1684.         // 获取当前更新的时间 这样与ground turth对比才更准确
  1685.         // 相当于是第一帧的时间
  1686.         static auto T1 = timeLaserInfoStamp;
  1687.         pose2 << laserOdometryROS.header.stamp - T1  << std::endl;
  1688.  
  1689.         pose1.close();
  1690.         pose2.close();
  1691.     }
  1692.  
  1693.     void publishFrames()
  1694.     {
  1695.         if (cloudKeyPoses3D->points.empty())
  1696.             return;
  1697.         // publish key poses
  1698.         publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, "odom");
  1699.         // Publish surrounding key frames
  1700.         publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, "odom");
  1701.         // publish registered key frame
  1702.         //gc: feature points
  1703.         if (pubRecentKeyFrame.getNumSubscribers() != 0)
  1704.         {
  1705.             pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
  1706.             PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
  1707.             *cloudOut += *transformPointCloud(laserCloudCornerLastDS,  &thisPose6D);
  1708.             *cloudOut += *transformPointCloud(laserCloudSurfLastDS,    &thisPose6D);
  1709.             publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, "odom");
  1710.         }
  1711.         // publish registered high-res raw cloud
  1712.         //gc: whole point_cloud of the scan
  1713.         if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
  1714.         {
  1715.             pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
  1716.             pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
  1717.             PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
  1718.             *cloudOut = *transformPointCloud(cloudOut,  &thisPose6D);
  1719.             publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, "odom");
  1720.         }
  1721.         // publish path
  1722.         if (pubPath.getNumSubscribers() != 0)
  1723.         {
  1724.             globalPath.header.stamp = timeLaserInfoStamp;
  1725.             globalPath.header.frame_id = "odom";
  1726.             pubPath.publish(globalPath);
  1727.         }
  1728.     }
  1729. };
  1730.  
  1731.  
  1732. int main(int argc, char** argv)
  1733. {
  1734.     ros::init(argc, argv, "lio_sam");
  1735.  
  1736.     mapOptimization MO;
  1737.  
  1738.     ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");
  1739.     
  1740.     std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
  1741.     std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
  1742.  
  1743.     ros::spin();
  1744.  
  1745.     loopthread.join();
  1746.     visualizeMapThread.join();
  1747.  
  1748.     return 0;
  1749. }

之后会抽空详细写一下kitti数据集的制作、轨迹代码的选择和修改、evo工具的使用等等内容。

最后放几张笔者的测试结果图：

 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/花生_TL007/article/detail/227155
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号