text: #include "utility.h"
  #include "lio_sam/cloud_info.h"
  #include "lio_sam/save_map.h"
  
  #include <gtsam/geometry/Rot3.h>
  #include <gtsam/geometry/Pose3.h>
  #include <gtsam/slam/PriorFactor.h>
  #include <gtsam/slam/BetweenFactor.h>
  #include <gtsam/navigation/GPSFactor.h>
  #include <gtsam/navigation/ImuFactor.h>
  #include <gtsam/navigation/CombinedImuFactor.h>
  #include <gtsam/nonlinear/NonlinearFactorGraph.h>
  #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
  #include <gtsam/nonlinear/Marginals.h>
  #include <gtsam/nonlinear/Values.h>
  #include <gtsam/inference/Symbol.h>
  
  #include <gtsam/nonlinear/ISAM2.h>
  
  using namespace gtsam;
  
  using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
  using symbol_shorthand::G; // GPS pose
  using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
  using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
  
  /*
      * 6D位姿点云结构定义
      */
  struct PointXYZIRPYT
  {
      PCL_ADD_POINT4D
      PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
      float roll;
      float pitch;
      float yaw;
      double time;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
  } EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment
  
  POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRPYT,
                                    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, roll, roll)(float, pitch, pitch)(float, yaw, yaw)(double, time, time))
  
  typedef PointXYZIRPYT PointTypePose;
  
  class mapOptimization : public ParamServer
  {
  
  public:
      // gtsam
      NonlinearFactorGraph gtSAMgraph;
      Values initialEstimate;
      Values optimizedEstimate;
      ISAM2 *isam; // 优化器
      Values isamCurrentEstimate;
      Eigen::MatrixXd poseCovariance;
  
      ros::Publisher pubLaserCloudSurround;
      ros::Publisher pubLaserOdometryGlobal;
      ros::Publisher pubLaserOdometryIncremental;
      ros::Publisher pubKeyPoses;
      ros::Publisher pubPath;
  
      ros::Publisher pubHistoryKeyFrames;
      ros::Publisher pubIcpKeyFrames;
      ros::Publisher pubRecentKeyFrames;
      ros::Publisher pubRecentKeyFrame;
      ros::Publisher pubCloudRegisteredRaw;
      ros::Publisher pubLoopConstraintEdge;
  
      ros::Subscriber subCloud;
      ros::Subscriber subGPS;
      ros::Subscriber subLoop;
  
      ros::ServiceServer srvSaveMap;
  
      std::deque<nav_msgs::Odometry> gpsQueue;
      lio_sam::cloud_info cloudInfo;
  
      // 历史所有关键帧（已降采样）的角点、平面点集合
      vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
      vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
  
      // 历史关键帧优化后的位置,X Y Z I（i表示关键帧index）
      pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
      // 历史关键帧优化后的位置,XYZI、RPY、time，始终与3D保持同步
      pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
      // 拷贝是用于回环检测的
      pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;
      pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;
  
      // 当前激光帧角点集合
      pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization
      // 当前激光帧平面点集合
      pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
      // 当前激光帧角点、平面点集合，降采样，DS: DownSize
      pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
      pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS;   // downsampled surf featuer set from odoOptimization
      // 当前帧与局部map匹配上了的角点、平面点，加入同一集合；coeffSel是对应点的参数
      pcl::PointCloud<PointType>::Ptr laserCloudOri;
      pcl::PointCloud<PointType>::Ptr coeffSel;
      // 当前帧与局部map匹配上了的角点、参数、标记
      std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
      std::vector<PointType> coeffSelCornerVec;
      std::vector<bool> laserCloudOriCornerFlag;
      // 当前帧与局部map匹配上了的平面点、参数、标记
      std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
      std::vector<PointType> coeffSelSurfVec;
      std::vector<bool> laserCloudOriSurfFlag;
  
      map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
      // 局部map的角点集合
      pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
      // 局部map的平面点集合
      pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
      pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
      pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
      // 局部关键帧构建的map点云，对应kdtree，用于scan-to-map找相邻点
      pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
      pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
  
      pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
      pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
      // 体素降采样滤波器
      pcl::VoxelGrid<PointType> downSizeFilterCorner;
      pcl::VoxelGrid<PointType> downSizeFilterSurf;
      pcl::VoxelGrid<PointType> downSizeFilterICP;
      pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
  
      ros::Time timeLaserInfoStamp;
      double timeLaserInfoCur;
      // 依次为 R P Y ,X Y Z，表示激光所在位置，初始估计和优化都会对其进行更新
      float transformTobeMapped[6];
  
      std::mutex mtx; // 点云相关操作的互斥量
      std::mutex mtxLoopInfo;
      // 该值反映了在第一次迭代时，该方向特征值超过了阈值
      bool isDegenerate = false;
      cv::Mat matP;
      // 局部map角点数量
      int laserCloudCornerFromMapDSNum = 0;
      // 局部map平面点数量
      int laserCloudSurfFromMapDSNum = 0;
      // 当前激光帧角点数量
      int laserCloudCornerLastDSNum = 0;
      // 当前激光帧面点数量
      int laserCloudSurfLastDSNum = 0;
  
      bool aLoopIsClosed = false;
      map<int, int> loopIndexContainer; // from new to old
      vector<pair<int, int>> loopIndexQueue;
      vector<gtsam::Pose3> loopPoseQueue;
      vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;
      deque<std_msgs::Float64MultiArray> loopInfoVec;
  
      nav_msgs::Path globalPath;
  
      // 当前帧位姿
      Eigen::Affine3f transPointAssociateToMap;
      // 前一帧位姿，来自前一次因子图优化的结果
      Eigen::Affine3f incrementalOdometryAffineFront;
      // 当前帧位姿，来自激光里程计的结果
      Eigen::Affine3f incrementalOdometryAffineBack;
      /**
       * 构造函数
      */
      mapOptimization()
      {
          ISAM2Params parameters;
          parameters.relinearizeThreshold = 0.1;
          parameters.relinearizeSkip = 1;
          isam = new ISAM2(parameters);
          // 发布历史关键帧里程计
          pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
          // 发布全局关键帧map的特征点云
          pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
          // 发布激光里程计，rviz中表现为坐标轴
          pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry>("lio_sam/mapping/odometry", 1);
          //TODO 发布激光里程计，它与上面的激光里程计基本一样，只是roll、pitch用imu数据加权平均了一下，z做了限制（没看到）
          pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 1);
          // 发布激光里程计路径，rviz中表现为载体的运行轨迹
          pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
  
          // 订阅当前激光帧点云信息，来自featureExtraction
          subCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
          // 订阅GPS里程计
          subGPS = nh.subscribe<nav_msgs::Odometry>(gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
          // 订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上
          subLoop = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());
          // 发布地图保存服务
          srvSaveMap = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);
          // 发布闭环匹配关键帧局部map
          pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
          // 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云
          pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
          // 发布闭环边，rviz中表现为闭环帧之间的连线
          pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1);
          // 发布局部map的降采样平面点集合
          pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
          // 发布历史帧（累加的）的角点、平面点降采样集合
          pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
          // 发布当前帧原始点云配准之后的点云
          pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);
  
          downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
          downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
          downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
          downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
  
          allocateMemory();
      }
      /**
       * 初始化
      */
      void allocateMemory()
      {
          cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
          cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
          copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
          copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
  
          kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
          kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
  
          laserCloudCornerLast.reset(new pcl::PointCloud<PointType>());   // corner feature set from odoOptimization
          laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());     // surf feature set from odoOptimization
          laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
          laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>());   // downsampled surf featuer set from odoOptimization
  
          laserCloudOri.reset(new pcl::PointCloud<PointType>());
          coeffSel.reset(new pcl::PointCloud<PointType>());
  
          laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
          coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
          laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
          laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
          coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
          laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
  
          std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
          std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
  
          laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
          laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
          laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
          laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
  
          kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
          kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
  
          for (int i = 0; i < 6; ++i)
          {
              transformTobeMapped[i] = 0;
          }
  
          matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
      }
      /**
       * 订阅当前激光帧点云信息，来自featureExtraction
       * 1、当前帧位姿初始化
       *   1) 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
       *   2) 后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
       * 2、提取局部角点、平面点云集合，加入局部map
       *   1) 对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
       *   2) 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
       * 3、当前激光帧角点、平面点集合降采样
       * 4、scan-to-map优化当前帧位姿
       *   (1) 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
       *   (2) 迭代30次（上限）优化
       *      1) 当前激光帧角点寻找局部map匹配点
       *          a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
       *          b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
       *      2) 当前激光帧平面点寻找局部map匹配点
       *          a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
       *          b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
       *      3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
       *      4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
       *   (3)用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
       * 5、设置当前帧为关键帧并执行因子图优化
       *   1) 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
       *   2) 添加激光里程计因子、GPS因子、闭环因子
       *   3) 执行因子图优化
       *   4) 得到当前帧优化后位姿，位姿协方差
       *   5) 添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
       * 6、更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
       * 7、发布激光里程计
       * 8、发布里程计、点云、轨迹
      */
      void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn)
      {
          // 当前激光帧时间戳
          timeLaserInfoStamp = msgIn->header.stamp;
          timeLaserInfoCur = msgIn->header.stamp.toSec();
  
          // 提取当前激光帧角点、平面点集合
          cloudInfo = *msgIn;
          pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
          pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
  
          std::lock_guard<std::mutex> lock(mtx);
          // 上一个点云的时间戳
          static double timeLastProcessing = -1;
          // mapping执行频率控制，默认0.15s
          if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
          {
              timeLastProcessing = timeLaserInfoCur;
              // 当前帧位姿初始化。并不是基于匀速运动假设，而是用imu的增量进行初始化。
              // 1、如果是第一帧，或者是imu里程计出问题了，则用原始imu数据的RPY初始化当前帧位姿（仅旋转部分，感觉没必要舍弃位移）
              // 2、后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿，存于transformTobeMapped
              updateInitialGuess();
              // 提取附近关键帧，并组成局部角-边点地图
              // 1. KD树提取上一个点云帧附近的关键帧位置，并进行降采样
              // 2. 添加早于当前帧10s内的关键帧
              // 3. 将1、2中的地点的关键帧里的角点和平面点添加到Local里
              // 4. Local降采样
              extractSurroundingKeyFrames();
              //对当前帧的角点和边点进行降采样
              downsampleCurrentScan();
              // scan-to-map优化当前帧位姿
              // 1、要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
              // 2、迭代30次（上限）优化
              //    1) 当前激光帧角点寻找局部map匹配点
              //       a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
              //       b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
              //    2) 当前激光帧平面点寻找局部map匹配点
              //       a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
              //       b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
              //    3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
              //    4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
              // 3、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
              scan2MapOptimization();
              // 设置当前帧为关键帧并执行因子图优化
              // 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
              // 2、添加激光里程计因子、GPS因子、闭环因子
              // 3、执行因子图优化
              // 4、得到当前帧优化后位姿，位姿协方差
              // 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
              saveKeyFramesAndFactor();
              // 当有GPS或闭环时，更新因子图中所有变量节点（历史关键帧）的位姿，更新里程计轨迹
              correctPoses();
              // 发布激光里程计
              publishOdometry();
              // 发布里程计、点云、轨迹
              // 1、发布历史关键帧位姿集合
              // 2、发布局部map的降采样平面点集合
              // 3、发布历史帧（累加的）的角点、平面点降采样集合
              // 4、发布里程计轨迹
              publishFrames();
          }
      }
  
      void gpsHandler(const nav_msgs::Odometry::ConstPtr &gpsMsg)
      {
          gpsQueue.push_back(*gpsMsg);
      }
  
      /**
       * @brief 根据当前帧的位姿，将点pi投影到世界坐标系
       * 
       * @param pi 输入点
       * @param po 输出点
       */
      void pointAssociateToMap(PointType const *const pi, PointType *const po)
      {
          po->x = transPointAssociateToMap(0, 0) * pi->x + transPointAssociateToMap(0, 1) * pi->y + transPointAssociateToMap(0, 2) * pi->z + transPointAssociateToMap(0, 3);
          po->y = transPointAssociateToMap(1, 0) * pi->x + transPointAssociateToMap(1, 1) * pi->y + transPointAssociateToMap(1, 2) * pi->z + transPointAssociateToMap(1, 3);
          po->z = transPointAssociateToMap(2, 0) * pi->x + transPointAssociateToMap(2, 1) * pi->y + transPointAssociateToMap(2, 2) * pi->z + transPointAssociateToMap(2, 3);
          po->intensity = pi->intensity;
      }
  /**
   * @brief 根据6D，将点云转换至直接坐标系
   * 
   * @param cloudIn 原始点云
   * @param transformIn 6D点
   * @return pcl::PointCloud<PointType>::Ptr 
   */
      pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn)
      {
          pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
  
          int cloudSize = cloudIn->size();
          cloudOut->resize(cloudSize);
  
          Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
  
  #pragma omp parallel for num_threads(numberOfCores)
          for (int i = 0; i < cloudSize; ++i)
          {
              const auto &pointFrom = cloudIn->points[i];
              cloudOut->points[i].x = transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
              cloudOut->points[i].y = transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
              cloudOut->points[i].z = transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
              cloudOut->points[i].intensity = pointFrom.intensity;
          }
          return cloudOut;
      }
  
      gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
      {
          return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
                              gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
      }
  
      gtsam::Pose3 trans2gtsamPose(float transformIn[])
      {
          return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
                              gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
      }
  
      Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
      {
          return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
      }
  
      Eigen::Affine3f trans2Affine3f(float transformIn[])
      {
          return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
      }
  
      PointTypePose trans2PointTypePose(float transformIn[])
      {
          PointTypePose thisPose6D;
          thisPose6D.x = transformIn[3];
          thisPose6D.y = transformIn[4];
          thisPose6D.z = transformIn[5];
          thisPose6D.roll = transformIn[0];
          thisPose6D.pitch = transformIn[1];
          thisPose6D.yaw = transformIn[2];
          return thisPose6D;
      }
  
      bool saveMapService(lio_sam::save_mapRequest &req, lio_sam::save_mapResponse &res)
      {
          string saveMapDirectory;
  
          cout << "****************************************************" << endl;
          cout << "Saving map to pcd files ..." << endl;
          if (req.destination.empty())
              saveMapDirectory = std::getenv("HOME") + savePCDDirectory;
          else
              saveMapDirectory = std::getenv("HOME") + req.destination;
          cout << "Save destination: " << saveMapDirectory << endl;
          // create directory and remove old files; 为了不删除文件夹，故注释
          // int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());
          // unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());
          // save key frame transformations
          pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);
          pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);
          // extract global point cloud map
          pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
          for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++)
          {
              *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
              *globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
              cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
          }
  
          if (req.resolution != 0)
          {
              cout << "\n\nSave resolution: " << req.resolution << endl;
  
              // down-sample and save corner cloud
              downSizeFilterCorner.setInputCloud(globalCornerCloud);
              downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);
              downSizeFilterCorner.filter(*globalCornerCloudDS);
              pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);
              // down-sample and save surf cloud
              downSizeFilterSurf.setInputCloud(globalSurfCloud);
              downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution);
              downSizeFilterSurf.filter(*globalSurfCloudDS);
              pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);
          }
          else
          {
              // save corner cloud
              pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);
              // save surf cloud
              pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);
          }
  
          // save global point cloud map
          *globalMapCloud += *globalCornerCloud;
          *globalMapCloud += *globalSurfCloud;
  
          int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);
          res.success = ret == 0;
  
          downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
          downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
  
          cout << "****************************************************" << endl;
          cout << "Saving map to pcd files completed\n"
               << endl;
  
          return true;
      }
  
      void visualizeGlobalMapThread()
      {
          ros::Rate rate(0.2);
          while (ros::ok())
          {
              rate.sleep();
              publishGlobalMap();
          }
  
          if (savePCD == false)
              return;
  
          lio_sam::save_mapRequest req;
          lio_sam::save_mapResponse res;
  
          if (!saveMapService(req, res))
          {
              cout << "Fail to save map" << endl;
          }
      }
  
      void publishGlobalMap()
      {
          if (pubLaserCloudSurround.getNumSubscribers() == 0)
              return;
  
          if (cloudKeyPoses3D->points.empty() == true)
              return;
  
          pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());
          ;
          pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());
  
          // kd-tree to find near key frames to visualize
          std::vector<int> pointSearchIndGlobalMap;
          std::vector<float> pointSearchSqDisGlobalMap;
          // search near key frames to visualize
          mtx.lock();
          kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
          kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
          mtx.unlock();
  
          for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
              globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
          // downsample near selected key frames
          pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses;                                                                                            // for global map visualization
          downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
          downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
          downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
          for (auto &pt : globalMapKeyPosesDS->points)
          {
              kdtreeGlobalMap->nearestKSearch(pt, 1, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);
              pt.intensity = cloudKeyPoses3D->points[pointSearchIndGlobalMap[0]].intensity;
          }
  
          // extract visualized and downsampled key frames
          for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i)
          {
              if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
                  continue;
              int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
              *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
              *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
          }
          // downsample visualized points
          pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames;                                                                                   // for global map visualization
          downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
          downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
          downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
          publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);
      }
  
      void loopClosureThread()
      {
          if (loopClosureEnableFlag == false)
              return;
  
          ros::Rate rate(loopClosureFrequency);
          while (ros::ok())
          {
              rate.sleep();
              performLoopClosure();
              visualizeLoopClosure();
          }
      }
  
      void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr &loopMsg)
      {
          std::lock_guard<std::mutex> lock(mtxLoopInfo);
          if (loopMsg->data.size() != 2)
              return;
  
          loopInfoVec.push_back(*loopMsg);
  
          while (loopInfoVec.size() > 5)
              loopInfoVec.pop_front();
      }
  
      void performLoopClosure()
      {
          if (cloudKeyPoses3D->points.empty() == true)
              return;
  
          mtx.lock();
          *copy_cloudKeyPoses3D = *cloudKeyPoses3D;
          *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
          mtx.unlock();
  
          // find keys
          int loopKeyCur; // 上一个关键帧
          int loopKeyPre; // 候选帧
          // 接受外部的回环结果，目前没被使用
          if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
              if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
                  return;
  
          // extract cloud
          pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
          {
              loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
              loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
              if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
                  return;
              if (pubHistoryKeyFrames.getNumSubscribers() != 0)
                  publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
          }
  
          // ICP Settings
          static pcl::IterativeClosestPoint<PointType, PointType> icp;
          icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius * 2);
          icp.setMaximumIterations(100);
          icp.setTransformationEpsilon(1e-6);
          icp.setEuclideanFitnessEpsilon(1e-6);
          icp.setRANSACIterations(0);
  
          // Align clouds
          icp.setInputSource(cureKeyframeCloud);
          icp.setInputTarget(prevKeyframeCloud);
          pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
          icp.align(*unused_result);
          // fitnessScore动态场景不见得好用
          if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
              return;
  
          // publish corrected cloud
          if (pubIcpKeyFrames.getNumSubscribers() != 0)
          {
              pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
              pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
              publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
          }
  
          // Get pose transformation
          float x, y, z, roll, pitch, yaw;
          Eigen::Affine3f correctionLidarFrame;
          correctionLidarFrame = icp.getFinalTransformation();
          // transform from world origin to wrong pose
          Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
          // transform from world origin to corrected pose
          Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong; // pre-multiplying -> successive rotation about a fixed frame
          pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
          gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
          gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
          gtsam::Vector Vector6(6);
          float noiseScore = icp.getFitnessScore();
          Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
          noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
  
          // Add pose constraint
          mtx.lock();
          loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
          // from^(-1)*To
          loopPoseQueue.push_back(poseFrom.between(poseTo));
          loopNoiseQueue.push_back(constraintNoise);
          mtx.unlock();
  
          // add loop constriant
          loopIndexContainer[loopKeyCur] = loopKeyPre;
      }
  
      bool detectLoopClosureDistance(int *latestID, int *closestID)
      {
          int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
          int loopKeyPre = -1;
  
          // 若上一帧已经被加入队列了，则跳过
          auto it = loopIndexContainer.find(loopKeyCur);
          if (it != loopIndexContainer.end())
              return false;
  
          // find the closest history key frame
          std::vector<int> pointSearchIndLoop;
          std::vector<float> pointSearchSqDisLoop;
          kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
          kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
          // 仅考虑时间戳最远的那一帧
          for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
          {
              int id = pointSearchIndLoop[i];
              if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
              {
                  loopKeyPre = id;
                  break;
              }
          }
  
          if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
              return false;
  
          *latestID = loopKeyCur;
          *closestID = loopKeyPre;
  
          return true;
      }
  
      bool detectLoopClosureExternal(int *latestID, int *closestID)
      {
          // this function is not used yet, please ignore it
          int loopKeyCur = -1;
          int loopKeyPre = -1;
  
          std::lock_guard<std::mutex> lock(mtxLoopInfo);
          if (loopInfoVec.empty())
              return false;
  
          double loopTimeCur = loopInfoVec.front().data[0];
          double loopTimePre = loopInfoVec.front().data[1];
          loopInfoVec.pop_front();
  
          if (abs(loopTimeCur - loopTimePre) < historyKeyframeSearchTimeDiff)
              return false;
  
          int cloudSize = copy_cloudKeyPoses6D->size();
          if (cloudSize < 2)
              return false;
  
          // latest key
          loopKeyCur = cloudSize - 1;
          for (int i = cloudSize - 1; i >= 0; --i)
          {
              if (copy_cloudKeyPoses6D->points[i].time >= loopTimeCur)
                  loopKeyCur = round(copy_cloudKeyPoses6D->points[i].intensity);
              else
                  break;
          }
  
          // previous key
          loopKeyPre = 0;
          for (int i = 0; i < cloudSize; ++i)
          {
              if (copy_cloudKeyPoses6D->points[i].time <= loopTimePre)
                  loopKeyPre = round(copy_cloudKeyPoses6D->points[i].intensity);
              else
                  break;
          }
  
          if (loopKeyCur == loopKeyPre)
              return false;
  
          auto it = loopIndexContainer.find(loopKeyCur);
          if (it != loopIndexContainer.end())
              return false;
  
          *latestID = loopKeyCur;
          *closestID = loopKeyPre;
  
          return true;
      }
  /**
   * @brief 搜索key附近searchNum帧点云，将其特征点组合成局部特征地图。
   * 
   * @param nearKeyframes out 拼接的局部特征地图
   * @param key 点云序号
   * @param searchNum 局部地图大小
   */
      void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr &nearKeyframes, const int &key, const int &searchNum)
      {
          // extract near keyframes
          nearKeyframes->clear();
          int cloudSize = copy_cloudKeyPoses6D->size();
          // 若searchNum为0则跳过for，否则将前后若干帧的角点和面点都集合起来。
          for (int i = -searchNum; i <= searchNum; ++i)
          {
              int keyNear = key + i;
              if (keyNear < 0 || keyNear >= cloudSize)
                  continue;
              *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
              *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
          }
  
          if (nearKeyframes->empty())
              return;
  
          // downsample near keyframes
          pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
          downSizeFilterICP.setInputCloud(nearKeyframes);
          downSizeFilterICP.filter(*cloud_temp);
          *nearKeyframes = *cloud_temp;
      }
  
      void visualizeLoopClosure()
      {
          if (loopIndexContainer.empty())
              return;
  
          visualization_msgs::MarkerArray markerArray;
          // loop nodes
          visualization_msgs::Marker markerNode;
          markerNode.header.frame_id = odometryFrame;
          markerNode.header.stamp = timeLaserInfoStamp;
          markerNode.action = visualization_msgs::Marker::ADD;
          markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
          markerNode.ns = "loop_nodes";
          markerNode.id = 0;
          markerNode.pose.orientation.w = 1;
          markerNode.scale.x = 0.3;
          markerNode.scale.y = 0.3;
          markerNode.scale.z = 0.3;
          markerNode.color.r = 0;
          markerNode.color.g = 0.8;
          markerNode.color.b = 1;
          markerNode.color.a = 1;
          // loop edges
          visualization_msgs::Marker markerEdge;
          markerEdge.header.frame_id = odometryFrame;
          markerEdge.header.stamp = timeLaserInfoStamp;
          markerEdge.action = visualization_msgs::Marker::ADD;
          markerEdge.type = visualization_msgs::Marker::LINE_LIST;
          markerEdge.ns = "loop_edges";
          markerEdge.id = 1;
          markerEdge.pose.orientation.w = 1;
          markerEdge.scale.x = 0.1;
          markerEdge.color.r = 0.9;
          markerEdge.color.g = 0.9;
          markerEdge.color.b = 0;
          markerEdge.color.a = 1;
  
          for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
          {
              int key_cur = it->first;
              int key_pre = it->second;
              geometry_msgs::Point p;
              p.x = copy_cloudKeyPoses6D->points[key_cur].x;
              p.y = copy_cloudKeyPoses6D->points[key_cur].y;
              p.z = copy_cloudKeyPoses6D->points[key_cur].z;
              markerNode.points.push_back(p);
              markerEdge.points.push_back(p);
              p.x = copy_cloudKeyPoses6D->points[key_pre].x;
              p.y = copy_cloudKeyPoses6D->points[key_pre].y;
              p.z = copy_cloudKeyPoses6D->points[key_pre].z;
              markerNode.points.push_back(p);
              markerEdge.points.push_back(p);
          }
  
          markerArray.markers.push_back(markerNode);
          markerArray.markers.push_back(markerEdge);
          pubLoopConstraintEdge.publish(markerArray);
      }
  
      void updateInitialGuess()
      {
          // save current transformation before any processing
          incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
  
          static Eigen::Affine3f lastImuTransformation;
          // initialization
          if (cloudKeyPoses3D->points.empty())
          {
              transformTobeMapped[0] = cloudInfo.imuRollInit;
              transformTobeMapped[1] = cloudInfo.imuPitchInit;
              transformTobeMapped[2] = cloudInfo.imuYawInit;
  
              if (!useImuHeadingInitialization)
                  transformTobeMapped[2] = 0;
  
              lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
              return;
          }
  
          // use imu pre-integration estimation for pose guess
          static bool lastImuPreTransAvailable = false;
          static Eigen::Affine3f lastImuPreTransformation;
          if (cloudInfo.odomAvailable == true)
          {
              Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
                                                                 cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
              if (lastImuPreTransAvailable == false)
              {
                  lastImuPreTransformation = transBack;
                  lastImuPreTransAvailable = true;
              }
              else
              {
                  Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
                  Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
                  Eigen::Affine3f transFinal = transTobe * transIncre;
                  pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                    transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  
                  lastImuPreTransformation = transBack;
  
                  lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
                  return;
              }
          }
  
          // use imu incremental estimation for pose guess (only rotation)
          if (cloudInfo.imuAvailable == true)
          {
              Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
              Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
  
              Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
              Eigen::Affine3f transFinal = transTobe * transIncre;
              pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  
              lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
              return;
          }
      }
  
      void extractForLoopClosure()
      {
          pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
          int numPoses = cloudKeyPoses3D->size();
          for (int i = numPoses - 1; i >= 0; --i)
          {
              if ((int)cloudToExtract->size() <= surroundingKeyframeSize)
                  cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
              else
                  break;
          }
  
          extractCloud(cloudToExtract);
      }
  
      void extractNearby()
      {
          pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
          std::vector<int> pointSearchInd;
          std::vector<float> pointSearchSqDis;
  
          // extract all the nearby key poses and downsample them
          kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
          kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
          for (int i = 0; i < (int)pointSearchInd.size(); ++i)
          {
              int id = pointSearchInd[i];
              surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
          }
  
          downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
          downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
          for (auto &pt : surroundingKeyPosesDS->points)
          {
              kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
              pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
          }
  
          // also extract some latest key frames in case the robot rotates in one position
          int numPoses = cloudKeyPoses3D->size();
          for (int i = numPoses - 1; i >= 0; --i)
          {
              if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
                  surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
              else
                  break;
          }
  
          extractCloud(surroundingKeyPosesDS);
      }
  
      void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
      {
          // fuse the map
          laserCloudCornerFromMap->clear();
          laserCloudSurfFromMap->clear();
          for (int i = 0; i < (int)cloudToExtract->size(); ++i)
          {
              if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
                  continue;
  
              int thisKeyInd = (int)cloudToExtract->points[i].intensity;
              if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end())
              {
                  // transformed cloud available
                  *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
                  *laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
              }
              else
              {
                  // transformed cloud not available
                  pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
                  pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
                  *laserCloudCornerFromMap += laserCloudCornerTemp;
                  *laserCloudSurfFromMap += laserCloudSurfTemp;
                  laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
              }
          }
  
          // Downsample the surrounding corner key frames (or map)
          downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
          downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
          laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
          // Downsample the surrounding surf key frames (or map)
          downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
          downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
          laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
  
          // clear map cache if too large
          if (laserCloudMapContainer.size() > 1000)
              laserCloudMapContainer.clear();
      }
  
      void extractSurroundingKeyFrames()
      {
          if (cloudKeyPoses3D->points.empty() == true)
              return;
  
          // if (loopClosureEnableFlag == true)
          // {
          //     extractForLoopClosure();
          // } else {
          //     extractNearby();
          // }
  
          extractNearby();
      }
  
      void downsampleCurrentScan()
      {
          // Downsample cloud from current scan
          laserCloudCornerLastDS->clear();
          downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
          downSizeFilterCorner.filter(*laserCloudCornerLastDS);
          laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
  
          laserCloudSurfLastDS->clear();
          downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
          downSizeFilterSurf.filter(*laserCloudSurfLastDS);
          laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
      }
  
      void updatePointAssociateToMap()
      {
          transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
      }
      /**
       * 当前激光帧角点寻找局部map匹配点
       * 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
       * 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
      */
      void cornerOptimization()
      {
          // 从transformTobeMapped获取位姿
          updatePointAssociateToMap();
  
  #pragma omp parallel for num_threads(numberOfCores)
          for (int i = 0; i < laserCloudCornerLastDSNum; i++)
          {
              PointType pointOri, pointSel, coeff;
              std::vector<int> pointSearchInd;
              std::vector<float> pointSearchSqDis;
  
              pointOri = laserCloudCornerLastDS->points[i];
              pointAssociateToMap(&pointOri, &pointSel);
              kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
  
              cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
              cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
              cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
  
              if (pointSearchSqDis[4] < 1.0)
              {
                  // 计算五个近邻的质心
                  float cx = 0, cy = 0, cz = 0;
                  for (int j = 0; j < 5; j++)
                  {
                      cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                      cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                      cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                  }
                  cx /= 5;
                  cy /= 5;
                  cz /= 5;
                  // 计算质心的协方差矩阵
                  float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                  for (int j = 0; j < 5; j++)
                  {
                      float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                      float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                      float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
  
                      a11 += ax * ax;
                      a12 += ax * ay;
                      a13 += ax * az;
                      a22 += ay * ay;
                      a23 += ay * az;
                      a33 += az * az;
                  }
                  a11 /= 5;
                  a12 /= 5;
                  a13 /= 5;
                  a22 /= 5;
                  a23 /= 5;
                  a33 /= 5;
  
                  matA1.at<float>(0, 0) = a11;
                  matA1.at<float>(0, 1) = a12;
                  matA1.at<float>(0, 2) = a13;
                  matA1.at<float>(1, 0) = a12;
                  matA1.at<float>(1, 1) = a22;
                  matA1.at<float>(1, 2) = a23;
                  matA1.at<float>(2, 0) = a13;
                  matA1.at<float>(2, 1) = a23;
                  matA1.at<float>(2, 2) = a33;
                  // 计算特征值和特征向量
                  cv::eigen(matA1, matD1, matV1);
                  // 若最大特征值远大于次大特征值，认为是线
                  if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1))
                  {
                      // 当前帧角点坐标（map系下）
                      float x0 = pointSel.x;
                      float y0 = pointSel.y;
                      float z0 = pointSel.z;
                      // 局部map对应中心角点，沿着特征向量（直线方向）方向，前后各取一个点
                      float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                      float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                      float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                      float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                      float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                      float z2 = cz - 0.1 * matV1.at<float>(0, 2);
                      // area_012，也就是三个点组成的三角形面积*2，叉积的模|axb|=a*b*sin(theta)
                      float a012 = sqrt(((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)));
                      // line_12，底边边长
                      float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));
                      //两次叉积，得到点到直线的垂线段单位向量，x分量，下面同理
                      // (la,lb,lc)=\frac{AB}{|AB|}\times\frac{OA\times OB}{|OA\times OB|}
                      float la = ((y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + (z1 - z2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))) / a012 / l12;
  
                      float lb = -((x1 - x2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) - (z1 - z2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;
  
                      float lc = -((x1 - x2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + (y1 - y2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;
                      // 三角形的高，也就是点到直线距离
                      float ld2 = a012 / l12;
                      // 距离越大，s越小，是个距离惩罚因子（权重）
                      float s = 1 - 0.9 * fabs(ld2);
                      // xyzi分别表示距离对平移的偏导和残差项
                      // 点到直线的垂线段单位向量
                      coeff.x = s * la;
                      coeff.y = s * lb;
                      coeff.z = s * lc;
                      // 点到直线距离
                      coeff.intensity = s * ld2;
  
                      if (s > 0.1)
                      {
                          laserCloudOriCornerVec[i] = pointOri;
                          coeffSelCornerVec[i] = coeff;
                          laserCloudOriCornerFlag[i] = true;
                      }
                  }
              }
          }
      }
  
      void surfOptimization()
      {
          // 更新当前帧位姿
          updatePointAssociateToMap();
          // 遍历当前帧平面点集合
  #pragma omp parallel for num_threads(numberOfCores)
          for (int i = 0; i < laserCloudSurfLastDSNum; i++)
          {
              PointType pointOri, pointSel, coeff;
              std::vector<int> pointSearchInd;
              std::vector<float> pointSearchSqDis;
              // 平面点（坐标还是lidar系）
              pointOri = laserCloudSurfLastDS->points[i];
              // 根据当前帧位姿，变换到世界坐标系（map系）下
              pointAssociateToMap(&pointOri, &pointSel);
              // 在局部平面点map中查找当前平面点相邻的5个平面点
              kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
  
              Eigen::Matrix<float, 5, 3> matA0;
              Eigen::Matrix<float, 5, 1> matB0;
              Eigen::Vector3f matX0;
  
              matA0.setZero();
              matB0.fill(-1);
              matX0.setZero();
              // 要求距离都小于1m
              if (pointSearchSqDis[4] < 1.0)
              {
                  for (int j = 0; j < 5; j++)
                  {
                      matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                      matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                      matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                  }
                  // 假设平面方程为ax+by+cz+d=0，这里就是求方程的系数abc，matB0=-d=-1
                  //colPivHouseholderQr是求解matA0 x = matB0 这个方程式。
                  matX0 = matA0.colPivHouseholderQr().solve(matB0);
  
                  float pa = matX0(0, 0);
                  float pb = matX0(1, 0);
                  float pc = matX0(2, 0);
                  float pd = 1;
                  // 单位法向量
                  float ps = sqrt(pa * pa + pb * pb + pc * pc);
                  pa /= ps;
                  pb /= ps;
                  pc /= ps;
                  pd /= ps;
                  // 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面
                  bool planeValid = true;
                  for (int j = 0; j < 5; j++)
                  {
                      if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                               pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                               pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2)
                      {
                          planeValid = false;
                          break;
                      }
                  }
                  // 平面合格
                  if (planeValid)
                  {
                      // 当前激光帧点到平面距离
                      float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
                      // 惩罚当前点离平面远的情况，同时增加远处点的权重。
                      float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
                      // 点到平面垂线比例法向量（其实等价于平面法向量）
                      coeff.x = s * pa;
                      coeff.y = s * pb;
                      coeff.z = s * pc;
                      // 点到平面比例距离
                      coeff.intensity = s * pd2;
  
                      if (s > 0.1)
                      {
                          // 当前激光帧平面点，加入匹配集合中
                          laserCloudOriSurfVec[i] = pointOri;
                          // 参数
                          coeffSelSurfVec[i] = coeff;
                          laserCloudOriSurfFlag[i] = true;
                      }
                  }
              }
          }
      }
  
      void combineOptimizationCoeffs()
      {
          // combine corner coeffs
          for (int i = 0; i < laserCloudCornerLastDSNum; ++i)
          {
              if (laserCloudOriCornerFlag[i] == true)
              {
                  laserCloudOri->push_back(laserCloudOriCornerVec[i]);
                  coeffSel->push_back(coeffSelCornerVec[i]);
              }
          }
          // combine surf coeffs
          for (int i = 0; i < laserCloudSurfLastDSNum; ++i)
          {
              if (laserCloudOriSurfFlag[i] == true)
              {
                  laserCloudOri->push_back(laserCloudOriSurfVec[i]);
                  coeffSel->push_back(coeffSelSurfVec[i]);
              }
          }
          // reset flag for next iteration
          std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
          std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
      }
  
      bool LMOptimization(int iterCount)
      {
          // 这个优化程序似乎是为camera写的，因此数据需要被转换到camere坐标系里。
          // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
          // lidar <- camera      ---     camera <- lidar
          // x = z                ---     x = y
          // y = x                ---     y = z
          // z = y                ---     z = x
          // roll = yaw           ---     roll = pitch
          // pitch = roll         ---     pitch = yaw
          // yaw = pitch          ---     yaw = roll
          // srx crx分别表示 sin(roll)和cos(roll),下面类似
          // lidar -> camera
          float srx = sin(transformTobeMapped[1]);
          float crx = cos(transformTobeMapped[1]);
          float sry = sin(transformTobeMapped[2]);
          float cry = cos(transformTobeMapped[2]);
          float srz = sin(transformTobeMapped[0]);
          float crz = cos(transformTobeMapped[0]);
  
          int laserCloudSelNum = laserCloudOri->size();
          if (laserCloudSelNum < 50)
          {
              return false;
          }
  
          cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
          cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
          cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
          cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
          cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
          cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
  
          PointType pointOri, coeff;
  
          for (int i = 0; i < laserCloudSelNum; i++)
          {
              // lidar -> camera
              pointOri.x = laserCloudOri->points[i].y;
              pointOri.y = laserCloudOri->points[i].z;
              pointOri.z = laserCloudOri->points[i].x;
              // lidar -> camera
              coeff.x = coeffSel->points[i].y;
              coeff.y = coeffSel->points[i].z;
              coeff.z = coeffSel->points[i].x;
              coeff.intensity = coeffSel->points[i].intensity;
              // in camera
              // 距离对旋转的偏导，是在欧拉角转换公式里进行求导的
              // R由欧拉角得到。这样的R分别对x y z 求导后，得到了矩阵R'。
              // \partial loss/ partial ex = (la,lb,lc)*R'*(px,py,pz)T;
              float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y - srx * sry * pointOri.z) * coeff.x + (-srx * srz * pointOri.x - crz * srx * pointOri.y - crx * pointOri.z) * coeff.y + (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y - cry * srx * pointOri.z) * coeff.z;
  
              float ary = ((cry * srx * srz - crz * sry) * pointOri.x + (sry * srz + cry * crz * srx) * pointOri.y + crx * cry * pointOri.z) * coeff.x + ((-cry * crz - srx * sry * srz) * pointOri.x + (cry * srz - crz * srx * sry) * pointOri.y - crx * sry * pointOri.z) * coeff.z;
  
              float arz = ((crz * srx * sry - cry * srz) * pointOri.x + (-cry * crz - srx * sry * srz) * pointOri.y) * coeff.x + (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y + ((sry * srz + cry * crz * srx) * pointOri.x + (crz * sry - cry * srx * srz) * pointOri.y) * coeff.z;
              // lidar -> camera
              // 雅可比矩阵中距离对旋转的偏导
              matA.at<float>(i, 0) = arz;
              matA.at<float>(i, 1) = arx;
              matA.at<float>(i, 2) = ary;
              // 雅可比矩阵中距离对平移的偏导
              matA.at<float>(i, 3) = coeff.z;
              matA.at<float>(i, 4) = coeff.x;
              matA.at<float>(i, 5) = coeff.y;
              // 残差项
              matB.at<float>(i, 0) = -coeff.intensity;
          }
          // 转置
          cv::transpose(matA, matAt);
          // 利用高斯牛顿法求解
          // 公式原型是：J^(T)*J * delta(x) = -J*f(x)
          // 通过QR分解求解matAtA*matX = matAtB，得到解matX
          matAtA = matAt * matA;
          matAtB = matAt * matB;
          cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
          // 检查A^TA是否退化（奇异），行列式=0
          // 对于使用G-N或L-M求解的优化问题来说，J可以视为是A的一阶近似
          // 通过Jacobian的eigenvalue判断哪个分量的约束不足, 不更新那个方向上的迭代
          if (iterCount == 0)
          {
  
              cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
              cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
              cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
          // matE为特征值，从大到小排列
              cv::eigen(matAtA, matE, matV);
              matV.copyTo(matV2);
  
              isDegenerate = false;
              float eignThre[6] = {100, 100, 100, 100, 100, 100};
              // 从小到大开始检查
              for (int i = 5; i >= 0; i--)
              {
                  // 特征值小于100，则认为退化，
                  // 论文《On Degeneracy of Optimization-based State Estimation Problems》里取200
                  if (matE.at<float>(0, i) < eignThre[i])
                  {
                      for (int j = 0; j < 6; j++)
                      {
                          matV2.at<float>(i, j) = 0;
                      }
                      isDegenerate = true;
                  }
                  else
                  {
                      break;
                  }
              }
              // 在论文里，对应 P=V_f^{-1} * V_u
              // 若不退化，则matP=I
              matP = matV.inv() * matV2;
          }
          // 见于论文《On Degeneracy of Optimization-based State Estimation Problems》
          if (isDegenerate)
          {
              cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
              matX.copyTo(matX2);
              // x_f =V_f^{-1} * V_u * X_u
              // 与论文里有出入，目前若某个维度退化了，则值在下一次迭代中仍保持不变（加了个0）。
              // 由于优化时，是利用transformTobeMapped也就是估算的位姿直接将点云给投影到了世界坐标系里
              // 因此，迭代初值就变成了0,这种做法在NDT_D2D里也有提到。
              // 这样来看，之所以比论文里少了预测项，应该也是因为预测值是0
              // 另外，本文实际上还是用到了guess的，因为transformTobeMapped在updateInitialGuess()函数里
              // 被imu里程计给赋值了。
              matX = matP * matX2;
          }
          // 改变下一步迭代值
          transformTobeMapped[0] += matX.at<float>(0, 0);
          transformTobeMapped[1] += matX.at<float>(1, 0);
          transformTobeMapped[2] += matX.at<float>(2, 0);
          transformTobeMapped[3] += matX.at<float>(3, 0);
          transformTobeMapped[4] += matX.at<float>(4, 0);
          transformTobeMapped[5] += matX.at<float>(5, 0);
          // 判断收敛条件
          float deltaR = sqrt(
              pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
              pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
              pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
          float deltaT = sqrt(
              pow(matX.at<float>(3, 0) * 100, 2) +
              pow(matX.at<float>(4, 0) * 100, 2) +
              pow(matX.at<float>(5, 0) * 100, 2));
  
          if (deltaR < 0.05 && deltaT < 0.05)
          {
              return true; // converged
          }
          return false; // keep optimizing
      }
  
      void scan2MapOptimization()
      {
          if (cloudKeyPoses3D->points.empty())
              return;
  
          if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
          {
              kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
              kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
              // 迭代优化三十次
              for (int iterCount = 0; iterCount < 30; iterCount++)
              {
                  laserCloudOri->clear();
                  coeffSel->clear();
                  // 当前激光帧角点寻找局部map匹配点
                  // 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
                  // 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
                  cornerOptimization();
                  // 当前激光帧平面点寻找局部map匹配点
                  // 1、更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
                  // 2、计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
                  surfOptimization();
                  // 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
                  combineOptimizationCoeffs();
  
                  // scan-to-map优化
                  // 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
                  if (LMOptimization(iterCount) == true)
                      break;
              }
              // 用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
              transformUpdate();
          }
          else
          {
              ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
          }
      }
  
      void transformUpdate()
      {
          if (cloudInfo.imuAvailable == true)
          {
              // abs(pitch)小于80.21°
              if (std::abs(cloudInfo.imuPitchInit) < 1.4)
              {
                  double imuWeight = imuRPYWeight;
                  tf::Quaternion imuQuaternion;
                  tf::Quaternion transformQuaternion;
                  double rollMid, pitchMid, yawMid;
  
                  // roll角求加权均值，用scan-to-map优化得到的位姿与imu原始RPY数据，进行加权平均
                  transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
                  imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
                  tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                  transformTobeMapped[0] = rollMid;
  
                  // pitch角求加权均值，用scan-to-map优化得到的位姿与imu原始RPY数据，进行加权平均
                  imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
                  tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                  transformTobeMapped[1] = pitchMid;
              }
          }
          // 更新当前帧位姿的roll, pitch, z坐标；因为是小车，roll、pitch是相对稳定的，不会有很大变动，一定程度上可以信赖imu的数据，z是进行高度约束
          // 相当于对roll pitch z加了先验的饱和函数，作用不大
          transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
          transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
          transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
  
          incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
      }
  
      // saturation函数，即去顶
      float constraintTransformation(float value, float limit)
      {
          if (value < -limit)
              value = -limit;
          if (value > limit)
              value = limit;
  
          return value;
      }
  
      bool saveFrame()
      {
          if (cloudKeyPoses3D->points.empty())
              return true;
  
          Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
          Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
          Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
          float x, y, z, roll, pitch, yaw;
          pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
  
          if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
              abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
              abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
              sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
              return false;
  
          return true;
      }
  
      void addOdomFactor()
      {
          if (cloudKeyPoses3D->points.empty())
          {
              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
              gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
              initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
          }
          else
          {
              noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
              gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
              gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
              gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
              initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
          }
      }
  
      void addGPSFactor()
      {
          if (gpsQueue.empty())
              return;
          // 如果没有关键帧，或者首尾关键帧距离小于5m，不添加gps因子
          if (cloudKeyPoses3D->points.empty())
              return;
          else
          {
              if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
                  return;
          }
  
          // 位姿(x和y)协方差很小，没必要加入GPS数据进行校正
          // (3,3)和(4,4)每次update后更新，是isam对协方差边缘化的结果，可以表征当前位姿估计的置信度
          if (poseCovariance(3, 3) < poseCovThreshold && poseCovariance(4, 4) < poseCovThreshold)
              return;
  
          // last gps position
          static PointType lastGPSPoint;
  
          while (!gpsQueue.empty())
          {
              // 删除当前帧0.2s之前的GPS里程计
              if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
              {
                  // message too old
                  gpsQueue.pop_front();
              }
              // GPS超过当前帧0.2s之后，退出
              else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
              {
                  // message too new
                  break;
              }
              else
              {
                  nav_msgs::Odometry thisGPS = gpsQueue.front();
                  gpsQueue.pop_front();
  
                  // GPS too noisy, skip
                  float noise_x = thisGPS.pose.covariance[0];
                  float noise_y = thisGPS.pose.covariance[7];
                  float noise_z = thisGPS.pose.covariance[14];
                  if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
                      continue;
  
                  float gps_x = thisGPS.pose.pose.position.x;
                  float gps_y = thisGPS.pose.pose.position.y;
                  float gps_z = thisGPS.pose.pose.position.z;
                  // gps高程估计比较差的话，在params里禁用，并用激光的z值去覆盖gps的z值。
                  if (!useGpsElevation)
                  {
                      gps_z = transformTobeMapped[5];
                      noise_z = 0.01;
                  }
  
                  // GPS not properly initialized (0,0,0)
                  if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
                      continue;
  
                  // Add GPS every a few meters
                  PointType curGPSPoint;
                  curGPSPoint.x = gps_x;
                  curGPSPoint.y = gps_y;
                  curGPSPoint.z = gps_z;
                  if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
                      continue;
                  else
                      lastGPSPoint = curGPSPoint;
  
                  gtsam::Vector Vector3(3);
                  Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
                  noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
                  gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
                  gtSAMgraph.add(gps_factor);
                  // 该标志位会增加优化次数，并重置局部地图，闭环也有同样效果
                  aLoopIsClosed = true;
                  break;
              }
          }
      }
      /**
       * 添加闭环因子
      */
      void addLoopFactor()
      {
          if (loopIndexQueue.empty())
              return;
  
          for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
          {
              // 闭环边对应两帧的索引
              int indexFrom = loopIndexQueue[i].first;
              int indexTo = loopIndexQueue[i].second;
              // 闭环边的位姿变换
              gtsam::Pose3 poseBetween = loopPoseQueue[i];
              gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
              gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
          }
  
          loopIndexQueue.clear();
          loopPoseQueue.clear();
          loopNoiseQueue.clear();
          // 该标志位会增加优化次数，并重置局部地图，GPS也有同样效果
          aLoopIsClosed = true;
      }
      /**
       * 设置当前帧为关键帧并执行因子图优化
       * 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
       * 2、添加激光里程计因子、GPS因子、闭环因子
       * 3、执行因子图优化
       * 4、得到当前帧优化后位姿，位姿协方差
       * 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
      */
      void saveKeyFramesAndFactor()
      {
          // 计算当前帧与前一帧位姿变换，如果变化足够大则返回true，表示需要设为关键帧
          // 如果没有关键帧则直接返回true
          if (saveFrame() == false)
              return;
  
          // odom factor
          // 第一帧则一个误差比较大的Prior Factor，其余则是between factor
          addOdomFactor();
  
          // gps factor
          addGPSFactor();
  
          // loop factor
          addLoopFactor();
  
          // cout << "****************************************************" << endl;
          // gtSAMgraph.print("GTSAM Graph:\n");
  
          // update iSAM
          isam->update(gtSAMgraph, initialEstimate);
          isam->update();
  
          if (aLoopIsClosed == true)
          {
              isam->update();
              isam->update();
              isam->update();
              isam->update();
              isam->update();
          }
  
          gtSAMgraph.resize(0);
          initialEstimate.clear();
  
          //save key poses
          PointType thisPose3D;
          PointTypePose thisPose6D;
          Pose3 latestEstimate;
  
          isamCurrentEstimate = isam->calculateEstimate();
          latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size() - 1);
          // cout << "****************************************************" << endl;
          // isamCurrentEstimate.print("Current estimate: ");
          // cloudKeyPoses3D和6D加入当前帧位姿
          thisPose3D.x = latestEstimate.translation().x();
          thisPose3D.y = latestEstimate.translation().y();
          thisPose3D.z = latestEstimate.translation().z();
          thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
          cloudKeyPoses3D->push_back(thisPose3D);
  
          thisPose6D.x = thisPose3D.x;
          thisPose6D.y = thisPose3D.y;
          thisPose6D.z = thisPose3D.z;
          thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
          thisPose6D.roll = latestEstimate.rotation().roll();
          thisPose6D.pitch = latestEstimate.rotation().pitch();
          thisPose6D.yaw = latestEstimate.rotation().yaw();
          thisPose6D.time = timeLaserInfoCur;
          cloudKeyPoses6D->push_back(thisPose6D);
  
          // cout << "****************************************************" << endl;
          // cout << "Pose covariance:" << endl;
          // cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
          // 位姿协方差
          poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);
  
          // save updated transform
          transformTobeMapped[0] = latestEstimate.rotation().roll();
          transformTobeMapped[1] = latestEstimate.rotation().pitch();
          transformTobeMapped[2] = latestEstimate.rotation().yaw();
          transformTobeMapped[3] = latestEstimate.translation().x();
          transformTobeMapped[4] = latestEstimate.translation().y();
          transformTobeMapped[5] = latestEstimate.translation().z();
  
          // save all the received edge and surf points
          pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
          pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
          pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
          pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
  
          // save key frame cloud
          cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
          surfCloudKeyFrames.push_back(thisSurfKeyFrame);
  
          // save path for visualization
          updatePath(thisPose6D);
      }
      /**
       * 若有GPS或回环则清空局部地图，更新因子图中所有变量节点（历史关键帧）的位姿，更新里程计轨迹
      */
      void correctPoses()
      {
          if (cloudKeyPoses3D->points.empty())
              return;
  
          if (aLoopIsClosed == true)
          {
              // clear local map cache
              laserCloudMapContainer.clear();
              // clear path
              globalPath.poses.clear();
              // update key poses
              int numPoses = isamCurrentEstimate.size();
              for (int i = 0; i < numPoses; ++i)
              {
                  cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
                  cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
                  cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
  
                  cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
                  cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
                  cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
                  cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
                  cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
                  cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
  
                  updatePath(cloudKeyPoses6D->points[i]);
              }
  
              aLoopIsClosed = false;
          }
      }
  
      void updatePath(const PointTypePose &pose_in)
      {
          geometry_msgs::PoseStamped pose_stamped;
          pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
          pose_stamped.header.frame_id = odometryFrame;
          pose_stamped.pose.position.x = pose_in.x;
          pose_stamped.pose.position.y = pose_in.y;
          pose_stamped.pose.position.z = pose_in.z;
          tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
          pose_stamped.pose.orientation.x = q.x();
          pose_stamped.pose.orientation.y = q.y();
          pose_stamped.pose.orientation.z = q.z();
          pose_stamped.pose.orientation.w = q.w();
  
          globalPath.poses.push_back(pose_stamped);
      }
  
      void publishOdometry()
      {
          // 发布激光里程计/mapping/odometry，值来自因子图优化
          nav_msgs::Odometry laserOdometryROS;
          laserOdometryROS.header.stamp = timeLaserInfoStamp;
          laserOdometryROS.header.frame_id = odometryFrame;
          laserOdometryROS.child_frame_id = "odom_mapping";
          laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
          laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
          laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
          laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
          pubLaserOdometryGlobal.publish(laserOdometryROS);
  
          // Publish TF odom->lidar，数值上同里程计
          static tf::TransformBroadcaster br;
          tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
                                                        tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
          tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");
          br.sendTransform(trans_odom_to_lidar);
  
          // Publish odometry for ROS (incremental)
          static bool lastIncreOdomPubFlag = false;
          static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
          static Eigen::Affine3f increOdomAffine;         // incremental odometry in affine
          if (lastIncreOdomPubFlag == false)
          {
              lastIncreOdomPubFlag = true;
              laserOdomIncremental = laserOdometryROS;
              increOdomAffine = trans2Affine3f(transformTobeMapped);
          }
          else
          {
              // Front是上一帧优化后的位姿，Back是当前帧激光里程计和IMU插值后的结果，
              Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
              // increOdomAffine是上次优化到这次激光的变换的累积，相比上面的里程计，缺乏了这次激光到这次优化的变换
              // 从数据来看，前期是一样的，但后期有闭环引入后是不一样的。
              increOdomAffine = increOdomAffine * affineIncre;
              float x, y, z, roll, pitch, yaw;
              pcl::getTranslationAndEulerAngles(increOdomAffine, x, y, z, roll, pitch, yaw);
              if (cloudInfo.imuAvailable == true)
              {
                  // imu姿态对结果进行再次加权，这里的权重是写死的，而不是从yaml拿，相当于这部分经过了两次插值
                  if (std::abs(cloudInfo.imuPitchInit) < 1.4)
                  {
                      double imuWeight = 0.1;
                      tf::Quaternion imuQuaternion;
                      tf::Quaternion transformQuaternion;
                      double rollMid, pitchMid, yawMid;
  
                      // slerp roll
                      transformQuaternion.setRPY(roll, 0, 0);
                      imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
                      tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                      roll = rollMid;
  
                      // slerp pitch
                      transformQuaternion.setRPY(0, pitch, 0);
                      imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
                      tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                      pitch = pitchMid;
                  }
              }
              laserOdomIncremental.header.stamp = timeLaserInfoStamp;
              laserOdomIncremental.header.frame_id = odometryFrame;
              laserOdomIncremental.child_frame_id = "odom_mapping";
              laserOdomIncremental.pose.pose.position.x = x;
              laserOdomIncremental.pose.pose.position.y = y;
              laserOdomIncremental.pose.pose.position.z = z;
              laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
              // 退化说明在第一次LM优化时，六个特征值至少有一个大于阈值
              if (isDegenerate)
                  laserOdomIncremental.pose.covariance[0] = 1;
              else
                  laserOdomIncremental.pose.covariance[0] = 0;
          }
          pubLaserOdometryIncremental.publish(laserOdomIncremental);
      }
  
      void publishFrames()
      {
          if (cloudKeyPoses3D->points.empty())
              return;
          // publish key poses，"lio_sam/mapping/trajectory"
          publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
          // Publish 局部地图的平面点，lio_sam/mapping/map_local
          publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
          // publish当前关键帧匹配后的特征点，"lio_sam/mapping/cloud_registered"
          if (pubRecentKeyFrame.getNumSubscribers() != 0)
          {
              pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
              PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
              *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
              *cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
              publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
          }
          // 发布当前帧配准之后的原始点云
          if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
          {
              pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
              pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
              PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
              *cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
              publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
          }
          // publish path
          if (pubPath.getNumSubscribers() != 0)
          {
              globalPath.header.stamp = timeLaserInfoStamp;
              globalPath.header.frame_id = odometryFrame;
              pubPath.publish(globalPath);
          }
      }
  };
  
  int main(int argc, char **argv)
  {
      ros::init(argc, argv, "lio_sam");
  
      mapOptimization MO;
  
      ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");
  
      std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
      std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
  
      ros::spin();
  
      loopthread.join();
      visualizeMapThread.join();
  
      return 0;
  }
  