(02)Cartographer源码无死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→点云的体素滤波

这篇具有很好参考价值的文章主要介绍了(02)Cartographer源码无死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→点云的体素滤波。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文 末 正 下 方 中 心 提 供 了 本 人 联 系 方 式 , 点 击 本 人 照 片 即 可 显 示 W X → 官 方 认 证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} WX
 

一、前言

首先这里对点云的体素滤波进行一个简单的介绍:体素滤波器是一种下采样的滤波器,它的作用是使用体素化方法减少点云数量,采用体素格中接近中心点的点替代体素内的所有点云,这种方法比直接使用中心点要慢,但是更加精确。这种方式即减少点云数据,并同时保存点云的形状特征,在提高配准,曲面重建,形状识别等算法速度中非常实用,来看下面两张效果图:
(02)Cartographer源码无死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→点云的体素滤波
(02)Cartographer源码无死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→点云的体素滤波
可以看到,经过体素滤波的点云更加稀疏,其原理也比较简单:把三维空间按照一定分辨率分割成一个个的小正方体(或者说长方体),每个小正方体中使用一个点云来表示,这样就起到下采样的效果。至于如何处理获得一个小正方体最具有代表性的点云,不同算法,实现的方式也是不一样的。另外,这里的小正方体,也称为体素,所以叫体素滤波。
 

二、滤波函数总览

首先说明一下,该篇博客主要接着上一篇博客进行讲解,也就是对src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 文件 中LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter 函数的如下代码进行细节分析:

  // Step: 6 对点云进行体素滤波
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
      sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};

也就是进行体素滤波,输入滤波的点云数据帧已经完成了时间同步、运动畸变校正,以及重力校正与Z轴过滤。不过需要注意的是,src/cartographer/cartographer/sensor/internal/voxel_filter.cc 文件 中定义了很多滤波函数,其中有些为模板函数,如下所示:

1.// 进行体素滤波
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution){......}

2.
TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,const float resolution) {......}

3.
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
	const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&range_measurements,
	const float resolution) {....}

为什么定义这么的滤波函数暂时不是很了解,可能是为了处理各种类型的点云(如带时间与不带时间),且需要返回不同的数据格式。下面要讲解的是其上的 1,如下:

PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {

该滤波输入与输出的数据格式都是都是 PointCloud 类型的实例,该类在上一篇博客中提及过,且对其成员函数:

  template <class UnaryPredicate>
  PointCloud copy_if(UnaryPredicate predicate) const {....}

进行了细节的分析。简单的可以把该类理解为,存储一帧点云数据的类,且该类还实现了点云的过滤成员函数→copy_if()。

另外还有一个参数 options_.voxel_filter_size() 表示滤波的体素大小,该变量在 src/cartographer/configuration_files/trajectory_builder_2d.lua 文件中设置,默认为 0.025,也就是每2.5cm为一个体素。

 

三、sensor::VoxelFilter() 总体介绍

/**
 * @brief 对点云数据进行体素滤波
 * 
 * @param[in] point_cloud 该类包含需要处理的所有点云数据
 * @param[in] resolution 滤波分辨率,即每个体素的体积大小
 * @return PointCloud 过滤之后的点云数据
 */
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
  
  //得到标记后的点,记录一个体素中,应该取那个点。(每个体素只取一个点进行代替)
  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      [](const RangefinderPoint& point) { return point.position; });

  // 根据标记,把对应的点云存储在 filtered_points 之中
  std::vector<RangefinderPoint> filtered_points;
  for (size_t i = 0; i < point_cloud.size(); i++) {
    if (points_used[i]) {
      filtered_points.push_back(point_cloud[i]);
    }
  }
  //根据滤波之后的点云标记,把点云对应的强度添加到 filtered_intensities 之中
  std::vector<float> filtered_intensities;
  CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size());
  for (size_t i = 0; i < point_cloud.intensities().size(); i++) {
    if (points_used[i]) {
      filtered_intensities.push_back(point_cloud.intensities()[i]);
    }
  }

  //通过std::move移交所有权,减少数据的拷贝
  return PointCloud(std::move(filtered_points),
                    std::move(filtered_intensities));
}

该函数主要分三个步骤对点云进行处理,这里主要分析第一个部分,即 RandomizedVoxelFilterIndices() 函数。如下所示:

  //得到标记后的点
  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      [](const RangefinderPoint& point) { return point.position; });

可见其传入了三个参数,①需要处理的点云帧数据;②体素滤波分辨率;③lambda表达式,该表示的功能为传入一个 RangefinderPoint 实例对象,其会返回 Eigen::Vector3f 类型的点云坐标。该函数具体实现于
src/cartographer/cartographer/sensor/internal/voxel_filter.cc 中的,代码如下:

// 进行体素滤波, 标记体素滤波后的点
template <class T, class PointFunction>
std::vector<bool> RandomizedVoxelFilterIndices(       
    const std::vector<T>& point_cloud, //需要进行处理的点云帧数据,  
     const float resolution, //体素滤波分辨率,即体素大小
    PointFunction&& point_function) { //获取点云坐标的函数指针
  // According to https://en.wikipedia.org/wiki/Reservoir_sampling
  std::minstd_rand0 generator; //定义一个随机数生成器

  //std::pair<int, int>> 第一个元素保存该voxel内部的点的个数, 
  //std::pair<int, int>> 第二个元素保存该voxel中选择的那一个点的序号
  //VoxelKeyType 就是 uint64_t 类型,可以理解为体素的索引
  absl::flat_hash_map<VoxelKeyType, std::pair<int, int>>
      voxel_count_and_point_index;

  // 遍历所有的点, 计算
  for (size_t i = 0; i < point_cloud.size(); i++) {
    //通过GetVoxelCellIndex()计算该点处于的voxel的序号
    //最终获取VoxelKeyType对应的value的引用
    auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex(
        point_function(point_cloud[i]), resolution)];

    voxel.first++;//该体素记录的点云数量+1

    // 如果这个体素格子只有1个点, 那这个体素格子里的点的索引就是i
    if (voxel.first == 1) {
      voxel.second = i; //这里的i为目前遍历点云数据的索引
    } 
    else {
      // 生成随机数的范围是 [1, voxel.first]
      std::uniform_int_distribution<> distribution(1, voxel.first);
      // 生成的随机数与个数相等, 就让这个点代表这个体素格子
      if (distribution(generator) == voxel.first) {
        voxel.second = i;
      }
    }
  }

  // 为体素滤波之后的点做标记
  std::vector<bool> points_used(point_cloud.size(), false);
  for (const auto& voxel_and_index : voxel_count_and_point_index) {
    points_used[voxel_and_index.second.second] = true;
  }
  return points_used;
}

注释比较详细,这里就不在重复了,其主要是返回变量:

std::vector<bool> points_used(point_cloud.size(), false);

可见其大小与输入的点云数目一致,默认设置为都为 false,其对应的点云如果被选中会标志为 true。该函数执行完成之后则会调用
 

四、sensor::VoxelFilter() 函数返回

其上完成体素滤波之后,这里回到之前的 LocalTrajectoryBuilder2D::AddRangeData() 函数,可见代码如下:

    return AddAccumulatedRangeData(
        time,
        // 对点云进行重力对齐,也就是让点云的Z轴与重力方向平行
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);

可知其又调用了 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数,该函数后面再进行详细的讲解,因为涉及到了点云的扫描匹配,但是其中还有调用滤波相关的代码,所以先对该部分内容进行讲解,如下所示:


  // Step: 7 对 returns点云 进行自适应体素滤波,返回的点云的数据类型是PointCloud
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());
  if (filtered_gravity_aligned_point_cloud.empty()) {
    return nullptr;
  }

首先其只对 gravity_aligned_range_data.returns 点云进行了自适应体素滤波,而 gravity_aligned_range_data.returns.misses 点云没有进行滤波,该函数定义如下所示:

// 进行自适应体素滤波
PointCloud AdaptiveVoxelFilter(
    const PointCloud& point_cloud,
    const proto::AdaptiveVoxelFilterOptions& options) {
  return AdaptivelyVoxelFiltered(
      // param: adaptive_voxel_filter.max_range 距远离原点超过max_range的点被移除
      // 这里的最大距离是相对于local坐标系原点的
      options, FilterByMaxRange(point_cloud, options.max_range()));
}

也就是重点还是 src/cartographer/cartographer/sensor/internal/voxel_filter.cc 中的 AdaptivelyVoxelFiltered() 函数。注意其上的 options 包含如下参数:

  -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
  -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
  adaptive_voxel_filter = {
    max_length = 0.5,             -- 尝试确定最佳的立方体边长, 边长最大为0.5
    min_num_points = 200,         -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数
    max_range = 50.,              -- 距远离local坐标系原点超过 max_range 的点被移除
  },

另外还有一个比较简单的函数 FilterByMaxRange() , 距远离 local坐标系原点 超过max_range 的点云被移除。这里需要注意的,其是相对云 local坐标系原点,与前面分析的代码 LocalTrajectoryBuilder2D::AddRangeData() 函数中的 options_.min_range() 与 options_.mix_range() 是不一样的,如下:

    // param: min_range max_range
    if (range >= options_.min_range()) {//如果该点云大于等于最近测量距离
      if (range <= options_.max_range()) {//如果该点小于等于最远测量距离

  //默认配置
  min_range = 0.,                 -- 雷达数据的最近滤波, 保存中间值
  max_range = 30.,                -- 雷达数据的最远滤波, 保存中间值

在 LocalTrajectoryBuilder2D::AddRangeData() 函数中过滤的是相对于雷达原点的距离,默认 max_range = 30。AdaptivelyVoxelFiltered() 是相对于 local坐标系原点的距离 max_range = 30,通常情况下后者的设置都比前者大。max_range 过滤之后,随后就是正式进入到 AdaptivelyVoxelFiltered() 函数了。
 

五、AdaptivelyVoxelFiltered()

该函数还是一样,位于 src/cartographer/cartographer/sensor/internal/voxel_filter.cc 文件中,再复制一下 adaptive_voxel_filter 配置信息:

  -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
  -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
  adaptive_voxel_filter = {
    max_length = 0.5,             -- 尝试确定最佳的立方体边长, 边长最大为0.5
    min_num_points = 200,         -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数
    max_range = 50.,              -- 距远离local坐标系原点超过 max_range 的点被移除
  },

( 01 ) \color{blue}(01) (01) 判断需要进行自适应体素滤波的帧点云数据,是否已经比较稀疏。如果 point_cloud.size() <= options.min_num_points() 表示目前点云足够稀疏不需要再进行滤波了。直接返回 point_cloud 即可。

( 03 ) \color{blue}(03) (03) 如果目前点云的非常稠密,尝试使用把体素的边长设置为 max_length = 0.5 进行滤波。滤波之后依旧不够稀疏,那么没有办法,说明已经是 max_length = 0.5 参数下最稀疏的状态了, 直接返回。

( 04 ) \color{blue}(04) (04) 如果目前的点云不是非常稠密(如果非常稠密第二步就返回了),那么就使用二分法,找到最合适的体素边长进行滤波。简单的说,就是每次体素边长缩小到原来的1/2 ,进行滤波,如果滤波之后的点云多了,则表示需要增加体素边长,如果少了,则减少体素的边长。

( 05 ) \color{blue}(05) (05) 源码中首先使用一个 for 循环,对体素边长进行控制,每次循环都会把体素边长缩小为上一次的1/2,如果边长低于 1e-2f * options.max_length() 则跳出 for 循环。但是注意,如果 for 循环执行完了,说明其一直不满足条件 result.size() >= options.min_num_points(),也就时点云一直比较稀疏,最终使用体素边长为 1e-2f * options.max_length() 的滤波。

( 06 ) \color{blue}(06) (06) 如果for循环的过程中,发现点云比较稠密了,即满足 options.min_num_points() 条件。则进入到 while 循环进行精细求解,该求解建立在 for循环的基础上,即在来自于 for 循环获得的 low_length 与 high_length 之间找到最合适的体素边长。

结 果 \color{blue}结果 经过 for 循环与 while 的最终结果,是希望点云数目保持在 options.min_num_points()=200 最优。代码注释如下:

// 自适应体素滤波
PointCloud AdaptivelyVoxelFiltered(
    const proto::AdaptiveVoxelFilterOptions& options,
    const PointCloud& point_cloud) {
  // param: adaptive_voxel_filter.min_num_points 满足小于等于这个值的点云满足要求, 足够稀疏
  //也就是说,该帧数据的点云已经比较稀疏了,没有必要再进行滤波了
  if (point_cloud.size() <= options.min_num_points()) {
    // 'point_cloud' is already sparse enough.
    return point_cloud;
  }

  // param: adaptive_voxel_filter.max_length 进行一次体素滤波
  PointCloud result = VoxelFilter(point_cloud, options.max_length());
  // 如果按照最大边长进行体素滤波之后还超过这个数了, 就说明已经是这个参数下最稀疏的状态了, 直接返回
  if (result.size() >= options.min_num_points()) {
    // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
    return result;
  }

  // Search for a 'low_length' that is known to result in a sufficiently
  // dense point cloud. We give up and use the full 'point_cloud' if reducing
  // the edge length by a factor of 1e-2 is not enough.
  // 将体素滤波的边长从max_length逐渐减小, 每次除以2
  for (float high_length = options.max_length();
       high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
    // 减小边长再次进行体素滤波
    float low_length = high_length / 2.f;
    result = VoxelFilter(point_cloud, low_length);
    
    // 重复for循环直到 滤波后的点数多于min_num_points
    if (result.size() >= options.min_num_points()) {
      // Binary search to find the right amount of filtering. 'low_length' gave
      // a sufficiently dense 'result', 'high_length' did not. We stop when the
      // edge length is at most 10% off.
      // 以二分查找的方式找到合适的过滤边长, 当边缘长度最多减少 10% 时停止
      while ((high_length - low_length) / low_length > 1e-1f) {
        const float mid_length = (low_length + high_length) / 2.f;
        const PointCloud candidate = VoxelFilter(point_cloud, mid_length);
        // 如果点数多, 就将边长变大, 让low_length变大
        if (candidate.size() >= options.min_num_points()) {
          low_length = mid_length;
          result = candidate;
        } 
        // 如果点数少, 就将边长变小, 让high_length变小
        else {
          high_length = mid_length;
        }
      }
      return result;
    }
  }
  return result;
}

 

六、结语

该篇博客主要讲解了点云体素滤波的知识,但是讲解代码过程中跳跃得比较多,可能导致不太好理解。不过没有关系,下一篇博客我们来做一个复盘。这样就不会觉得比较杂乱了。

 
 
 文章来源地址https://www.toymoban.com/news/detail-490322.html

到了这里,关于(02)Cartographer源码无死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→点云的体素滤波的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包