LOAM_velodyne 特征点的提取点云/IMU数据处理

注解：

代码流程：订阅了2个节点和发布了6个节点。通过回调函数的处理，将处理后的点云重新发出去。

功能：对点云和IMU数据进行预处理，用于特征点的配准。

具体实现：一次扫描的点通过曲率值来分类，特征点曲率大于阈值的为边缘点；特征点曲率小于阈值的为平面点。为了使特征点均匀的分布在环境中，将一次扫描划分为4个独立的子区域。每个子区域最多提供2个边缘点和4个平面点。此外，将不稳定的特征点（瑕点）排除。

代码流程：

1、主函数：main

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

1.构造 loam::MultiScanRegistration multiScan 对象

  MultiScanRegistration(const MultiScanMapper& scanMapper = MultiScanMapper());

  MultiScanMapper(const float& lowerBound = -15,
                  const float& upperBound = 15,
                  const uint16_t& nScanRings = 16);

默认设定了激光的扫描角度

第一扫描环的垂直角度 _lowerBound

最后一个扫描环的垂直角度 _upperBound

线性差值因子 _factor = (nScanRings - 1) / (upperBound - lowerBound)

2.setup函数的调用

2.multiScan 调用 setup 函数

multiScan.setup(node, privateNode)

bool MultiScanRegistration::setup(ros::NodeHandle& node, ros::NodeHandle& privateNode)
{
  RegistrationParams config;
  if (!setupROS(node, privateNode, config))
    return false;

  configure(config);
  return true;
}

*1 调用ScanRegistration 中的 setupROS

  if (!ScanRegistration::setupROS(node, privateNode, config_out))
    return false;

1)配置参数 RegistrationParams

    RegistrationParams(const float& scanPeriod_ = 0.1,
      const int& imuHistorySize_ = 200,
      const int& nFeatureRegions_ = 6,
      const int& curvatureRegion_ = 5,
      const int& maxCornerSharp_ = 2,
      const int& maxSurfaceFlat_ = 4,
      const float& lessFlatFilterSize_ = 0.2,
      const float& surfaceCurvatureThreshold_ = 0.1);

scanPeriod   激光每次扫描的时间
imuHistorySize IMU历史状态缓冲区的大小。
nFeatureRegions 用于在扫描中分布特征提取的（大小相等）区域的数量
curvatureRegion  用于计算点曲率的周围点数（点周围的+/-区域）
maxCornerSharp 每个要素区域的最大锐角点数。
maxCornerLessSharp 每个要素区域的最小锐角点数的最大数量  10 * maxCornerSharp_
maxSurfaceFlat  每个要素区域的最大平面点数。
lessFlatFilterSize 用于缩小剩余的较小平坦表面点的体素尺寸。
surfaceCurvatureThreshold  低于/高于点的曲率阈值被认为是平坦/角点

2)!setupROS(node, privateNode, config)

!ScanRegistration::setupROS(node, privateNode, config_out)

该函数里面第一步，解析参数

  if (!parseParams(privateNode, config_out))

该函数是从launch 文件中读取参数

3)订阅IMU话题和发布话题

  // subscribe to IMU topic
  _subImu = node.subscribe<sensor_msgs::Imu>("/imu/data", 50, &ScanRegistration::handleIMUMessage, this);

  // advertise scan registration topics
  _pubLaserCloud            = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);
  _pubCornerPointsSharp     = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);
  _pubCornerPointsLessSharp = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);
  _pubSurfPointsFlat        = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);
  _pubSurfPointsLessFlat    = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);
  _pubImuTrans              = node.advertise<sensor_msgs::PointCloud2>("/imu_trans", 5);

*2继续执行

确定激光的类型VLP-16 HDL-32 HDL-64E，并且设定了垂直视场角和线数

订阅点云数据

  // subscribe to input cloud topic
  _subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>
      ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);

3.IMU回调函数 handleIMUMessage

1.将imu的方向四元素转化成 rpy角

  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  double roll, pitch, yaw;
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

2.将x y z 的加速度转化到世界坐标系中

  Vector3 acc;
  acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81);
  acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81);
  acc.z() = float(imuIn->linear_acceleration.x + sin(pitch)             * 9.81);

3.跟新IMU数据 updateIMUData(acc, newState);

随时间累积IMU位置和速度

    // accumulate IMU position and velocity over time
    rotateZXY(acc, newState.roll, newState.pitch, newState.yaw);

    const IMUState& prevState = _imuHistory.last();
    float timeDiff = toSec(newState.stamp - prevState.stamp);
    newState.position = prevState.position
                        + (prevState.velocity * timeDiff)
                        + (0.5 * acc * timeDiff * timeDiff);
    newState.velocity = prevState.velocity
                        + acc * timeDiff;

4.点云数据回调函数

1.系统启动延迟计数器 int _systemDelay = 20;

systemDelay 有延时作用，保证有imu数据后在调用laserCloudHandler

  if (_systemDelay > 0) 
  {
    --_systemDelay;
    return;
  }

2.将激光数据转化成 pcl 点云描述

  pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

在ROS中点云的数据类型

在ROS中表示点云的数据结构有： sensor_msgs::PointCloud sensor_msgs::PointCloud2 pcl::PointCloud<T>

关于PCL在ros的数据的结构，具体的介绍可查看 wiki.ros.org/pcl/Overview

关于sensor_msgs::PointCloud2 和 pcl::PointCloud<T>之间的转换使用pcl::fromROSMsg 和 pcl::toROSMsg

sensor_msgs::PointCloud 和 sensor_msgs::PointCloud2之间的转换

使用sensor_msgs::convertPointCloud2ToPointCloud 和sensor_msgs::convertPointCloudToPointCloud2.

3.确定扫描开始和结束方向

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

4.clear all scanline points 清除所有扫描线点

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

5.从输入云中提取有效点，就是遍历点云数据

  for (int i = 0; i < cloudSize; i++) {

首先剔除 NaN和INF值点和零点

    if (!pcl_isfinite(point.x) ||
        !pcl_isfinite(point.y) ||
        !pcl_isfinite(point.z)) {
      continue;
    }
    if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
      continue;
    }

然后计算垂直角度和 ID scanID为计算激光在那条激光线上（16线）

    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 ){
      continue;
    }

计算水平点角度

    float ori = -std::atan2(point.x, point.z);

根据点方向计算相对扫描时间扫描一次时间*比例加上scanID

    float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
    point.intensity = scanID + relTime;

使用相应的IMU数据投射到扫描开始的点 projectPointToStartOfSweep

projectPointToStartOfSweep(point, relTime);

将点云根据scanID有序放到容器中 //for循环结束

_laserCloudScans[scanID].push_back(point);

std::vector<pcl::PointCloud<pcl::PointXYZI> > _laserCloudScans;

将新云处理为一组扫描线。 processScanlines

processScanlines(scanTime, _laserCloudScans);

4、 projectPointToStartOfSweep 函数

1.设置IMU转换根据时间计算 IMU位姿的偏移

插入IMU状态根据该时间内插的IMU状态对应于当前处理的激光扫描点的时间

interpolateIMUStateFor(relTime, _imuCur);

计算IMU的偏移

  float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;
  _imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;

2.将该点转化到 startIMU上

旋转点到全局IMU系统下

  rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);

添加全局IMU位姿的偏移

  point.x += _imuPositionShift.x();
  point.y += _imuPositionShift.y();
  point.z += _imuPositionShift.z();

相对于启动IMU状态，将点旋转回本地IMU系统 rotate point back to local IMU system relative to the start IMU state

  rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);

5、processScanlines 函数

    void processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans);

1. 重置内部缓冲区并根据当前扫描时间设置IMU启动状态

reset(scanTime);

初始化 _scanTime imuIdx =0,同时初始化IMU插入的位姿在扫描开始时清除内部云缓冲区

    interpolateIMUStateFor(0, _imuStart);

2.构建排序的全分辨率云

  size_t cloudSize = 0;
  for (int i = 0; i < laserCloudScans.size(); i++) {
    _laserCloud += laserCloudScans[i];

    IndexRange range(cloudSize, 0);
    cloudSize += laserCloudScans[i].size();
    range.second = cloudSize > 0 ? cloudSize - 1 : 0;
    _scanIndices.push_back(range);
  }

_scanIndices std::vector<IndexRange> typedef std::pair<size_t, size_t> IndexRange;

IndexRange 第一个参数为：每一线激光的开头的index ，第二个参数为：每一线激光完成的index

3.提取特征 extractFeatures

extractFeatures();

4.跟新IMU转化

imuTrans[0] x y z = imuStart pitch yaw roll

imuTrans[1] x y z = imuCur pitch yaw roll

imu位姿偏移 imuShiftFromStart

imuTrans[2] x y z = imuShiftFromStart pitch yaw roll

imuTrans[3] x y z = imuVelocityFromStart pitch yaw roll

6、extractFeatures 函数

1.从单个扫描中提取特征

  size_t nScans = _scanIndices.size();
  for (size_t i = beginIdx; i < nScans; i++) {

_scanIndices std::vector<IndexRange> typedef std::pair<size_t, size_t> IndexRange;

2.找出这帧数据在激光点云坐标的起始，并且跳过空的 scan 数据

    size_t scanStartIdx = _scanIndices[i].first;
    size_t scanEndIdx = _scanIndices[i].second;

    // skip empty scans
    if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) {
      continue;
    }

3.重新设定scan 的 buffers setScanBuffersFor函数

setScanBuffersFor(scanStartIdx, scanEndIdx);

遍历所有点（除去前五个和后六个），判断该点及其周边点是否可以作为特征点位：当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离），且两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点；若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平）。
获取激光一条线上的scan的数量

  size_t scanSize = endIdx - startIdx + 1;
  _scanNeighborPicked.assign(scanSize, 0);

将不可靠的点标记为已挑选

  for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) {

_config.curvatureRegion 为：用于计算点曲率的周围点数（点周围的+/-区域）

得到前一个点，当前点，后一个点

    const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]);
    const pcl::PointXYZI& point = (_laserCloud[i]);
    const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]);

计算下一个点与当前点的距离（ $\Delta x^{2} \right +\Delta y^{2} \right+\Delta z^{2}$ ）

 float diffNext = calcSquaredDiff(nextPoint, point);

如果该值大于0.1时，计算point和nextPoint到远点的距离值

    if (diffNext > 0.1) {
      float depth1 = calcPointDistance(point);
      float depth2 = calcPointDistance(nextPoint);

计算权重距离

      if (depth1 > depth2) {
        float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2;

        if (weighted_distance < 0.1) {
          std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1);

          continue;
        }
      } else {
        float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1;

        if (weighted_distance < 0.1) {
          std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1);
        }
      }

fill_n 将之间的数填充本文中填充的值为1

    float diffPrevious = calcSquaredDiff(point, previousPoint);
    float dis = calcSquaredPointDistance(point);

    if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) {
      _scanNeighborPicked[i - startIdx] = 1;
    }

4.从相同大小的扫描区域提取特征

** nFeatureRegions 6 用于在扫描中分布特征提取的（大小相等）区域的数量。*/

/ **curvatureRegion 5 用于计算点曲率的周围点数（点周围的+/-区域）。*/

将每个线等分为六段，分别进行处理（sp、ep分别为各段的起始和终止位置）

    for (int j = 0; j < _config.nFeatureRegions; j++) {
      size_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j)
                   + (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions;
      size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j)
                   + (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1;

跳过空白区域

      if (ep <= sp) {
        continue;
      }

5.为求特征区域设置区域缓冲区 setRegionBuffersFor(sp, ep);

获取其buffers 的尺寸

  size_t regionSize = endIdx - startIdx + 1;
  _regionCurvature.resize(regionSize);
  _regionSortIndices.resize(regionSize);
  _regionLabel.assign(regionSize, SURFACE_LESS_FLAT);

std::vector<float> _regionCurvature; ///< point curvature buffer 点曲率缓冲区
std::vector<PointLabel> _regionLabel; ///< point label buffer 点标签缓冲区
std::vector<size_t> _regionSortIndices; ///< sorted region indices based on point curvature基于点曲率的分类区域索引
std::vector<int> _scanNeighborPicked; ///< flag if neighboring point was already picked如果已经选择了相邻点，则标记

计算点曲率并重置排序指数

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

排序点曲率

  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]);
      }
    }
  }

6.提取角落特征

      for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) {
        size_t idx = _regionSortIndices[--k];
        size_t scanIdx = idx - scanStartIdx;
        size_t regionIdx = idx - sp;

        if (_scanNeighborPicked[scanIdx] == 0 &&
            _regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) {

          largestPickedNum++;
          if (largestPickedNum <= _config.maxCornerSharp) {
            _regionLabel[regionIdx] = CORNER_SHARP;
            _cornerPointsSharp.push_back(_laserCloud[idx]);
          } else {
            _regionLabel[regionIdx] = CORNER_LESS_SHARP;
          }
          _cornerPointsLessSharp.push_back(_laserCloud[idx]);

          markAsPicked(idx, scanIdx);
        }
      }

int maxCornerLessSharp 2 / **每个特征区域的最小锐角点数的最大数量。*/

std::vector<size_t> _regionSortIndices; 基于点曲率的分类区域索引

regionSize = ep - sp + 1;

scanIdx 为该线激光的第多少帧id

取出这部分当曲率大于阈值时：在最大标记范围内角点否则为：角落不太敏锐的点

将这些点设置为：标记为已选择

7.提取平面特征

      for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) {
        size_t idx = _regionSortIndices[k];
        size_t scanIdx = idx - scanStartIdx;
        size_t regionIdx = idx - sp;

        if (_scanNeighborPicked[scanIdx] == 0 &&
            _regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) {

          smallestPickedNum++;
          _regionLabel[regionIdx] = SURFACE_FLAT;
          _surfacePointsFlat.push_back(_laserCloud[idx]);

          markAsPicked(idx, scanIdx);
        }
      }

跟提取角点一样，首先找到曲率最小的idx，进而找到 scanIdx 为该线激光的第多少帧id

如果曲率小于阈值且平面特征点小于预定值则选出，然后将平面特征点存好

将这些点设置为：标记为已选择

8.提取较少的平坦表面特征

      for (int k = 0; k < regionSize; k++) {
        if (_regionLabel[k] <= SURFACE_LESS_FLAT) {
          surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);
        }
      }

9.降采样

   pcl::PointCloud<pcl::PointXYZI> surfPointsLessFlatScanDS;
    pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);
    downSizeFilter.filter(surfPointsLessFlatScanDS);