LOAM代码笔记（一）

LOAM主要由四个节点组成，分别为：

multiScanRegistration、laserOdometry、laserMapping和transformMaintenance。

其中，核心算法封装在BasicScanRegistration、BasicLaserOdomotry、BasicLaserMapping和BasicTransformMaintenance中，这几个类的实现不涉及ROS的函数，为loam移植到其他平台提供了方便。

按照数据流的顺序，我们来一步一步分析loam的算法。首先是multiScanRegistraition。

multiScanRegistration

设置节点的主程序位于multi_scan_registration_node.cpp中，代码十分简洁，只有20行，事实上，四个节点的主程序部分都是20行，非常工整。

multi_scan_registration_node.cpp：

#include 
#include "loam_velodyne/MultiScanRegistration.h"


/** Main node entry point. */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "scanRegistration");
  ros::NodeHandle node;
  ros::NodeHandle privateNode("~");

  loam::MultiScanRegistration multiScan;

  if (multiScan.setup(node, privateNode)) {
    // initialization successful
    ros::spin();
  }

  return 0;
}

点云注册的算法被封装在了MultiScanRegistration类中，他的继承关系为：

BasicLaserRegistration ———> ScanRegistraition ——>MultiScanRegistraion

ScanRegistration在父类的基础上增加了读取节点参数、发布/订阅消息的函数。MultiScanRegistraion在父类的基础上增加了对点云数据的预处理。

loam支持16线、32线和64线的激光雷达，默认参数为16线，可以对MultiScanMapper进行修改来切换不同的激光雷达模式。

MultiScanRegistration算法流程：

（1）预处理：读取点云后，先计算激光雷达的起始角度和终止角度，这里有一点需要注意一下，激光雷达一个扫描周期扫过的角度不一定刚好是360度。起始角为一帧点云数据中第一个点的水平角，终止角为最后一个点的水平角。去除点云中的极小值（坐标接近于0），对点云的坐标顺序进行调整，将坐标系改成：向左—x轴，向上—y轴，向前—z轴，根据垂直角计算每个点在第几条扫描线上（scanID），然后根据数据点的水平角计算该点在一个扫描周期中的相对时间，公式为：
$$RelativeTime=ScanPeriod\times \frac{HorizontalAngle-StartAngle}{EndAngle-StartAngle}$$
如果有IMU的话，借助IMU数据把数据点投影到开始扫描的位置。然后把调整过的点云按照各自所在的扫描线存入一个vector中，vector的大小为激光雷达的线数。这样一来就得到了一堆分布有序的点云。

void MultiScanRegistration::process(const pcl::PointCloud& laserCloudIn, const Time& scanTime)//scanTime:发送时候的时间
{
  size_t cloudSize = laserCloudIn.size();

  // determine scan start and end orientations
  float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);
  float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,
                             laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
  if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  }

  bool halfPassed = false;
  pcl::PointXYZI point;
  _laserCloudScans.resize(_scanMapper.getNumberOfScanRings());//_scanMapper.getNumberOfScanRings() = 16
  // clear all scanline points
  std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), [](auto&&v) {v.clear(); }); 

  // extract valid points from input cloud
  for (int i = 0; i < cloudSize; i++) 
  {
    point.x = laserCloudIn[i].y;
    point.y = laserCloudIn[i].z;
    point.z = laserCloudIn[i].x;

    // skip NaN and INF valued points
    if (!pcl_isfinite(point.x) ||
        !pcl_isfinite(point.y) ||
        !pcl_isfinite(point.z)) {
      continue;
    }

    // skip zero valued points
    if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
      continue;
    }

    // calculate vertical point angle and scan ID
    float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
    int scanID = _scanMapper.getRingForAngle(angle);
    if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 )//_scanMapper.getNumberOfScanRings() = 16
    {
      continue;
    }

    // calculate horizontal point angle
    float ori = -std::atan2(point.x, point.z);
    if (!halfPassed) 
    {
      if (ori < startOri - M_PI / 2) 
      {
        ori += 2 * M_PI;
      } 
      else if (ori > startOri + M_PI * 3 / 2) 
      {
        ori -= 2 * M_PI;
      }

      if (ori - startOri > M_PI) 
      {
        halfPassed = true;
      }
    } 
    else 
    {
      ori += 2 * M_PI;

      if (ori < endOri - M_PI * 3 / 2) 
      {
        ori += 2 * M_PI;
      } 
      else if (ori > endOri + M_PI / 2) 
      {
        ori -= 2 * M_PI;
      }
    }

    // calculate relative scan time based on point orientation
    float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
    point.intensity = scanID + relTime;

    projectPointToStartOfSweep(point, relTime);//没IMU，point不变

    _laserCloudScans[scanID].push_back(point);
  }

  processScanlines(scanTime, _laserCloudScans);
  publishResult();
}

(2)提取特征：loam把点云的特征点根据曲率分为两类——角点和平面点。计算曲率的算法跟论文里的公式有一点出入。论文里计算曲率的公式为：

而代码里用的是公式求和后向量的平方和。计算曲率时，为了让特征点能够尽量均匀地分布，把每条扫描线分成若干个区域，默认分成6个区域，在每个区域内取2个角点，4个平面点。计算曲率时，在这个点的左边取n个点，右边取n个点作为领域，默认的n = 5。然后对当前特征区域内的点根据曲率排序，取曲率最大的2个点作为角点，曲率最小的4个点作为平面点，存到各自的点云变量中。这部分排序的代码看上去有点粗糙，还可以优化一下。以下是计算曲率并排序的代码：

void BasicScanRegistration::setRegionBuffersFor(const size_t& startIdx, const size_t& endIdx)
{
  // resize buffers
  size_t regionSize = endIdx - startIdx + 1;
  _regionCurvature.resize(regionSize);
  _regionSortIndices.resize(regionSize);
  _regionLabel.assign(regionSize, SURFACE_LESS_FLAT);

  // calculate point curvatures and reset sort indices
  float pointWeight = -2 * _config.curvatureRegion;

  for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) 
  {
    float diffX = pointWeight * _laserCloud[i].x;
    float diffY = pointWeight * _laserCloud[i].y;
    float diffZ = pointWeight * _laserCloud[i].z;

    for (int j = 1; j <= _config.curvatureRegion; j++) 
    {
      diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;
      diffY += _laserCloud[i + j].y + _laserCloud[i - j].y;
      diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z;
    }

    _regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ;
    _regionSortIndices[regionIdx] = i;
  }

  // sort point curvatures
  for (size_t i = 1; i < regionSize; i++) 
  {
    for (size_t j = i; j >= 1; j--) 
    {
      if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) 
      {
        std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]);
      }
    }
  }
}

得到的特征点存在cornerPointsSharp和surfacePointsFlat变量里，根据曲率阈值surfaceCurvatureThreshold把完整点云中大于阈值的点存入cornerPointsLessSharp中，小于阈值的存入surfacePointsLessFlat中，发送给laserOdomotry节点。