LidarSlam（二）关键点提取

简介

关键点提取算法在SpinningSensorKeypointExtractor.h 和 SpinningSensorKeypointExtractor.cxx文件中。本特征点提取算法用于对于旋转扫描式雷达的特征点提取。是LOAM算法的延续。

在SpinningSensorKeypointExtractor.h中声明了LineFitting结构体和SpinningSensorKeypointExtractor类。

其曲率计算是用邻域内的点计算一条直线，通过左邻域和右邻域拟合的直线使用叉乘计算其sin值，然后进行sin值比较。

雷达坐标系方向，X轴向前，Y轴向左，Z轴向上。右手坐标系。

LineFitting

LineFitting用于直线拟合，使用点集的中心点坐标以及直线方向来表达拟合的直线，结构体LineFitting声明如下：

struct LineFitting
{
  using Point = LidarPoint;
  using PointCloud = pcl::PointCloud<Point>;

  //! Fitting using very local line and check if this local line is consistent
  //! in a more global neighborhood
  bool FitLineAndCheckConsistency(const PointCloud& cloud,
                                 const std::vector<int>& indices);

  //! Compute the squared distance of a point to the fitted line
  // 计算点到直线的距离
  inline float DistanceToPoint(Eigen::Vector3f const& point) const;

  // Direction and position
  Eigen::Vector3f Direction;
  Eigen::Vector3f Position;

  //! Max line width to be trustworthy for lines < 20cm
  float MaxLineWidth = 0.02;  // [m]

  // Ratio between length and width to be trustworthy
  float LengthWidthRatio = 10.; // [.]
};

FitLineAndCheckConsistency用于直线拟合，并检查该直线是否符合要求。

通过检查点集中的点到直线的最大距离是否超过阈值，来判断拟合的直线是否符合要求，符合要求返回true，不符合要求返回false。

bool LineFitting::FitLineAndCheckConsistency(const SpinningSensorKeypointExtractor::PointCloud& cloud,
                                            const std::vector<int>& indices)
{
  // Check line width
  // 计算该点集中两端点的距离，这些点是同一条扫描线上的连续点
  float lineLength = (cloud[indices.front()].getVector3fMap() - cloud[indices.back()].getVector3fMap()).norm();
  // 线的宽度，也就是点点到直线的距离的最大值，大于该值则认为无法形成直线
  // this->LengthWithRatio = 长度 / 宽度。即宽度 = 长度 / LengthWithRatio
  // 选取默认值和通过长度计算出来的宽度的最大值。
  float widthTheshold = std::max(this->MaxLineWidth, lineLength / this->LengthWidthRatio);

  float maxDist = widthTheshold;
  Eigen::Vector3f bestDirection;

  this->Position = Eigen::Vector3f::Zero();
  for (int idx : indices) // 计算点集的中心位置，即各分量平均值
    this->Position += cloud[idx].getVector3fMap();
  this->Position /= indices.size();

  // RANSAC
  // 通过使点集中的点到拟合直线的最大距离最小化来计算最佳拟合直线
  // 直线的方向通过两个点向量做差得出
  // 邻域中的没两个点都要进行方向计算
  // 选取领域中点到直线的最大距离最小的方向作为直线
  for (int i = 0; i < int(indices.size()) - 1; ++i)
  {
    // Extract first point
    auto& point1 = cloud[indices[i]].getVector3fMap();
    for (int j = i+1; j < int(indices.size()); ++j)
    {
      // Extract second point
      auto& point2 = cloud[indices[j]].getVector3fMap();

      // Compute line formed by point1 and point2
      this->Direction = (point1 - point2).normalized();

      // Reset score for new points pair
      float currentMaxDist = 0;
      // Compute score : maximum distance of one neighbor to the current line
      for (int idx : indices)
      {
        // DistanceToPoint计算点到直线的距离
        // 选取最大值
        currentMaxDist = std::max(currentMaxDist, this->DistanceToPoint(cloud[idx].getVector3fMap()));

        // If the current point distance is too high,
        // the current line won't be selected anyway so we
        // can avoid computing next points' distances
        if (currentMaxDist > widthTheshold)
          break;
      }

      // If the current line implies high error for one neighbor
      // the output line is considered as not trustworthy
      if (currentMaxDist > 2.f * widthTheshold)
        return false;

      if (currentMaxDist <= maxDist)
      {
        bestDirection = this->Direction;
        maxDist = currentMaxDist;
      }

    }
  }

  if (maxDist >= widthTheshold - 1e-10)
    return false;

  this->Direction = bestDirection;

  return true;
}

 直线的位置通过对领域内所有点的中心（所有点加和求平均）。

直线的方向通过对领域内所有点进行两两做差计算备选方向，然后计算所有点到该方向上的距离，如果最大距离没有超过 2.f * widthTheshold，则选择每个方向上点到直线的最大距离最小的距离对应的方向。如下图：

第一个点一次与其后续所有点做差，计算方向，然后选择每个方向上所有点到该方向对应直线的距离，记录最大距离。然后依次计算第二个点与其后所有点两两做差，记录最大距离......

对于所有直线，选择最大距离最小的直线作为该领域点拟合的直线。

SpinningSensorKeypointExtractor

用于提取旋转式扫描激光雷达的关键点，将点云根据扫描线进行分离，每条扫面线中按方位角顺序依次存储点，然后根据计算曲率，根据曲率大小将点分为平面点以及角点。

调用函数ComputeKeyPoints进行关键点提取。该函数的传入参数是当前帧点云，然后对点云进行了一系列预处理、计算曲率、根据曲率提取对应的特征点。

void SpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
{
  this->Scan = pc;

  // Split whole pointcloud into separate laser ring clouds
  this->ConvertAndSortScanLines();

  // Initialize the features vectors and keypoints
  this->PrepareDataForNextFrame();

  // Compute keypoints scores
  this->ComputeCurvature();

  // Labelize and extract keypoints
  // Warning : order matters
  if (this->Enabled[Keypoint::BLOB])
    this->ComputeBlobs();
  if (this->Enabled[Keypoint::PLANE])
    this->ComputePlanes();
  if (this->Enabled[Keypoint::EDGE])
    this->ComputeEdges();
  if (this->Enabled[Keypoint::INTENSITY_EDGE])
    this->ComputeIntensityEdges();
}

其中：

ConvertAndSortScanLines函数将PointCloud中存储的点云按扫描线id进行划分，其结果存储在this->ScanLines中，this->ScanLines的类型为std::vector<PointCloud::Ptr>。如果事先没有指定方位角分辨率，会调用函数EstimateAzimuthalResolution进行方位角分辨率计算。

PrepareDataForNextFrame函数将计算过程中的数据结构进行重置。

ComputeCurvature函数中进行了曲率计算，其基本思想与LOAM一致。

剩下的四个函数分别计算了相应的关键点。

函数ConvertAndSortScanLines

遍历所有点，将其按扫描线存放。其中laser_id需要从0开始连续增加。该函数中没有计算每个点的方位角，因此要求点云中点的顺序需要是按扫描顺序存放。即先扫描的点在前（方位角小的点），后扫描的点在后（方位角大的点）。

步骤如下：

（1）重置this->ScanLines内存

（2）遍历该帧点云，根据laser_id将其放到对应的扫描线点云中。要求laser_id从0开始增长

（3）保存扫描吸线的数量this->NbLaserRings

（4）如果没有设定方位角分辨率（水平角度分辨率），则根据点云计算方位角分辨率。

void SpinningSensorKeypointExtractor::ConvertAndSortScanLines()
{
  // 重置this->ScanLines内存
  // Clear previous scan lines
  for (auto& scanLineCloud: this->ScanLines)
  {
    // Use clear() if pointcloud already exists to avoid re-allocating memory.
    // No worry as ScanLines is never shared with outer scope.
    if (scanLineCloud)
      scanLineCloud->clear();
    else
      scanLineCloud.reset(new PointCloud);
  }

  // 根据laser_id属性，将每一个点放入到对应的this->ScanLines中
  // 此处没有计算每个点的方位角，因此是默认点云中的点是根据扫描顺序（即方位角）依次存储
  // 要求laser_id是连续的。
  // Separate pointcloud into different scan lines
  for (const Point& point: *this->Scan)
  {
    // Ensure that there are enough available scan lines
    while (point.laser_id >= this->ScanLines.size())
      this->ScanLines.emplace_back(new PointCloud);

    // Add the current point to its corresponding laser scan
    this->ScanLines[point.laser_id]->push_back(point);
  }

  // 记录扫描线数量
  // Save the number of lasers
  this->NbLaserRings = this->ScanLines.size();

  // 如果没有指定方位角分辨率，则通过点估计方位角分辨率
  // Estimate azimuthal resolution if not already done
  // or if the previous value found is not plausible
  // (because last scan was badly formed, e.g. lack of points)
  if (this->AzimuthalResolution < 1e-6 || M_PI/4. < this->AzimuthalResolution)
    this->EstimateAzimuthalResolution();
}

函数PrepareDataForNextFrame

将计算需要的变量重置，重新分配内存。

this->Angles：std::vector<std::vector<float>>类型，左邻域和右邻域拟合直线方向夹角的sin值。如果是角点，会存储其最大的sin值，其邻域内为-1。

this->DepthGap：std::vector<std::vector<float>>类型，由当前点指向前一个点的向量在前一个激光束上的投影的大小，以及当前点指向后一个点的向量在当前激光束上的投影的大小。两个投影模长的最大值。

this->SpaceGap ：std::vector<std::vector<float>>类型， 存储当同一条扫描线上，当前点到前一个点的距离以及后一个点距离的最大值。该点是有效点，即激光束与平面不是水平（角度大于10度），当前点与其前后相邻的点的距离小于阈值（5个点乘以水平角分辨率）。

this->IntensityGap：std::vector<std::vector<float>>类型，当前点左邻域点强的平均值减去右领域点强度的平均值的绝对值

this->Label：std::vector<std::vector<KeypointFlags>>类型，每个点的特征值，KeypointFlags类型为std::bitset<Keypoint::nKeypointTypes>，即其对应位上存储了是否为某个特征点。

void SpinningSensorKeypointExtractor::PrepareDataForNextFrame()
{
  // 根据扫描线数量重置各个变量
  // Initialize the features vectors with the correct length
  this->Angles.resize(this->NbLaserRings);
  this->DepthGap.resize(this->NbLaserRings);
  this->SpaceGap.resize(this->NbLaserRings);
  this->IntensityGap.resize(this->NbLaserRings);
  this->Label.resize(this->NbLaserRings);

  // 初始化各个变量的每条扫描线对应的内存，并赋值为默认值。
  // Initialize the scan lines features vectors with the correct length
  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided)
  for (int scanLine = 0; scanLine < static_cast<int>(this->NbLaserRings); ++scanLine)
  {
    size_t nbPoint = this->ScanLines[scanLine]->size();
    this->Label[scanLine].assign(nbPoint, KeypointFlags().reset());  // set all flags to 0
    this->Angles[scanLine].assign(nbPoint, -1.);
    this->DepthGap[scanLine].assign(nbPoint, -1.);
    this->SpaceGap[scanLine].assign(nbPoint, -1.);
    this->IntensityGap[scanLine].assign(nbPoint, -1.);
  }

  // Reset voxel grids
  LidarPoint minPt, maxPt;
  pcl::getMinMax3D(*this->Scan, minPt, maxPt);

  // 设置当前帧地图范围
  for (auto k : KeypointTypes)
  {
    if (this->Keypoints.count(k))
      this->Keypoints[k].Clear();
    if (this->Enabled[k])
      this->Keypoints[k].Init(minPt.getVector3fMap(), maxPt.getVector3fMap(), this->VoxelResolution, this->Scan->size());
  }
}

 

函数ComputeCurvature

计算曲率。

该函数中曲率的计算并没有使用LOAM中的曲率计算公式。

而是对左邻域和右邻域的点分别进行直线拟合，如果直线拟合成功，则计算左邻域和右邻域拟合的直线的夹角，this->Angles中存储了两条直线夹角的sin值。

该函数没有传入参数，没有返回值。

其中点云数据已经存储在this->ScanLines中。

函数开始先计算了一系列参数的值，包括角度转弧度，计算cos，sin等。判断最大最小角度。

该部分中创建了随机数，函数中使用随机数进行随机点选取，从而达到减小计算量的目的。

代码如下：

  // Compute useful const values to lighten the loop
  // 激光束和扫描的平面的最小夹角
  const float cosMinBeamSurfaceAngle = std::cos(Utils::Deg2Rad(this->MinBeamSurfaceAngle));
  // 水平角分辨率（方位角分辨率）
  const float cosMaxAzimuth = std::cos(1.5 * this->AzimuthalResolution);
  // 水平方向上两个点的角度（this->EdgeNbGapPoints=5）
  const float cosSpaceGapAngle = std::cos(this->EdgeNbGapPoints * this->AzimuthalResolution);
  float azimuthMinRad = Utils::Deg2Rad(this->AzimuthMin); // 雷达扫过的方位角的起始角度，默认值为0
  float azimuthMaxRad = Utils::Deg2Rad(this->AzimuthMax); // 雷达扫过的方位角的终止角度，默认值为360

  // Rescale angles in [0, 2pi]
  while (azimuthMinRad < 0)
    azimuthMinRad += 2 * M_PI;
  while (azimuthMaxRad < 0)
    azimuthMaxRad += 2 * M_PI;

  std::random_device rd;  // Will be used to obtain a seed for the random number engine
  std::mt19937 gen(rd()); // Standard mersenne_twister_engine seeded with rd()
  std::uniform_real_distribution<> dis(0.0, 1.0);

然后在for循环中依次对每条扫描线进行处理。

  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided)
  for (int scanLine = 0; scanLine < static_cast<int>(this->NbLaserRings); ++scanLine)
  {

  }

对于每条扫描线，首先判断点的数量，只有数量满足计算曲率的要求时进行计算，否则跳入吓一跳扫面线。

    // 每个循环中处理一条扫描线上的点
    // Useful shortcuts
    const PointCloud& scanLineCloud = *(this->ScanLines[scanLine]);
    const int Npts = scanLineCloud.size();

    // if the line is almost empty, skip it
    // 扫描线中的点数量过少，无法计算曲率
    if (this->IsScanLineAlmostEmpty(Npts))
      continue;

然后依次处理每条扫面线中的每一个点：

  for (int index = 0; index < Npts; ++index)
    {

    }

对于每一个点，首先使用随机数进行筛选，跳过一定比例的点，达到减小数据量的作用。然后判断当前点的距离，离坐标原点太近的点被认为是无效点。

      // Random sampling to decrease keypoints extraction
      // computation time
      // 使用随机数省略一些点，减少运行时间
      // dis产生0-1之间的随机数
      // 如果 this->InputSamplingRatio > 1，则说明所有点都进行检测
      // this->InputSamplingRatio 默认为 1
      if (this->InputSamplingRatio < 1.f && dis(gen) > this->InputSamplingRatio)
        continue;
      // Central point
      const Eigen::Vector3f& centralPoint = scanLineCloud[index].getVector3fMap();
      float centralDepth = centralPoint.norm();

      // Check distance to sensor
      // 如果该点靠近原点，则认为是不可靠点（this->MinDistanceToSensor=1.5m）
      if (centralDepth < this->MinDistanceToSensor)
        continue;

然后检查方位角。方位角应该在设定的最小、最大方位角之间。

      // Check azimuth angle
      if (std::abs(azimuthMaxRad - azimuthMinRad) < 2 * M_PI - 1e-6)
      {
        // X轴正方向为0度，右手坐标系为正方向，Y轴正方向为90度，X轴负方向为180度，Y轴负方向为270度。
        float cosAzimuth = centralPoint.x() / std::sqrt(std::pow(centralPoint.x(), 2) + std::pow(centralPoint.y(), 2));
        // 根据Y值进行判断，将方位角转换成[0, 2*M_PI]
        float azimuth = centralPoint.y() > 0? std::acos(cosAzimuth) : 2*M_PI - std::acos(cosAzimuth);
        if (azimuthMinRad == azimuthMaxRad)
          continue;
        if (azimuthMinRad < azimuthMaxRad &&
            (azimuth < azimuthMinRad ||
             azimuth > azimuthMaxRad ))
          continue;

        if (azimuthMinRad > azimuthMaxRad &&
            (azimuth < azimuthMinRad && azimuth > azimuthMaxRad))
          continue;
      }

将左右邻域的点的索引分别加入到对应的索引向量中。

需要注意的是leftNeighbors和rightNeighbors中点索引的存储顺序。靠近当前点的点在leftNeighbors和rightNeighbors索引小，远离当前点索引增大。该索引会影响直线拟合的方向。

      // Fill left and right neighbors
      // Those points must be more numerous than MinNeighNb and occupy more space than MinNeighRadius
      std::vector<int> leftNeighbors;
      int idxNeigh = 1;
      float lineLength = 0.f;
      while ((int(leftNeighbors.size()) < this->MinNeighNb
              || lineLength < this->MinNeighRadius)
              && int(leftNeighbors.size()) < Npts)
      { // 将左邻域点的索引加入到点集，从靠近当前的位置向前添加
        leftNeighbors.push_back((index - idxNeigh + Npts) % Npts); // index从0开始，此算法针对360扫描的雷达
        lineLength = (scanLineCloud[leftNeighbors.back()].getVector3fMap() - scanLineCloud[leftNeighbors.front()].getVector3fMap()).norm();
        ++idxNeigh;
      }

      std::vector<int> rightNeighbors;
      idxNeigh = 1;
      lineLength = 0.f;
      // this->MinNeihNB 默认值为4
      while ((int(rightNeighbors.size()) < this->MinNeighNb
             || lineLength < this->MinNeighRadius)
            && int(rightNeighbors.size()) < Npts)
      { // 将右邻域点的索引加入到点集，从靠近当前点的位置向后添加
        rightNeighbors.push_back((index + idxNeigh) % Npts);
        lineLength = (scanLineCloud[rightNeighbors.back()].getVector3fMap() - scanLineCloud[rightNeighbors.front()].getVector3fMap()).norm();
        ++idxNeigh;
      }

 计算长度夹角等一些量。

      // 当前点左右相邻的两个点
      const auto& rightPt = scanLineCloud[rightNeighbors.front()].getVector3fMap();
      const auto& leftPt = scanLineCloud[leftNeighbors.front()].getVector3fMap();

      // 点到原点的长度
      const float rightDepth = rightPt.norm();
      const float leftDepth = leftPt.norm();

      //当前点和前后点的夹角
      // centralPoint 当前点 x y z Vector3f
      const float cosAngleRight = std::abs(rightPt.dot(centralPoint) / (rightDepth * centralDepth));
      const float cosAngleLeft = std::abs(leftPt.dot(centralPoint) / (leftDepth * centralDepth));

      const Eigen::Vector3f diffVecRight = rightPt - centralPoint;
      const Eigen::Vector3f diffVecLeft = leftPt - centralPoint;

      const float diffRightNorm = diffVecRight.norm();
      const float diffLeftNorm = diffVecLeft.norm();

      float cosBeamLineAngleLeft = std::abs(diffVecLeft.dot(centralPoint) / (diffLeftNorm * centralDepth) );
      float cosBeamLineAngleRight = std::abs(diffVecRight.dot(centralPoint) / (diffRightNorm * centralDepth));

如果需要提取边缘角点，需要计算this->SpaceGap和this->DepthGap。

      if (this->Enabled[EDGE])
      {
        // Compute space gap

        // Init variables
        float distRight = -1.f;
        float distLeft = -1.f;

        // Compute space gap (if some neighbors were missed)
        // cosMinBeamSurfaceAngle 为 10 度时的cos值
        // cosBeamLineAngleRight < cosMinBeamSurfaceAngle 等价于 排除平行点
        if (cosBeamLineAngleRight < cosMinBeamSurfaceAngle && cosAngleRight < cosSpaceGapAngle)
          distRight = diffRightNorm;

        if (cosBeamLineAngleLeft < cosMinBeamSurfaceAngle && cosAngleLeft < cosSpaceGapAngle)
          distLeft = diffLeftNorm;

        // 角度Gap：两个点之间的角度大于阈值
        // 非平面点
        // 两个点与原点形成的向量，之间的角度大于5倍的水平角分辨率
        // 此时说明当前点与点云中统一扫描线上相邻的点之间间隔过大
        // 当前点到相邻的左邻域点和右邻域点的最大距离。
        this->SpaceGap[scanLine][index] = std::max(distLeft, distRight);
        // 如果是平面点，或者当前点与相邻的前后两点在5倍的角度分辨率内，则SpaceGap值为-1，即不构成Gap

        // Stop search for first and last points of the scan line
        // because the discontinuity may alter the other criteria detection
        if (index < int(leftNeighbors.size()) || index >= Npts - int(rightNeighbors.size()))
          continue;

        // Compute depth gap

        // Reinit variables
        distRight = -1.f;
        distLeft = -1.f;

        // 同一条扫描线上相邻激光束之间的夹角小于阈值（分辨率的1.5倍）
        if (cosAngleRight > cosMaxAzimuth) 
          // diffVecRight在centralPoint方向上投影的长度
          distRight = centralPoint.dot(diffVecRight) / centralDepth;
        if (cosAngleLeft > cosMaxAzimuth)
          // diffVecLeft在leftPt方向上投影的长度
          distLeft = leftPt.dot(diffVecLeft) / leftDepth;

        // Check right points are consecutive + not on a bended wall
        // If the points lay on a bended wall, previous and next points should be in the same direction
        if (distRight > this->EdgeDepthGapThreshold) // this->EdgeDepthGapThreshold = 0.5m
        { 
          // 当前点和右邻域第一个点的夹角小于1.5倍分辨率，但是两点组成的向量在当前点方向上的投影距离大于this->EdgeDepthGapThreshold
          // 当前点和右领域前两个点，当前点和左右邻域的第一个点
          // 这两组点右一组的三个点在一个平面上
          auto nextdiffVecRight = (scanLineCloud[rightNeighbors[1]].getVector3fMap() - rightPt).normalized();
          if ((nextdiffVecRight.dot(diffVecRight) / diffRightNorm) > cosMinBeamSurfaceAngle ||
              (-diffVecLeft.dot(diffVecRight) / (diffRightNorm * diffLeftNorm)) > cosMinBeamSurfaceAngle)
            distRight = -1.f;
        }

        // Check left points are consecutive + not on a bended wall
        // If the points lay on a bended wall, previous and next points should be in the same direction
        if (distLeft > this->EdgeDepthGapThreshold)
        {
          auto prevdiffVecLeft = (scanLineCloud[leftNeighbors[1]].getVector3fMap() - leftPt).normalized();
          if ((prevdiffVecLeft.dot(diffVecLeft) / diffLeftNorm > cosMinBeamSurfaceAngle) ||
              (-diffVecRight.dot(diffVecLeft) / (diffRightNorm * diffLeftNorm)) > cosMinBeamSurfaceAngle)
            distLeft = -1.f;
        }

        // 深度Gap：当前点与左右邻域第一个点组成的向量，在当前点方向上的投影距离大于阈值
        this->DepthGap[scanLine][index] = std::max(distLeft, distRight);
      }

进行直线拟合。然后对直线方向进行判断，如果直线与当前点与原点形成的向量接近平行，则说明这些点属于异常点，即LOAM中的平行点。即下图中的B点。分别对左邻域和右邻域拟合的直线进行判断。

      // 当前点与左右邻域第一个点的夹角大于10度。 AngleRight > 10度 或者 AngleLeft > 10度
      if (cosAngleRight < cosMaxAzimuth || cosAngleLeft < cosMaxAzimuth)
        continue;

      // 进行直线拟合。
      // Fit line on the left and right neighborhoods and
      // skip point if they are not usable
      LineFitting leftLine, rightLine;
      if (!leftLine.FitLineAndCheckConsistency(scanLineCloud, leftNeighbors) ||
          !rightLine.FitLineAndCheckConsistency(scanLineCloud, rightNeighbors))
        continue;

      // 左邻域的点形成的直线与激光束接近平行，则该点不可靠
      cosBeamLineAngleLeft = std::abs(leftLine.Direction.dot(centralPoint) / centralDepth);
      if (cosBeamLineAngleLeft > cosMinBeamSurfaceAngle)
        continue;

      // 右邻域的点形成的直线与激光束接近平行，则该点不可靠
      cosBeamLineAngleRight = std::abs(rightLine.Direction.dot(centralPoint) / centralDepth);
      if (cosBeamLineAngleRight > cosMinBeamSurfaceAngle)
        continue;

如果需要进行强度边缘角点提取，则需要对当前点左右领域的点强度进行计算。

      if (this->Enabled[INTENSITY_EDGE])
      {
        // Compute intensity gap
        if (std::abs(scanLineCloud[rightNeighbors.front()].intensity - scanLineCloud[leftNeighbors.front()].intensity) > this->EdgeIntensityGapThreshold)
        { // 当前点的点一个点和后一个点的强度之差的绝对值，大于阈值 （this->EdgeIntensityGapThreshold = 50.）
          // Compute mean intensity on the left
          float meanIntensityLeft = 0; // 左邻域点强度平均值
          for (int indexLeft : leftNeighbors)
            meanIntensityLeft += scanLineCloud[indexLeft].intensity;
          meanIntensityLeft /= leftNeighbors.size();
          // Compute mean intensity on the right
          float meanIntensityRight = 0; // 右邻域点强度平均值
          for (int indexRight : rightNeighbors)
            meanIntensityRight += scanLineCloud[indexRight].intensity;
          meanIntensityRight /= rightNeighbors.size();
          this->IntensityGap[scanLine][index] = std::abs(meanIntensityLeft - meanIntensityRight);

          // Remove neighbor points to get the best intensity discontinuity locally
          if (this->IntensityGap[scanLine][index-1] < this->IntensityGap[scanLine][index])
            this->IntensityGap[scanLine][index-1] = -1;
          else
            this->IntensityGap[scanLine][index] = -1;
        }
      }

 如果当前点到左邻域或右邻域直线的距离大于阈值，则判定为异常点，即LOAM中的遮挡点。

      // Check point is not too far from the fitted lines before computing an angle
      // 如果当前点到左邻域或者右邻域拟合的直线的距离过远，说明该存在遮挡情况，则当前点判定为不可靠点
      if (leftLine.DistanceToPoint(centralPoint) > this->MaxDistance || rightLine.DistanceToPoint(centralPoint) > this->MaxDistance)
        continue;

如果需要提起角点和面点，则需要计算this->Angle，该变量中存储的是左邻域和右邻域拟合的直线的角度的sin值。

      if (this->Enabled[PLANE] || this->Enabled[EDGE])
      {
        // Compute angles
        // 左邻域和右邻域拟合的直线之间的夹角
        this->Angles[scanLine][index] = (leftLine.Direction.cross(rightLine.Direction)).norm();
        // Remove previous point from angle inspection if the angle is not maximal locally
        if (this->Enabled[EDGE] && this->Angles[scanLine][index] > this->EdgeSinAngleThreshold)
        { // this->Angles > this->EdgeSinAngleThreshold
          // 等价与两个直线的夹角位于60-120度
          // this->EdgeSinAngleThreshold = 0.86，即sin(60°)
          
          // Check previously computed angle to keep only the maximum angle keypoint locally
          for (int indexLeft : leftNeighbors)
          {
            if (this->Angles[scanLine][indexLeft] <= this->Angles[scanLine][index])
              this->Angles[scanLine][indexLeft] = -1;
            else
            {
              this->Angles[scanLine][index] = -1;
              break;
            }
          }
        }
      }

函数AddKptsUsingCriterion

该函数对特定的观点进行提取。其声明如下：

  // Add all keypoints of the type k that comply with the threshold criteria for these values
  // The threshold can be a minimum or maximum value (threshIsMax)
  // The weight basis allow to weight the keypoint depending on its certainty
  void AddKptsUsingCriterion (Keypoint k,
                              const std::vector<std::vector<float>>& values,
                              float threshold,
                              bool threshIsMax = true,
                              double weightBasis = 1.);

传入参数values即为this->Angles，this->DepthGap，this->SpaceGap，this->IntensityGap。最终的结果保存在this->Label中。

其过程为对每条扫面先的对应数据进行排序，然后根据大于或小于阈值进行this->Label的设定。

代码如下：

void SpinningSensorKeypointExtractor::AddKptsUsingCriterion (Keypoint k,
                                                             const std::vector<std::vector<float>>& values,
                                                             float threshold,
                                                             bool threshIsMax,
                                                             double weightBasis)
{
  // Loop over the scan lines
  for (int scanlineIdx = 0; scanlineIdx < static_cast<int>(this->NbLaserRings); ++scanlineIdx)
  {
    const int Npts = this->ScanLines[scanlineIdx]->size();

    // If the line is almost empty, skip it
    if (this->IsScanLineAlmostEmpty(Npts))
      continue;

    // If threshIsMax : ascending order (lowest first)
    // If threshIsMin : descending order (greatest first)
    std::vector<size_t> sortedValuesIndices = Utils::SortIdx(values[scanlineIdx], threshIsMax);

    for (const auto& index: sortedValuesIndices)
    {
      // If the point was already picked, or is invalid, continue
      if (this->Label[scanlineIdx][index][k] || values[scanlineIdx][index] < 0)
        continue;

      // Check criterion threshold
      bool valueAboveThresh = values[scanlineIdx][index] > threshold;
      // If criterion is not respected, break loop as indices are sorted.
      if ((threshIsMax && valueAboveThresh) ||
          (!threshIsMax && !valueAboveThresh))
        break;

      // The points with the lowest weight have priority for extraction
      float weight = threshIsMax? weightBasis + values[scanlineIdx][index] / threshold : weightBasis - values[scanlineIdx][index] / values[scanlineIdx][0];

      // Indicate the type of the keypoint to debug and to exclude double edges
      this->Label[scanlineIdx][index].set(k);
      // Add keypoint
      this->Keypoints[k].AddPoint(this->ScanLines[scanlineIdx]->at(index), weight);
    }
  }
}

函数ComputePlanes

 选取this->Angle中小于this->PlaneSinAngleThreshold的点。

this->PlaneSinAngleThreshold = 0.5，即sin(30°)的值。即角度位于(0, 30)和(150, 180)两个区间内。

结合左邻域和右邻域点的排列方式，直线夹角位于(150, 180)度之间的点判定为平面点（Keypoint::PLANE）。

void SpinningSensorKeypointExtractor::ComputePlanes()
{
  this->AddKptsUsingCriterion(Keypoint::PLANE, this->Angles, this->PlaneSinAngleThreshold);
}

函数ComputeEdges

符合以下条件的点判定为边缘角点（Keypoint::EDGE）：

（1）this->DepthGap大于this->EdgeDepthGapThreshold（默认值为0.5）。

（2）this->Angles大于this->EdgeSinAngleThreshold（默认值为0.86，sin(60)）。

（3）this->SpaceGap大于this->EdgeDepthGapThreshold（默认值为0.5）。

其中this->Angles条件为选取两个直线夹角范围在（60，120）之间的点。

void SpinningSensorKeypointExtractor::ComputeEdges()
{
  this->AddKptsUsingCriterion(Keypoint::EDGE, this->DepthGap, this->EdgeDepthGapThreshold, false, 1);
  this->AddKptsUsingCriterion(Keypoint::EDGE, this->Angles, this->EdgeSinAngleThreshold, false, 2);
  this->AddKptsUsingCriterion(Keypoint::EDGE, this->SpaceGap, this->EdgeDepthGapThreshold, false, 3);
}

函数ComputeIntensityEdges

将强度大于this->EdgeIntensityGapThreshold（默认值为50）的点判定为强度角点。

void SpinningSensorKeypointExtractor::ComputeIntensityEdges()
{
  this->AddKptsUsingCriterion(Keypoint::INTENSITY_EDGE, this->IntensityGap, this->EdgeIntensityGapThreshold, false);
}

函数ComputeBlobs

 将所有点加入到Keypoint::BLOB特征中。即Keypoint::BLOB中存储了当前帧完整的点云。

void SpinningSensorKeypointExtractor::ComputeBlobs()
{
  for (unsigned int scanLine = 0; scanLine < this->NbLaserRings; ++scanLine)
  {
    for (unsigned int index = 0; index < this->ScanLines[scanLine]->size(); ++index)
      this->Keypoints[Keypoint::BLOB].AddPoint(this->ScanLines[scanLine]->at(index));
  }
}