MultiScanRegistration源码分析¶

Note

LOAM 的整体思想就是将复杂的 SLAM 问题分为:1. 高频的运动估计;2. 低频(低一个数量级)的环境建图。整个系统的框架如图:

Lidar 接收数据,首先进行 Point Cloud Registration,Lidar Odometry 以 10Hz 的频率进行运动估计和坐标转换,Lidar Mapping 以 1Hz 的频率构建三维地图。这样做主要是为了保证系统的实时性。下面再来看看 rqt 节点图:

MultiScanRegistration 主要实现多种激光类型类,继承 ScanRegistration, 具体激光数据由 ScanRegis-tration 处理

class MultiScanRegistration : virtual public ScanRegistration {

组合类 MultiScanMapper, 实现激光配置, 包括垂直方向最低,最高角度,线数。

  MultiScanMapper _scanMapper;  ///< mapper for mapping vertical point angles to scan ring IDs
  MultiScanMapper(const float& lowerBound = -15,
                  const float& upperBound = 15,
                  const uint16_t& nScanRings = 16);

节点调用逻辑, 首先 ros 节点初始化->MultiScanRegistration 对象初始化-> 调用 setup, 初始化激光配置,订阅激光数据,调用 ScanRegistration::setup 发布边特征点云,平面特征点云,抽取特征后的总点云,imu_trans-> 点云 callback() 调用 process() 为点云特征抽取做准备-> 调用 extractFeatures() 抽取点云特征-> 发布。后续几个节点调用逻辑类似:

/** 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;
}

重点介绍 process() 函数

void MultiScanRegistration::process(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn,
                                    const ros::Time& scanTime)
{
  size_t cloudSize = laserCloudIn.size();

  // reset internal buffers and set IMU start state based on current scan time
  reset(scanTime);

  // 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;
  std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans(_scanMapper.getNumberOfScanRings());

  // 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));//计算垂直方向角度，这里有一个旋转轴问题，x(z),y(x),z(y)
    int scanID = _scanMapper.getRingForAngle(angle);//计算激光线束
    if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){//范围0-15
      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;//点intensity域，整数部分为scanID,小数部分为扫描时间，这样就完成点云中每个点分类

    // project point to the start of the sweep using corresponding IMU data
    if (hasIMUData()) {
      setIMUTransformFor(relTime);
      transformToStartIMU(point);
    }

    laserCloudScans[scanID].push_back(point);
  }

  // construct sorted full resolution cloud
  cloudSize = 0;
  for (int i = 0; i < _scanMapper.getNumberOfScanRings(); i++) {
    _laserCloud += laserCloudScans[i];//把整理好后的点云，按照激光束的线序存储在_laserCloud点云中。

    IndexRange range(cloudSize, 0);
    cloudSize += laserCloudScans[i].size();
    range.second = cloudSize > 0 ? cloudSize - 1 : 0;
    _scanIndices.push_back(range);//整理好后的点云索引，second??,16个pair中，每个pair，first代表每一束激光点云起始位置，second代表每一束激光的终点位置
  }

  // extract features
  extractFeatures();

  // publish result
  publishResult();
}

接下来就交给 ScanRegistration 处理抽取边特征,平面特征, 函数 extractFeatures();