从零入门激光SLAM（十三）——LeGo-LOAM源码超详细解析3

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：951026257，欢迎大家加入讨论。

Lego_Loam包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分，下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。本节是解析Feature association文件中的特征匹配部分。
该专栏的其他章节链接如下
从零入门激光SLAM系列——总目录与更新情况_kiss icp-CSDN博客

一、Feature Extraction

经过上一节的点云特征提取，我们已经获得了稳健的角点和面点如下图所示，下面将介绍如何配准得到位姿Feature Association部分的源码解析

 1. Feature Extraction特征提取
// 1.1 点云运动补偿
        adjustDistortion();
// 1.2 计算平滑度
        calculateSmoothness();
// 1.3 标记遮挡点
        markOccludedPoints();
// 1.4 提取特征
        extractFeatures();
// 1.5 发布点云
        publishCloud();
2. Feature Association特征匹配
    if (!systemInitedLM) {
            // 2.1 检查系统初始化，点云投影到结束点
                checkSystemInitialization();
                return;}
// 2.2 更新初始猜测位置
        updateInitialGuess();
// 2.3 特征匹配，输出Transformation
        updateTransformation();
// 2.4 融合IMU坐标Transformation
        integrateTransformation();

二、函数解析

2.1 publishCloudsLast

• 作用：将当前帧点云投影到结束点并发布，作为下一帧匹配的对象，（匹配之前需要有一个匹配对象点云）
• 输入：

   cornerPointsSharp角特征  
   cornerPointsLessSharp缓角特征  
   surfPointsFlat面特征  
   surfPointsLessFlat缓面特征 
   adjustCloud修正点云              
  

• 输出：

   /laser_cloud_corner_last
   /laser_cloud_surf_last
   /outlier_cloud_last
   /laser_odom_to_init
  

• 代码：

  void publishCloudsLast(){
  //把特征点云投影到每帧的结尾时刻
  updateImuRollPitchYawStartSinCos();
  int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
  for (int i = 0; i < cornerPointsLessSharpNum; i++) {
    TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); }
    int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
  for (int i = 0; i < surfPointsLessFlatNum; i++) {
    TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);}
  //用KD树存储当前帧点云，为后续匹配做准备
  pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
  cornerPointsLessSharp = laserCloudCornerLast;
  laserCloudCornerLast = laserCloudTemp;
  laserCloudTemp = surfPointsLessFlat;
  surfPointsLessFlat = laserCloudSurfLast;
  laserCloudSurfLast = laserCloudTemp;
  laserCloudCornerLastNum = laserCloudCornerLast->points.size();
  laserCloudSurfLastNum = laserCloudSurfLast->points.size();
  if (frameCount >= skipFrameNum + 1) {
            frameCount = 0;
  //发布上一帧outlier
  pubOutlierCloudLast.publish(outlierCloudLast2);
  //发布上一帧线特征
  pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
  //发布上一帧面特征
  pubLaserCloudSurfLast.publish(laserCloudSurfLast2);}
  

  下图中红色为前一帧，绿色为当前帧

  

  2.2 updateInitialGuess

• 作用：把当前IMU数值作为位姿先验
• 输入：IMU数值
• 输出：transformCur位姿矩阵

  // 如果IMU有旋转，则更新变换矩阵以反映新的旋
  void updateInitialGuess(){
  if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){transformCur[0] = - imuAngularFromStartY;  // 根据Y轴旋转更新变换矩阵的第一个元素
  transformCur[1] = - imuAngularFromStartZ;  // 根据Z轴旋转更新变换矩阵的第二个元素
  transformCur[2] = - imuAngularFromStartX;  // 根据X轴旋转更新变换矩阵的第三个元素}
  // 如果IMU有移动，则更新变换矩阵以反映新的平移
  if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0)
  {transformCur[3] -= imuVeloFromStartX * scanPeriod;  // 根据X轴移动更新变换矩阵的第四个元素
  transformCur[4] -= imuVeloFromStartY * scanPeriod;  // 根据Y轴移动更新变换矩阵的第五个元素
  transformCur[5] -= imuVeloFromStartZ * scanPeriod;  // 根据Z轴移动更新变换矩阵的第六个元素 }   }
  

  2.3 updateTransformation

• 作用：匹配角和面特征点，计算位姿
• 输入：

   laserCloudSurfLast
   surfPointsFlat
   laserCloudCornerLast
   cornerPointsSharp
  

• 输出：transformCur位姿矩阵

• 代码

  void updateTransformation(){
       //检查特征点
       if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
           return;
  //  面特征匹配
       for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
           laserCloudOri->clear(); 
           coeffSel->clear(); 
           findCorrespondingSurfFeatures(iterCount1); 
        if (laserCloudOri->points.size() < 10)
               continue;
           // 通过面特征的匹配，计算变换矩阵
           if (calculateTransformationSurf(iterCount1) == false)
               break;
       }
  // 线特征匹配
       for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {
  
           laserCloudOri->clear();
           coeffSel->clear();
  
           findCorrespondingCornerFeatures(iterCount2); 
           if (laserCloudOri->points.size() < 10)
               continue;
           if (calculateTransformationCorner(iterCount2) == false) 
               break;
       }
   }

详情请见...
从零入门激光SLAM（十三）——LeGo-LOAM源码超详细解析3 - 古月居 (guyuehome.com)