lego-loam代码分析（1）-地面提取和点云类聚

概述

目前最新开源的3d slam算法lego-loam，为loam的改进版。同时另有高人进行了工程优化。
其原作者github
其工程优化的github
个人对其工程优化后的代码和原理进行了基本分析；
包含：

• imageProjecion
• FeatureAssociation
• mapOptmization
• transformFusion
  这四个文件

imageProjecion.cpp

imageProjecion.cpp为独立的线程，接收3D 点云数据，主要针对3D激光器采集的激光点云数据，因为代码中间所采用策略原理基本依赖于3d激光雷达扫描测距方式而来。
本线程主要工作是对3d 点云进行预处理，主要将其地面分割、聚类、非聚类、无序点云变有效点云等功能；
点云预处理基本流程：

1. 接收新的一帧cloud数据；
2. 初始化临时变量数据；
3. 将ros cloud 转换成pcl point格式；
4. 采用pcl库剔除无效值；
5. 统计雷达数据扫描起始角度和终止角度，包括角度差范围；
6. 将无序的点云数据，根据3d雷达（如16线 velodyne）的基本扫描原理，转换为有序点云，包括水平index和垂直index。可认为将3d点云转换为2维平面图像存储。
7. 地面和非地面分割；
8. 点云聚类
9. 发布点云

获取点云角度范围 （findStartEndAngle()）

先提供一张图，为vlp雷达驱动中计算点云坐标的方法，同时也是依据此还原序列的点云和距离信息。

从上图可看出，和一般单线激光雷达还有ros中scan的格式有一定差距，主要是水平扫描角度$\alpha$ 的表示意义差距较大。因此后续的许多坐标转换都和此有关系，其中$\alpha$为vlp16驱动中提供的扫描角度，因此后续畸变矫正等操作涉及扫描时刻和扫描方向也与此有关；

// 查找整个点云数据起始角度和终止角度
void ImageProjection::findStartEndAngle() {
  // start and end orientation of this cloud
  auto point = _laser_cloud_in->points.front();
  _seg_msg.startOrientation = -std::atan2(point.y, point.x);                // 起始角度

  point = _laser_cloud_in->points.back();
  _seg_msg.endOrientation = -std::atan2(point.y, point.x) + 2 * M_PI;       //  终止角度 + 360 

  if (_seg_msg.endOrientation - _seg_msg.startOrientation > 3 * M_PI) {     // 起始角度和终止角度 放在0~360之间
    _seg_msg.endOrientation -= 2 * M_PI;
  } else if (_seg_msg.endOrientation - _seg_msg.startOrientation < M_PI) {
    _seg_msg.endOrientation += 2 * M_PI;
  }
  _seg_msg.orientationDiff =                                                 // 终止与起始角度差
      _seg_msg.endOrientation - _seg_msg.startOrientation;
}

由于一个激光器是360度扫描，且VLP16其旋转速度和分辨率可调。存在一周所获取的点云个数可能会有一定跳动，且终点有可能大于此次起点的角度，也有可能未超过起点角度，故起始和终点角度在360左右，即可能超出360度，而不是一个准确常数。因此终点时刻的角度需要考虑是否进行2*PI翻转。

注：_seg_msg.startOrientation = -std::atan2(point.y, point.x); 此处角度求解形式与常见不同，主要根据传感器驱动有关。loam源码仅适合于VLP系列，若更换其他雷达，需要改正坐标系相关代码才可使用。其中取负号，猜测应该方便后续的计算，即逆时针旋转与笛卡尔坐标系角度方向一致，而VLP16是顺时针旋转

无序的点云变为有序（projectPointCloud()）

由于点云数据内存储每个点的信息为x，y，z为笛卡尔坐标，但是无法获知每个点云之间的相互关系。由于16线激光雷达实际上是16个激光探头同时旋转360度获取的距离信息。因此原数据为16组和单线激光雷达组成。根据所使用雷达已知参数（如16线雷达，包含16组，起始角度为-15度，终止的角度为15度，；每组中包含1800激光点）；
因此变为有序点云方法如下。

1. 获取此点到达激光头的距离

loat range = sqrt(thisPoint.x * thisPoint.x +                     // 反推点的距离
                       thisPoint.y * thisPoint.y +
                       thisPoint.z * thisPoint.z);

2. 获取此点与x，y轴平面的夹角

    float verticalAngle = std::asin(thisPoint.z / range);                             // 获取z轴的角度

3. 获取垂直方向上的索引

int rowIdn = (verticalAngle - _ang_bottom) / _ang_resolution_Y;    // 获取 扫描线中的索引号，_ang_bottom为起始角度，_ang_resolution_Y为垂直角度分辨率

4.获取此点与x，y轴平面内与x轴的夹角，并获取水平方向上的索引

    float horizonAngle = std::atan2(thisPoint.x, thisPoint.y);         // x/y ，范围为-PI～ PI， pi/2 表明为x轴方向
    int columnIdn = -round((horizonAngle - M_PI_2) / _ang_resolution_X) + _horizontal_scans * 0.5;   //  ??? 不知道为什么这么绕。 表明x轴方向为中间索引号

如此可获取16个在不同垂直（即俯仰角度）下的16组1维激光数据，并按顺序存储；

注意：理论上3d激光雷达与深度摄像机原始数据本身为有序的2维矩阵，但是ros驱动一般发布的为点云格式数据，因此此操作可认为是将点云格式还原回原始数据。如果针对使用的传感器，可修改相应的ros驱动相应直接发布有序序列。如此可利用激光扫描原理和有序性进行后续的分类和特征提取

地面分割groundRemoval()

  for (size_t j = 0; j < _horizontal_scans; ++j) {
    for (size_t i = 0; i < _ground_scan_index; ++i) {           // 仅遍历_ground_scan_index
      size_t lowerInd = j + (i)*_horizontal_scans;
      size_t upperInd = j + (i + 1) * _horizontal_scans;

      if (_full_cloud->points[lowerInd].intensity == -1 ||      // 垂直方向上相邻的两个点有一个存在无效值，？？？？？？？？没看到哪里赋值为无效值,不起任何作用
          _full_cloud->points[upperInd].intensity == -1) {
        // no info to check, invalid points
        _ground_mat(i, j) = -1;                                 // 表明此点无法判断
        continue;
      }

      float dX =
          _full_cloud->points[upperInd].x - _full_cloud->points[lowerInd].x;
      float dY =
          _full_cloud->points[upperInd].y - _full_cloud->points[lowerInd].y;
      float dZ =
          _full_cloud->points[upperInd].z - _full_cloud->points[lowerInd].z;

      float vertical_angle = std::atan2(dZ , sqrt(dX * dX + dY * dY + dZ * dZ));               // 存在bug，我觉的应该是sqrt(dX * dX + dY * dY）

      // TODO: review this change, 判断前后两点的角度变化在10度内

      if ( (vertical_angle - _sensor_mount_angle) <= 10 * DEG_TO_RAD) {
        _ground_mat(i, j) = 1;
        _ground_mat(i + 1, j) = 1;
      }
    }
  }

1. 由于目前存储的是按序存储，由于激光雷达主要为水平安装一定高度，因此１６组雷达中，第７组则一般为水平雷达，因此第７组雷达以下的点云数据有可能为地面点云；注：源码思想是假设地面为平的，同时激光雷达安装也应平行与地面，因此在使用时需要严格按照假设使用，否则无法获得预期效果。同时也是此算法的一个缺点
2. 按序存储的16组雷达，实际为可认为是16个上下固定角度间隔布置的单线激光雷达；
3. 判断是否为地面的方法为，找到第1组雷达（即朝下的第一组激光点）中每个点，找到对应相邻组中同一水平索引的点。如果此两点的俯仰角变化在一范围内，则认为为同一平面，则为地面上点；

注：水平索引方向相同，从第一行开始判断相邻的点。

4. 提取地面数据，提取非地面和非无效数据；

点云聚类（cloudSegmentation）

点云聚类的目的主要是将相邻较近的点认为为同一物体表面，主要用于后续的特征提取；认为为同一物体的原理，则遍历每个未被分类标记的点进行检测。以当前点开始将其上下，左右四个点分别列入待判断的buffer中，判断一点与其相邻点满足一条件。直到所有点都被分类；
这里的点云类聚仅考虑非无效值和非地面上的其他点云

      float d1 = std::max(_range_mat(fromInd.x(), fromInd.y()),    // 获取当前点和相邻点，距离较大值
                    _range_mat(thisIndX, thisIndY));
      float d2 = std::min(_range_mat(fromInd.x(), fromInd.y()),    // 获取较小值
                    _range_mat(thisIndX, thisIndY));

      float alpha = (iter.x() == 0) ? _ang_resolution_X : _ang_resolution_Y;  // 根据相邻方向获取水平或垂直方向的角度分辨率
      float tang = (d2 * sin(alpha) / (d1 - d2 * cos(alpha)));     // 实际为短线向长线做垂直线， 长线端点离垂线位置，越近，表明越平坦

      if (tang > segmentThetaThreshold) {                          // 越大表明越平坦，表明为同一分类，放入queue，继续扩展分类
        queue.push_back( {thisIndX, thisIndY } );

        _label_mat(thisIndX, thisIndY) = _label_count;             // 将其标记为同一分类
        lineCountFlag[thisIndX] = true;                            // 垂直方向的此行，已分类过

        all_pushed.push_back(  {thisIndX, thisIndY } );            // 是此分类的，均放入放入all pushed
      }

同一类原理如图所示;

水平方向和垂直方向alpha夹角不同，其中tang的角度越大，表明相邻的两个point越接近在一个平面上，故可认为是同一类；

在分类时还需考虑此物体大小限制，过小则不进行具体分类。其原理：
1.同一类别点个数需超过30个；
2.个数超过5个并且在垂直方向上跨过3个区域（因为垂直方向角度分辨率较大）；

---

点云发布

1. 发布已分类的点云数据，包含地面和非地面数据；其中地面点云在水平方向上被降采样；
2. 发布未被分类的点云（即小物体，孤立的点云簇），未被分类的在水平方向上被降采样;

注意：目前lego-loam假设匀速运动，未考虑点云的畸变处理