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

目录

一、注解:

二、代码流程:

2.1 主函数:main

2.2 MultiScanRegistration类的构造

2.3 类对象multiScan调用setup函数

2.3.1 参数配置 RegistrationParams

2.3.2 子类ScanRegistration调用其函数setupROS

2.3.3 激光参数确定

三、IMU回调函数  

四、点云数据 回调函数

4.1 激光运行的核心函数

4.2 projectPointToStartOfSweep  函数

五、激光处理函数

5.1. 点云中提取特征   extractFeatures 

5.2 两回调总结


一、注解:

  • 代码流程:订阅了2个节点和发布了6个节点。通过回调函数的处理,将处理后的点云重新发出去。
  • 功能:对点云和IMU数据进行预处理,用于特征点的配准。
  • 具体实现:一次扫描的点通过曲率值来分类,特征点曲率大于阈值的为边缘点;特征点曲率小于阈值的为平面点。为了使特征点均匀的分布在环境中,将一次扫描划分为4个独立的子区域。每个子区域最多提供2个边缘点和4个平面点。此外,将不稳定的特征点(瑕点)排除。

二、代码流程:

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

2.2 MultiScanRegistration类的构造

   loam::MultiScanRegistration  multiScan  对象

  •  MultiScanRegistration类 继承了ScanRegistration类,ScanRegistration类集成了BasicScanRegistration
  • 二者均为默认构造函数,不用管。
  MultiScanRegistration(const MultiScanMapper& scanMapper = MultiScanMapper());
  • 其中MultiScanMapper用于设置:激光垂直下限、激光垂直上限以及是多少线的激光,在初始化时MultiScanRegistration时可重新设置该参数。
  MultiScanMapper(const float& lowerBound = -15,  //垂直下限(度)
                  const float& upperBound = 15,  //垂直上限(度)
                  const uint16_t& nScanRings = 16);  //激光环的数量

参数:

  1. 默认设定了激光的扫描角度
  2. 第一扫描环的垂直角度  _lowerBound
  3. 最后一个扫描环的垂直角度  _upperBound
  4. 线性差值因子  _factor   =  (nScanRings - 1) / (upperBound - lowerBound)

同时,将velodyne的三种多线激光进行  static 设定

  /** Multi scan mapper for Velodyne VLP-16 according to data sheet. */
  static inline MultiScanMapper Velodyne_VLP_16() { return MultiScanMapper(-15, 15, 16); };

  /** Multi scan mapper for Velodyne HDL-32 according to data sheet. */
  static inline MultiScanMapper Velodyne_HDL_32() { return MultiScanMapper(-30.67f, 10.67f, 32); };

  /** Multi scan mapper for Velodyne HDL-64E according to data sheet. */
  static inline MultiScanMapper Velodyne_HDL_64E() { return MultiScanMapper(-24.9f, 2, 64); };

2.3 类对象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;
}

2.3.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);

参数变量有:

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

2.3.2 子类ScanRegistration调用其函数setupROS

!ScanRegistration::setupROS(node, privateNode, config_out)
  1. 该函数里面第一步,解析参数   
    1.  if (!parseParams(privateNode, config_out))
    2. 该函数是从launch 文件中读取参数 读取的参数为2.3.1中配置的参数
  2. 订阅IMU话题和发布话题 订阅IMU话题,发布点云等
    1. imu topic :  /imu/data
       
  // 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.3.3 激光参数确定

  1. 若有 "lidar" 这个参数,则代表velodyne,确定激光的类型VLP-16  HDL-32  HDL-64E,并且根据 static 设置的三种进行设置
    1. 同时设置激光 扫描时间  默认 0.1 
  2. 否则,设置 最小角、最大角以及 激光层数
  3. 订阅激光 topic   : multi_scan_points
  // subscribe to input cloud topic
  _subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>
      ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);

三、IMU回调函数  

ScanRegistration::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位置和速度
    1. 根据roll,pitch,yaw计算 出 世界坐标系下的加速度。
    2. 取到buff中上一帧IMU数据
    3. 推算出当前imu的最新状态,位姿+速度
    4. 放到 _imuHistory 队列中。
  • 位置:  P_{now} = P_{last} +V_{last} * \Delta_t + 0.5 a_{now} * \Delta t^2
  • 速度:V_{now} = V_{last} + a * \Delta_t
    // 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;

四、点云数据 回调函数

MultiScanRegistration::handleCloudMessage

  • 1.系统启动延迟计数器   int _systemDelay = 20;
    • systemDelay 有延时作用,保证有imu数据后在调用laserCloudHandler
    • 其实这样不太好,最好是等imu有数据时,在进行激光call
  if (_systemDelay > 0) 
  {
    --_systemDelay;
    return;
  }
  • 2.将 激光数据 转化成 pcl 点云 描述   
    •   pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
        pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
    • 在ROS中表示点云的数据结构有: sensor_msgs::PointCloud      sensor_msgs::PointCloud2 于PCL在ros的数据的结构 pcl::PointCloud<T>。具体的介绍可查 看  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. 开始核心函数: 
    • process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp));
  • 时间采用系统时间代替ros时间:Time = std::chrono::system_clock::time_point;  二者之间的相互转换
// ROS time adapters
inline Time fromROSTime(ros::Time const& rosTime)
{
  auto epoch = std::chrono::system_clock::time_point();
  auto since_epoch = std::chrono::seconds(rosTime.sec) + std::chrono::nanoseconds(rosTime.nsec);
  return epoch + since_epoch;
}

inline ros::Time toROSTime(Time const& time_point)
{
  return ros::Time().fromNSec(std::chrono::duration_cast<std::chrono::nanoseconds>(time_point.time_since_epoch()).count());
}

4.1 激光运行的核心函数

  • process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp));

  • 1. 确定 激光的 开始以及结束 角度
  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;
  }
  • 2.根据多少线激光重置 容器个数,并清除以前数据
  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(); }); 
  • 3.从输入点云中提取有效点,即遍历点云数据   for (int i = 0; i < cloudSize; i++) {
    • 注意:
      •     point.x = laserCloudIn[i].y;
            point.y = laserCloudIn[i].z;
            point.z = laserCloudIn[i].x;
    • 首先跳过   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;
            }
    • 然后计算垂直角度 angle确定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 ){
              continue;
            }
    • 计算水平点角度
      •     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;
              }
            }
        
    • 据点方向计算相对扫描时间 ,进而确定独特的 Id 放到 点云强度中: 正常ID + 比例  
      • ​​​​​​  float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
          point.intensity = scanID + relTime;
    • 使用相应的IMU数据投射到扫描开始的点    projectPointToStartOfSweep
    • 点云根据scanID有序放到容器中   //for循环结束
  • 4. 将新云处理为一组扫描线。 processScanlines
  • 5.发布结果:发布完整的分辨率点云和特征点云
    •   auto sweepStartTime = toROSTime(sweepStart());
        // publish full resolution and feature point clouds
        publishCloudMsg(_pubLaserCloud, laserCloud(), sweepStartTime, "/camera");
        publishCloudMsg(_pubCornerPointsSharp, cornerPointsSharp(), sweepStartTime, "/camera");
        publishCloudMsg(_pubCornerPointsLessSharp, cornerPointsLessSharp(), sweepStartTime, "/camera");
        publishCloudMsg(_pubSurfPointsFlat, surfacePointsFlat(), sweepStartTime, "/camera");
        publishCloudMsg(_pubSurfPointsLessFlat, surfacePointsLessFlat(), sweepStartTime, "/camera");
      
        // publish corresponding IMU transformation information
        publishCloudMsg(_pubImuTrans, imuTransform(), sweepStartTime, "/camera");

4.2 projectPointToStartOfSweep  函数

  • 有imu数据时才进行,否则直接略过

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

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

       interpolateIMUStateFor(const float &relTime, IMUState &outputState)

  1. 首先循环等待,直到imu时间超过该点时间,确定 _imuIdx
  2. 计算出该时间占两帧imu数据的比例。
  3. 进行线性差值,输出当前时间IMUState
  4. 根据两帧imu之间的比例,计算出 roll,pitch,yaw和 vel,position
  double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
  while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {
    _imuIdx++;
    timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
  }

  if (_imuIdx == 0 || timeDiff > 0) {
    outputState = _imuHistory[_imuIdx];
  } else {
    float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp);
    IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState);
  }
  • 计算IMU的累计偏移
  //最近激光的时间 - 开始扫描的时间 + delta_rel
  float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;
  //累积IMU位置和插值IMU位置之间的位置偏移
  _imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;

2.将该点转化到 startIMU上 

  • 旋转点到全局坐标系下   根据imu提供的旋转角roll,pitch,yaw
  rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);
  • 在该坐标系下补偿imu的偏移  
  point.x += _imuPositionShift.x();
  point.y += _imuPositionShift.y();
  point.z += _imuPositionShift.z();
  • 将该点反转换到实际坐标系下
  rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);

五、激光处理函数

processScanlines 

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

1.内部缓冲区 基于当前激光时间重置设置IMU启动状态  reset(scanTime); 

       得到当前时刻imu的状态并清除缓存数据

void BasicScanRegistration::reset(const Time& scanTime)
{
  _scanTime = scanTime;

  // re-initialize IMU start index and state
  _imuIdx = 0;
  if (hasIMUData()) {
    interpolateIMUStateFor(0, _imuStart);
  }

  // clear internal cloud buffers at the beginning of a sweep
  if (true/*newSweep*/) {
    _sweepStart = scanTime;

    // clear cloud buffers
    _laserCloud.clear();
    _cornerPointsSharp.clear();
    _cornerPointsLessSharp.clear();
    _surfacePointsFlat.clear();
    _surfacePointsLessFlat.clear();

    // clear scan indices vector
    _scanIndices.clear();
  }
}

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

   将多线点云搞成一组点云,并通过_scanIndices 记录每束激光在该组点云的下的开始和结尾index

  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

4.跟新IMU状态 updateIMUTransform

    imu位姿偏移  imuShiftFromStart

  • imuTrans[0]   x y z    =  imuStart pitch yaw roll
  • imuTrans[1]   x y z    =  imuCur pitch yaw roll
  • imuTrans[2]   x y z    =  imuShiftFromStart pitch yaw roll
  • imuTrans[3]   x y z    =  imuVelocityFromStart pitch yaw roll

5.1. 点云中提取特征   extractFeatures 

1. 遍历每一层激光    --从单个扫描中提取特征

  size_t nScans = _scanIndices.size();  //指有多少束激光
  for (size_t i = beginIdx; i < nScans; i++) {

2.找出这帧数据在激光点云坐标的起始,并且跳过 “计算曲率不够的点"  

[start+config.curvatureRegion,end -config.curvatureRegion]

    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函数  标记哪些点不可能作为特征点

  • 遍历所有点(除去前五个和后六个),判断该点及其周边点是否可以作为特征点位:当某点及其后点间的距离平方大于某阈值a(说明这两点有一定距离),且两向量夹角小于某阈值b时(夹角小就可能存在遮挡),将其一侧的临近6个点设为不可标记为特征点的点;
  • 若某点到其前后两点的距离均大于c倍的该点深度,则该点判定为不可标记特征点的点(入射角越小,点间距越大,即激光发射方向与投射到的平面越近似水平)。
setScanBuffersFor(scanStartIdx, scanEndIdx);
  1. 重置激光邻域选取  _scanNeighborPicked,获取激光一条线上的scan的数量
    1.   size_t scanSize = endIdx - startIdx + 1;
        _scanNeighborPicked.assign(scanSize, 0);
  2. 遍历可能成为特征的点的下标,后续操作都在该循环中,已将不可靠的点标记为已挑选, [start+config.curvatureRegion,end -config.curvatureRegion]
    1.   for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) {
      
      _config.curvatureRegion 为:用于计算点曲率的周围点数(点周围的+/-区域)
  3. 取出连续的3个点:前一个点,当前点,后一个点
    1. const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]);
      const pcl::PointXYZI& point = (_laserCloud[i]);
      const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]);
  4. 计算下一个点与当前点的距离  diffNext  = (\Delta x^{2} +\Delta y^{2}+\Delta z^{2}
    1.  float diffNext = calcSquaredDiff(nextPoint, point);
  5. 如果该值大于0.1时,则可能有差异, 计算point和nextPoint到原点的距离值 
    1.     if (diffNext > 0.1) {
            float depth1 = calcPointDistance(point);
            float depth2 = calcPointDistance(nextPoint);
    2. 根据 二者之间的距离差以及 权重 标记。 scanNeighborPicked表示一个点的周围不能再设置为特征点
      1.     距离权重的计算:红蓝线的比值,根据等腰三角形的性质,如果Xi与Xi+1的夹角小于阈值0.1即对应5.732°
      2.       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);
                
  6.  针对论文a情接着直接计算前一个点与后一个点与当前的距离,满足条件也设置相邻。
    1.   float diffPrevious = calcSquaredDiff(point, previousPoint);
          float dis = calcSquaredPointDistance(point);
       
          if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) {
            _scanNeighborPicked[i - startIdx] = 1;
          }

4.将扫描区域重新分配。即将每条线分成6段,计算每段点云的开头sp,结尾ep位置。

sp=s+(e-s)*j/6,ep=s+(e-s)/6+(e-s)*j/6-1

  1.  *nFeatureRegions 6  用于在扫描中分布特征提取的(大小相等)区域的数量。
  2. /curvatureRegion 5  用于计算点曲率的周围点数(点周围的+/-区域)。
  3. 将每个线等分为六段,分别进行处理(sp、ep分别为各段的起始和终止位置)
  4.     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;
  5. 跳过空白区域   跳过不符合条件的点
  6. if (ep <= sp) {
       continue;
    }
  7.  5.为求特征区域设置区域缓冲区  setRegionBuffersFor(sp, ep);
    1. 获取其buffers 的尺寸
    2.   size_t regionSize = endIdx - startIdx + 1;
        _regionCurvature.resize(regionSize);
        _regionSortIndices.resize(regionSize);
        _regionLabel.assign(regionSize, SURFACE_LESS_FLAT);
    3. std::vector<float> _regionCurvature;      ///< point curvature buffer  点曲率缓冲区
    4.     std::vector<PointLabel> _regionLabel;     ///< point label buffer  点标签缓冲区
    5.     std::vector<size_t> _regionSortIndices;   ///< sorted region indices based on point curvature基于点曲率的分类区域索引
    6.     std::vector<int> _scanNeighborPicked;     ///< 如果已经选择了相邻点,则标记
    7. 计算点曲率,取i点周围的10个点,计算距离,求出曲率半径,曲率=1/曲率半径
    8.   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;
        }
    9. 排序点曲率
    10.   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]);
            }
          }
        }
  8. 6.提取角特征  + point_less_sharp
    1. 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);
        }
      }
    2.  int  maxCornerLessSharp    2         / **每个特征区域的最小锐角点数的最大数量。*/
    3. std::vector<size_t> _regionSortIndices;    基于点曲率的分类区域索引
    4. regionSize = ep - sp + 1;    
    5. scanIdx  为该线激光的第多少帧id
    6. 取出这部分当曲率大于阈值时:    在最大标记范围内  角点 否则为: 角落不太敏锐的点
    7. 将这些点设置为:标记为已选择
  9. 7.提取平面特征 
    1.       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);
              }
            }
    2. 跟提取角点一样,首先找到曲率最小的idx,进而找到  scanIdx  为该线激光的第多少帧id
    3. 如果曲率小于阈值且平面特征点小于预定值   则 选出,然后将平面特征点存好
    4. 将这些点设置为:标记为已选择
    5. 提取较少的平坦表面特征 剩下归平面特征点备选
      1.       for (int k = 0; k < regionSize; k++) {
                if (_regionLabel[k] <= SURFACE_LESS_FLAT) {
                  surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);
                }
              }
  10. 8.降采样
  11.    pcl::PointCloud<pcl::PointXYZI> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);
        downSizeFilter.filter(surfPointsLessFlatScanDS);

5.2 两回调总结

  • 8
    点赞
  • 72
    收藏
    觉得还不错? 一键收藏
  • 7
    评论
评论 7
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值