Lego_Loam包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分，下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。本节是解析MapOptmization文件和Transform Fusion文件。
该专栏的其他章节链接如下
https://blog.csdn.net/HUASHUDEYANJING/article/details/130332367

一、整体框架

1.1 目的

主要根据里程计获得的先验位姿进行后端优化，闭环检测和图优化

1.2 输入

//接收相机坐标系下的点和里程计
//上一帧角点
subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this); 
//上一帧面点
subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
//上一帧无效点
subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this); 
//里程计位姿
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);  
//IMU数据
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 50, &mapOptimization::imuHandler, this);

输出

//机器人关键帧在全局坐标系下的位置信息，轨迹
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("/key_pose_origin", 2);
//机器人周围激光雷达点云数据
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 2);
//经过位姿图优化和点云配准后的里程计信息
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);
//机器人历史轨迹的点云数据
pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/history_cloud", 2);
//经过ICP配准后的机器人激光雷达点云数据
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/corrected_cloud", 2);
//机器人最近获取的点云数据
pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/recent_cloud", 2);
//经过位姿图优化和点云配准后的机器人点云数据用于建图
pubRegisteredCloud = nh.advertise<sensor_msgs::PointCloud2>("/registered_cloud", 2);

1.3 主函数

主要的功能是在run函数里面

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lego_loam");
    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
    mapOptimization MO;
    // 1.进行闭环检测与闭环的功能
    std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
    // 2.将数据发布到ros中，可视化
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
    ros::Rate rate(200);
    while (ros::ok())
    {
        ros::spinOnce();
        MO.run(); //进入执行run函数
        rate.sleep();
    }
    loopthread.join();
    visualizeMapThread.join();
    return 0;
}
// 3.run函数
void run(){
    if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
        timeLastProcessing = timeLaserOdometry;
        transformAssociateToMap(); //转换到map坐标系下
        extractSurroundingKeyFrames(); //提取周围的关键帧
        downsampleCurrentScan(); //下采样当前帧
        // 当前扫描进行边缘优化，图优化以及进行LM优化的过程
        scan2MapOptimization();
        saveKeyFramesAndFactor(); //保存关键帧和因子
        correctPoses(); //校正位姿
        publishTF(); //发布坐标变换
        publishKeyPosesAndFrames(); //发布关键帧和因子
        clearCloud();}}} //清除点云

二、函数解析

2.1 transformAssociateToMap

  • 作用：将坐标转移到世界坐标系下,得到可用于建图的Lidar坐标
  • 输入：transformBefMapped[] 前一帧在世界坐标系的位姿
  • transformSum 当前帧的位姿
  • 输出：transformTobeMapped当前帧在世界坐标系的位置

  • 代码：

    void transformAssociateToMap()
  {
   float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
       - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
   float y1 = transformBefMapped[4] - transformSum[4];
   float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
       + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
   float x2 = x1;
   float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
   float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
   // 计算平移增量
   transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
   transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
   transformIncre[5] = z2;
   ……
       x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) 
       * transformIncre[4];
   y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2])
       * transformIncre[4];
   z1 = transformIncre[5];

   x2 = x1;
   y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
   z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

   transformTobeMapped[3] = transformAftMapped[3] 
       - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
   transformTobeMapped[4] = transformAftMapped[4] - y2;
   transformTobeMapped[5] = transformAftMapped[5] 
       - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);}

    2.2 downsampleCurrentScan

  • 作用：对所有点云进行降采样
  • 输入：
    laserCloudCornerLast
    laserCloudSurfLast
    laserCloudOutlierLast
    laserCloudSurfTotalLast
  • 输出：
    laserCloudCornerLastDS
    laserCloudSurfLastDS
    laserCloudOutlierLastDS
    laserCloudSurfTotalLastDS
  • 代码 
    //函数是将当前各类点云降采样
void downsampleCurrentScan(){
  laserCloudCornerLastDS->clear();
  downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
  downSizeFilterCorner.filter(*laserCloudCornerLastDS);  //将上一帧的角点下采样保存到laserCloudCornerLastDS
  laserCloudCornerLastDSNum = laserCloudCornerLastDS->points.size();
  laserCloudSurfLastDS->clear();
  downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
  downSizeFilterSurf.filter(*laserCloudSurfLastDS);
  laserCloudSurfLastDSNum = laserCloudSurfLastDS->points.size();
  laserCloudOutlierLastDS->clear();
  downSizeFilterOutlier.setInputCloud(laserCloudOutlierLast);
  downSizeFilterOutlier.filter(*laserCloudOutlierLastDS);
  laserCloudOutlierLastDSNum = laserCloudOutlierLastDS->points.size();
  laserCloudSurfTotalLast->clear();
  laserCloudSurfTotalLastDS->clear();
  *laserCloudSurfTotalLast += *laserCloudSurfLastDS;
  *laserCloudSurfTotalLast += *laserCloudOutlierLastDS;
  downSizeFilterSurf.setInputCloud(laserCloudSurfTotalLast);
  downSizeFilterSurf.filter(*laserCloudSurfTotalLastDS);
  laserCloudSurfTotalLastDSNum = laserCloudSurfTotalLastDS->points.size(); }
    下采样结果如下图所示

    2.3 saveKeyFramesAndFactor

  • 作用：保存关键帧和因子
  • 输入：
    • transformAftMapped
    • previousRobotPosPoint
    • currentRobotPosPoint
  • 输出：关键帧及关键帧周围的下采样点云
    • cloudKeyPoses3D
    • cloudKeyPoses6D
    • cornerCloudKeyFrames
    • surfCloudKeyFrames
    • outlierCloudKeyFrames
  • 代码 
    void saveKeyFramesAndFactor(){
currentRobotPosPoint.x = transformAftMapped[3];
currentRobotPosPoint.y = transformAftMapped[4];
currentRobotPosPoint.z = transformAftMapped[5];
bool saveThisKeyFrame = true;
// 1、位姿距离大于0.3保存关键帧
if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x)+(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y)+(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.zcurrentRobotPosPoint.z)) < 0.3)
  {saveThisKeyFrame = false;}
//2、对于第一个点添加固定先验因子 
if (cloudKeyPoses3D->points.empty()){
gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]), Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
for (int i = 0; i < 6; ++i)
  transformLast[i] = transformTobeMapped[i];}
// 3、对于其他点添加位姿间的二元因子
else{
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),                                              Point3(transformLast[5], transformLast[3], transformLast[4]));            
gtsam::Pose3 poseTo   = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),                                              Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));    
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D>points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]))); }
//4、更新isam和计算位姿
isam->update(gtSAMgraph, initialEstimate);
isam->update();
gtSAMgraph.resize(0);
initialEstimate.clear();
// 5、保存位姿
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D); //cloudKeyPoses3D是一个一个坐标系关键位置点
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().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw   = latestEstimate.rotation().roll(); // in camera frame
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D); //cloudKeyPoses6D是一个一个欧拉位置点
// 6、保存关键帧点云 pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);
pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);

    2.4 extractSurroundingKeyFrames

  • 作用：添加关键帧，拼接成局部点云地图，为后续scan-to-map优化
  • 输入：
    cornerCloudKeyFrames
    surfCloudKeyFrames
    outlierCloudKeyFrames
    cloudKeyPoses3D
    cloudKeyPoses6D
  • 输出：
    laserCloudCornerFromMapDS
    laserCloudSurfFromMapDS
  • 代码 
    void extractSurroundingKeyFrames(){
   if (loopClosureEnableFlag == true){   // 如果启用回环检测 
  // 1. 添加最近的关键位姿到图
  //如果recentCornerCloudKeyFrames中点云保存的数量较少
  if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum)
  {// 从最新的关键帧开始，取关键帧push
      for (int i = numPoses-1; i >= 0; --i)
      {int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;
      PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
      updateTransformPointCloudSinCos(&thisTransformation);                                                                   recentCornerCloudKeyFrames.push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));                             recentSurfCloudKeyFrames.push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));                                 recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));                         if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum
      {break; }}
      // 如果recentCornerCloudKeyFrames中点云保存的数量较
  else{ 
  if (latestFrameID != cloudKeyPoses3D->points.size() - 1)
  {recentCornerCloudKeyFrames. pop_front();
  recentSurfCloudKeyFrames.   pop_front();
  recentOutlierCloudKeyFrames.pop_front();
  // 2. 弹出队列中时间最久的帧，添加最新的帧到队列
  latestFrameID = cloudKeyPoses3D->points.size() - 1;
  PointTypePose thisTransformation = cloudKeyPoses6D->points[latestFrameID];
  updateTransformPointCloudSinCos(&thisTransformation);
  recentCornerCloudKeyFrames.push_back(transformPointCloud(cornerCloudKeyFrames[latestFrameID]));
  recentSurfCloudKeyFrames.push_back(transformPointCloud(surfCloudKeyFrames[latestFrameID]));
  recentOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[latestFrameID]));}}}
  // 3. 拼接为点云
  for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i
  {*laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
  *laserCloudSurfFromMap   += *recentSurfCloudKeyFrames[i];
  *laserCloudSurfFromMap   += *recentOutlierCloudKeyFrames[i];}
  //4. 下采样
  downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
  downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
  laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size();
  downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
  downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
  laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size();

    2.5 scan2MapOptimization

  • 作用：根据现有地图与最新点云数据进行配准从而更新机器人精确位姿与融合建图
  • 输入：
    laserCloudCornerFromMapDS
    laserCloudSurfFromMapDS
    laserCloudCornerLastDS
    laserCloudSurfTotalLastDS
    transformSum
  • 输出：
    coeffSel
    transformAftMapped 
    void scan2MapOptimization(){if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {// 1. 根据周围关键帧点云创建kdtree
  kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
  kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
  for (int iterCount = 0; iterCount < 10; iterCount++) {
      laserCloudOri->clear();
      coeffSel->clear();
      // 2.角和面特征计算距离
      cornerOptimization(iterCount);//角点优化
      surfOptimization(iterCount);//面点优化
      //3. LM优化
      if (LMOptimization(iterCount) == true)//基于高斯牛顿法优化最小二乘
          break;}
  transformUpdate();// 迭代结束更新相关的转移矩阵}}

    闭环检测

    代码得闭环检测主要由detectLoopClosure、performLoopClosure、orrectPoses函数实现，下面将解析

    3.1 detectLoopClosure

  • 作用：检验是否进入闭环检测程序，并确定检测范围
  • 输入：cloudKeyPoses3D
  • 输出：
    True
    False
    nearHistorySurfKeyFrameCloudDS
  • 代码 
    bool detectLoopClosure(){
     for (int i = 0; i < pointSearchIndLoop.size(); ++i
          {int id = pointSearchIndLoop[i];
           // 两个时间差值大于30秒
           if (abs(cloudKeyPoses6D>points[id].time - timeLaserOdometry) > 30.0){
               closestHistoryFrameID = id;
               break; } }
          if (closestHistoryFrameID == -1){
              return false;
          // historyKeyframeSearchNum在utility.h中定义为25，前后25个点进行变换
          for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){
              if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)
                  continue;
              //将搜索范围内的角点点云与平面点点云均叠加至nearHistorySurfKeyFrameCloud中
              *nearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
              *nearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j],   &cloudKeyPoses6D->points[closestHistoryFrameID+j]);}
              // 下采样滤波减少数据量
              downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud);
              downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS);

    3.2 performLoopClosure

  • 作用：判断是否闭环，进行图优化
  • 输入：
    isamCurrentEstimate
    cloudKeyPoses3D
  • 输出：
    gtSAMgraph
    isam

  • 代码

    void performLoopClosure(){
      //1.判断是否进入闭环程序
      if (detectLoopClosure() == true)
      //2.接着使用icp迭代进行对齐
      pcl::IterativeClosestPoint<PointType, PointType> icp;
      icp.setMaxCorrespondenceDistance(100);  
      icp.setMaximumIterations(100);  //设置最大迭代次数
      icp.setTransformationEpsilon(1e-6);  //设置前后两次迭代的转换矩阵的最大容差
      icp.setEuclideanFitnessEpsilon(1e-6); //设置前后两次迭代的点对的欧式距离均值的最大容差
      icp.setRANSACIterations(0);  // 设置RANSAC运行次数
      // Align clouds进行icp点云对齐
      // 匹配当前帧点云和之前的历史点云
      icp.setInputSource(latestSurfKeyFrameCloud);
      icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
      pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
      icp.align(*unused_result); // 进行icp点云对齐
      //3.对齐之后判断迭代是否收敛以及噪声是否太大，是则返回并直接结束函数
      if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
            return;
      //4.发布
      pubIcpKeyFrames.publish(cloudMsgTemp);

      //5. 矫正后的当前最新优化关键帧位姿
      gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
       // 取闭环关键帧位姿
      gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);
      gtsam::Vector Vector6(6);
      float noiseScore = icp.getFitnessScore();
      Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
      constraintNoise = noiseModel::Diagonal::Variances(Vector6);
      /* 
      add constraints
      */
      //6.然后进行图优化过程。
      std::lock_guard<std::mutex> lock(mtx);
      // 因子图加入回环边
      gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
      isam->update(gtSAMgraph); //添加回环因子
      isam->update();
      gtSAMgraph.resize(0);
      aLoopIsClosed = true;
  }

    3.3 correctPoses

  • 作用：回环检测如果成功，会设置aLoopIsClosed为True，才会进行这一步。将isam优化后的位姿替换之前的位姿
  • 输入：
    isamCurrentEstimate
    cloudKeyPoses3D

  • 输出： cloudKeyPoses6D

    void correctPoses(){
  if (aLoopIsClosed == true){
      recentCornerCloudKeyFrames. clear();
      recentSurfCloudKeyFrames.   clear();
      recentOutlierCloudKeyFrames.clear();
      // update key poses
      // 1. 将isam优化后的位姿替换之前的位姿
      int numPoses = isamCurrentEstimate.size();
      for (int i = 0; i < numPoses; ++i){
          cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().y();
          cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().z();
          cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().x();

          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().pitch();
          cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
          cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
      }
      aLoopIsClosed = false; }}

    到此，就可以构建出效果比较好的地图如下

    四、Transform Fusion
    最后的Transform Fusion主要是对位姿的优化更新，内容比较少

  • 作用：主要功能是融合粗、精配准的里程计信息
  • 输入： isamCurrentEstimate cloudKeyPoses3D
  • 输出： integrated_to_init 
    pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/integrated_to_init", 5);
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &TransformFusion::laserOdometryHandler, this);//激光里程计
subOdomAftMapped = nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 5, &TransformFusion::odomAftMappedHandler, this); //地图匹配
    优化前和优化后的位姿

    至此LEGO_LOAM的全部代码解析完毕，下