loam代码分析(1)

loam代码分析(1)

  • 概述
  • imageProjecion.cpp
    • 获取点云角度范围 (findStartEndAngle())
    • 无序的点云变为有序(projectPointCloud())
    • 地面分割groundRemoval()
    • 点云聚类(cloudSegmentation)
    • 点云发布

概述

目前最新开源的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
  7. 地面和非地面分割;
  8. 点云聚类
  9. 发布点云

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

// 查找整个点云数据起始角度和终止角度
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;
}

无序的点云变为有序(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);
  1. 获取此点与x,y轴平面的夹角
    float verticalAngle = std::asin(thisPoint.z / range);                             // 获取z轴的角度
  1. 获取垂直方向上的索引
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维激光数据,并按顺序存储;

地面分割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. 由于目前存储的是按序存储,由于激光雷达主要为水平安装一定高度,因此16组雷达中,第7组则一般为水平雷达,因此第7组雷达以下的点云数据有可能为地面点云;
  2. 判断是否为地面的方法为,找到第1组雷达(即朝下的第一组激光点)中每个点,找到对应相邻组中同一水平索引的点。如果此两点的俯仰角变化在一范围内,则认为为同一平面,则为地面上点;

注:水平索引方向相同,从第一行开始判断相邻的点。
loam代码分析(1)_第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越接近在一个平面上,故可认为是同一类;
loam代码分析(1)_第2张图片
在分类时还需考虑此物体大小限制,过小则不进行具体分类。其原理:
1.同一类别点个数需超过30个;
2.个数超过5个并且在垂直方向上跨过3个区域(因为垂直方向角度分辨率较大);

点云发布

  1. 发布已分类的点云数据,包含地面和非地面数据;
  2. 发布未被分类的点云(即小物体,孤立的点云簇;

你可能感兴趣的:(slam)