SC-LEGO-LOAM 扩展以及深度解析

前言

本作者在16年大学开始接触ROS后，逐步向着机器人建图导航方面扩展，尤其是对激光雷达方向比较感兴趣，目前打算针对近阶段的SC-LEGO-LOAM进行分析讲述。从ScanContext和Lego LOAM两个部分进行分析阐述。一方面也是记录自己的学习成果，另一方面也是帮助他人一起熟悉这篇20年的经典文章。

LOAM系列发展

LOAM

LOAM作为该系列的鼻祖，在前几年kitti数据集中常年霸占榜首，文中的作者所写的代码由于可读性不高，所以有很多人对代码进行了重构。

论文中作者目标是使用一个三维空间中运动的多线激光雷达来实现激光里程计+建图的功能。为了可以同时获得低漂移和低复杂度，并且不需要高精度的测距和惯性测量。本文的核心思想是将定位和建图的分割，通过两个算法：一个是执行高频率的里程计但是低精度的运动估计（定位），另一个算法在比定位低一个数量级的频率执行匹配和注册点云信息（建图和校正里程计）。
第一个算法为了保证高频率，文中使用scan-to-scan匹配，利用前后两帧的位姿信息来实现粗略的位姿估计（当中包含有：特征点提取【按线束分割 $\rightarrow$ 计算曲率 $\rightarrow$ 删除异常点 $\rightarrow$按曲率大小筛选特征点】 $\Rightarrow$ 帧间匹配【投影到上一时刻，并计算损失函数】 $\Rightarrow$迭代优化【利用LM来实现位姿的迭代优化，并估算出这一帧数据中的点和上一帧数据中点的对应关系】）。
第二个算法为了保证高精度，则是使用了map-to-map的匹配。其优点是精度高，误差累计小；缺点就是计算量大，实时性压力大。当我们在第一步有了里程计校正后的点云数据后，接下来我们就可以做一个map-to-map的匹配了。但是map-to-map存在计算量大的问题，因此 我们可以让其执行的频率降低。这样的高低频率结合就保证了计算量的同时又兼具了精度。
值得注意的是LOAM仅仅是一个激光里程计算法，没有闭环检测，也就没有加入图优化框架。只是将SLAM问题分为两个算法并行运行：一个odometry算法，10Hz；另一个mapping算法，1Hz，最终将两者输出的pose做整合，实现10Hz的位姿实时输出。两个算法都是使用点云中提取出尖锐的边点和平整的面点作为特征点，然后进行特征点匹配，来估计lidar的位姿以及对位姿进行fine tune。

A-LOAM

A-LOAM相较于LOAM而言舍去了IMU对信息修正的接口，同时A-LOAM使用了Ceres库完成了LM优化和雅克比矩阵的正逆解。A-LOAM可读性更高，便于上手。简而言之，A-LOAM就是LOAM的清晰简化，版本。
A-LOAM的代码清晰度确实很高，整理的非常简洁，主要是使用了Ceres函数库代替了张继手推的ICP优化求解部分（用Ceres的自动求导，代替了手推的解析求导，效率会低一些）。整个代码目录如下：

LeGO-LOAM

LeGO-LOAM针对处理运算量做了优化，它的运算速度增加，同时相较于LOAM并没有牺牲精度。LeGO-LOAM作为近三年的论文，由于其代码开源以及对设备性能要求低等优点，到现在依然用的比较多。

论文里面主要是和LOAM对比，其相比LOAM具有以下五个特点

• 轻量级，能在嵌入式设备上实时运行。
  

• 地面优化，在点云处理部分加入了分割模块，这样做能够去除地面点的干扰，只在聚类的目标中提取特征。其中主要包括一个地面的提取（并没有假设地面是平面），和一个点云的分割，使用筛选过后的点云再来提取特征点，这样会大大提高效率。
  

• 在提取特征点时，将点云分成小块，分别提取特征点，以保证特征点的均匀分布。在特征点匹配的时候，使用预处理得到的segmenation标签筛选(点云分割为边缘点类和平面点类两类)，再次提高效率。

• 两步L-M优化法估计6个维度的里程计位姿。先使用平面点优化高度，同时利用地面的方向优化两个角度信息。地面提取的平面特征用于在第一步中获得[ $t_z$, $\theta_{roll}$， $\theta_{pitch}$]；再使用边角点优化剩下的三个变量，通过匹配从分段点云提取的边缘特征来获得其余部分的变换[ $t_x$, $t_y$， $\theta_{yaw}$]。匹配方式还是scan2scan，以这种方式分别优化，效率提升40%，但是并没有造成精度的损失

• 集成了回环检测以校正运动估计漂移的能力（即使用gtsam作回环检测并作图优化，但是本质上仍然是基于欧式距离的回环检测，不存在全局描述子）。
  

LIO-SAM

该文章作为LeGO-LOAM作者的正统续作，也是近年来比较有了解价值的多传感器融合里程计，为此我们拿出来说一说。LIO-SAM实际上是LeGO-LOAM的扩展版本，添加了IMU预积分因子和GPS因子，去除了帧帧匹配部分。

论文认为loam系列文章存在一些问题：将其数据保存在全局体素地图中，难以执行闭环检测；没有结合其他绝对测量（GPS，指南针等）；当该体素地图变得密集时，在线优化过程的效率降低。 为此作者决定使用因子图的思想优化激光SLAM，引入四种因子：IMU预积分因子；激光雷达里程因子；GPS因子；闭环因子。 下面是文章中主要的贡献点。

• 里程计部分改为scan2localmap的匹配，特征提取部分去除了原LeGO-LOAM中的聚、分割并提取较为突出的边缘点和平面点，而是沿用LOAM中的边缘和平面点。（精度高一些，LeGO-LOAM主要考虑性能多一点）
• 维护两个因子图，预积分因子图可联合优化激光雷达odom和IMU，并估计IMU偏差，进行实时的里程计估算，这里将雷达位姿作为预测，而把imu作为观测，去更新imu的bias。全局因子图联合优化雷达odom和GPS因子以及回环因子。（即最终建图没有用到IMU优化后的轨迹，有点奇怪，这样做的好处就是优化速度快）
• 利用robot_localization中的EKF节点融合GPS和9轴IMU数据，得到提供初始姿态的gps odom。（这里初始化部分使用到了9轴IMU，参考VINS）

该图对应了文中的两个因子图。当中主要有4种因子，imu预积分因子（橙色）、激光里程计因子（绿色）、GPS因子（黄色）和回环因子（黑色）。在激光里程计部分，沿用LOAM中的特征提取方法，提取边缘点和平面点，将当前帧特征点分别与之前n+1帧的局部特征点地图进行匹配，分别建立点到直线和点到平面的约束，最后联合优化估计位姿。而GPS因子则是在位姿估计协方差矩阵变得很大的时候通过对其时间戳进行线性插值而添加进来。
下面是系统的节点图：

这里可以很清晰的看到系统中odometry部分有三个数据，/odometry/gps和/odometry/navsat是用robot_localization包融合GPS而得到的，而/odometry/imu是由优化后的激光odom和实时的imu_raw数据，通过imu预积分得到一个实时的预测的odom，并更新imu偏差。

ISCLOAM

ISCLOAM是一篇2020年发表在ICRA上面的论文，论文里面作者提出了一种基于scan context(18 IROS)改进的全局点云描述符，主要是编码了强度信息。在保存了ISC特征描述符后可以快速进行回环检测。

文中提供的代码在点云预处理、特征提取、位姿估计和LEGO-LOAM基本一致。文中主要的贡献在于回环检测部分：根据预处理的点云数据，实时提取ISC特征，并根据这些特征进行回环检测。在发现回环后，则开始后端优化，即接收当前的边缘、平面特征，以及odom和回环信息，然后进行全局一致性优化，这里采用GTSAM完成图优化,此处使用了全局特征子，和SC-LeGO-LOAM一样。

文中使用的ISC特征和SC特征对比，在回环检测部分，两个关键步骤，一是计算ISC特征，二是特征相似性计算。

首先是提取ISC特征，用一张图像来表示，其中像素值存的是点云的强度。ISC描述子同时编码了LiDAR点云的几何（位置）和密度信息，密度信息的描述子表示了周围环境的反射率，该信息对于不同地点是独一无二的。更容易避免回环检测失误。

ISC特征的相似性计算部分这里分两步计算，第一步计算其空间距离得分，第二步计算其强度距离得分。这里进行两个阶段的检测，首先是快速的几何结构匹配，然后密度结构重识别。

bool ISCGenerationClass::is_loop_pair(ISCDescriptor &desc1,
                                      ISCDescriptor &desc2,
                                      double &geo_score,
                                      double &inten_score) {
  int angle = 0;
  geo_score = calculate_geometry_dis(desc1, desc2, angle); // 计算几何距离
  if (geo_score > GEOMETRY_THRESHOLD) {
    inten_score = calculate_intensity_dis(desc1, desc2, angle); // 计算强度得分
    if (inten_score > INTENSITY_THRESHOLD) {
      return true;
    }
  }
  return false;
}

double ISCGenerationClass::calculate_geometry_dis(const ISCDescriptor &desc1, const ISCDescriptor &desc2, int &angle) {
  double similarity = 0.0;

  // 几何结构匹配
  for (int i = 0; i < sectors; i++) { // 列
    int match_count = 0;
    // 矩阵逐元素异或
    for (int p = 0; p < sectors; p++) {
      int new_col = p + i >= sectors ? p + i - sectors : p + i;
      for (int q = 0; q < rings; q++) {  // 行
        // 由于二值矩阵的列向量表示方位角，因此LiDAR的旋转可以通过矩阵的列移动反映．
        // 因此，为了检测视角变化，我们需要计算具有最大几何相似度的列移动
        if ((desc1.at<unsigned char>(q, p) == true && desc2.at<unsigned char>(q, new_col) == true)
            || (desc1.at<unsigned char>(q, p) == false && desc2.at<unsigned char>(q, new_col) == false)) {
          match_count++;
        }

      }
    }
    if (match_count > similarity) {
      similarity = match_count;
      angle = i;
    }
  }
  return similarity / (sectors * rings);
}
double ISCGenerationClass::calculate_intensity_dis(const ISCDescriptor &desc1, const ISCDescriptor &desc2, int &angle) {
  double difference = 1.0;
  // 密度结构重识别 计算上一步最佳列旋转后的和候选帧的ISC的密度相似度
  // 取ISC对应列的余弦距离的均值
  double angle_temp = angle;
  for (int i = angle_temp - 10; i < angle_temp + 10; i++) {

    int match_count = 0;
    int total_points = 0;
    for (int p = 0; p < sectors; p++) {
      int new_col = p + i;
      if (new_col >= sectors)
        new_col = new_col - sectors;
      if (new_col < 0)
        new_col = new_col + sectors;
      for (int q = 0; q < rings; q++) {
        match_count += abs(desc1.at<unsigned char>(q, p) - desc2.at<unsigned char>(q, new_col));
        total_points++;
      }
    }
    double diff_temp = ((double) match_count) / (sectors * rings * 255);
    if (diff_temp < difference)
      difference = diff_temp;
  }
  return 1 - difference;
}

ICP && NDT

ICP和NDT原本是作为点云与点云的直接匹配方法来作为前端里程计估计的方法，但是目前由于LOAM系列的出现，导致这种方法已经很少使用。此处我们提到ICP和NDT是想说明其在回环检测的作用，由于回环检测需要寻找到回环点。以LeGO-LOAM算法为代表的回环检测就是使用ICP+欧式距离的方法来寻找到回环点。而以SC-LeGO-LOAM算法为代表的回环检测使用了scan context系列提取的全局特征子来进行查找，同时有些方法会在此基础上再加入ICP来提升回环检测后重定位的精度，方便图优化。这两种方法各有千秋，可以根据需求进行选择。

回环检测

在LOAM系列中回环检测主要存在有四种方法

1. 传统的领域距离搜索+ICP匹配
2. 基于scan context系列的粗匹配+ICP精准匹配的回环检测
3. 基于scan context的回环检测
4. 基于Intensity scan context+ICP的回环检测

在参考很多大佬的比对结果中我们发现，传统的领域距离搜索+ICP匹配是这三个方法中最耗时的，相较于基于scan context的回环检测而言利用类梯度下降法做ICP匹配需要消耗大量的计算资源。在使用基于scan context的回环检测时，会出现误检和漏检情况，而基于Intensity scan context的回环检测，对于scan context建图精度未有太大提升，但漏检的情况要少很多。

这里感谢26.3分的HITer、攻城狮の家、shuang_yu_等人的相关博文提供的指导和帮助。

–>