地面点云提取:Autoware预处理ray_ground_filter节点解析 + 解决ray_ground_filter无输出的问题

news2024/10/7 20:29:14

文章目录

  • 一、解决Autoware的ray_ground_filter节点无点云输出的问题
  • 二、ray_ground_filter节点代码分析
    • 2.1.监听bask_link和velodyne之间的TF
    • 2.2 裁切过高点云
    • 2.3 消除雷达近身反射点的影响
    • 2.4 角度和距离微分(转换到柱坐标)
    • 2.5 地面判断(核心部分)


一、解决Autoware的ray_ground_filter节点无点云输出的问题

autoware/core_perception/points_preprocessor/nodes/ray_ground_filter的包ray_ground_filter存在无点云输出的问题,下面解决它并具体分析一下ray_ground_filter的代码,参考:https://adamshan.blog.csdn.net/article/details/82901295#t6
(1)首先通过终端报错发现是tf的问题:

[ERROR] [1689559482.301474720]: Failed transform from base_link to velodyne
[ WARN] [1689559483.304340852]: Lookup would require extrapolation into the past.  Requested time 1671007347.691825151 but the earliest data is at time 1689559474.270626140, when looking up transform from frame [velodyne] to frame [base_link]

报错的源码在这句:

transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,in_cloud_ptr->header.stamp, ros::Duration(1.0));

它用的是in_cloud_ptr->header.stamp,就是说它寻找的tf关系是在我们接收输入点云的时刻比如时间为100的时刻的tf关系,但有时当我们寻找时间为100时的tf关系时已经找不到了,系统存储的tf关系已经被更新了,存储的最早的可能就只有103时刻的了,这时候就会报错或者抛出异常,对于transformPointCloud而言就是返回false,虽然合乎情理,但是我就想要他的tf变换,不是100时刻的也行,105时刻的也行,这时候我们就要用ros::Time(0)获取当前时刻tf,这样不会再找不到吧,ros::Duration(5.0)也可以设大点,让它能找到更多的时间范围。

transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,ros::Time(0), ros::Duration(5.0));

成功解决:
在这里插入图片描述

另外注意调整其以下几个参数(下一节会具体描述):
clipping_height:去除高于底盘1.2米的点,原算法是去除高于雷达0.2米的点,但是Autoware利用TF将点云转换到base_link坐标系(为了省去代码中的雷达高度参数),因此这里是1.2(加上雷达高度1m);
min_point_distance:去除雷达2米范围内的点云;
radial_divider_angle:将雷达点云坐标转换到柱坐标,并对360度进行微分,每一份的角度为0.18度
concentric_divider_distanc:上面的角度微分一份近似的可以看作一条射线,在射线上根据水平距离再进行微分
general_max_slopelocal_max_slope:全局和局部的最大坡度(角度)
min_point_distance:一条射线上两个相邻点的最小高度差
reclass_distance_threshold:重新分类时两个点的最小距离阈值
在这里插入图片描述

二、ray_ground_filter节点代码分析

过滤地面是激光雷达感知中一步基础的预处理操作,因为我们环境感知通常只对路面上的障碍物感兴趣,且地面的点对于障碍物聚类容易产生影响,所以在做Lidar Obstacle Detection(障碍物探测)之前通常将地面点和非地面点进行分离。

2.1.监听bask_link和velodyne之间的TF

作用是利用TF将点云转换到bask_link坐标系,这样就不需要原算法中的车身高度:

bool RayGroundFilter::TransformPointCloud(const std::string &in_target_frame,
                                          const sensor_msgs::PointCloud2::ConstPtr &in_cloud_ptr,
                                          const sensor_msgs::PointCloud2::Ptr &out_cloud_ptr)
{
  if (in_target_frame == in_cloud_ptr->header.frame_id)
  {
    *out_cloud_ptr = *in_cloud_ptr;
    return true;
  }

  geometry_msgs::TransformStamped transform_stamped;
  try
  {
    // 监听开始了,但是坐标之间的变换关系还未处理完毕,在此期间访问tf便会出现不存在的报错
    // 将函数的超时参数设定为5s,即最多有5s的时间让listener处理坐标关系
    transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
                                                   ros::Time(0), ros::Duration(5.0));
    // transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
    //                                                in_cloud_ptr->header.stamp, ros::Duration(5.0));
  }
  catch (tf2::TransformException &ex)
  {
    ROS_WARN("%s", ex.what());
    return false;
  }
  // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
  Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
  pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr);
  out_cloud_ptr->header.frame_id = in_target_frame;
  return true;
}

2.2 裁切过高点云

在分割地面之前,可以滤除掉过高的非地面点,以减少点云的数量,提升整体的处理速率,而且过高的障碍物对于小车来说并不算障碍物。具体的裁切高度由clipping_height指定。在这里面我加了一句代码去掉NAN值,如果有NAN值可能造成错误:

void RayGroundFilter::ClipCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, const double in_clip_height,
                                pcl::PointCloud<pcl::PointXYZI>::Ptr out_clipped_cloud_ptr)
{
  pcl::ExtractIndices<pcl::PointXYZI> extractor;
  extractor.setInputCloud(in_cloud_ptr);
  pcl::PointIndices indices;
// #pragma omp for语法OpenMP的并行化语法,即希望通过OpenMP并行化执行这条语句后的for循环,从而起到加速的效果。
#pragma omp for
  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    // 添加判断,去除空点
    if (in_cloud_ptr->points[i].z > in_clip_height || isnan(in_cloud_ptr->points[i].x) || isnan(in_cloud_ptr->points[i].y) || isnan(in_cloud_ptr->points[i].z))
    {
      indices.indices.push_back(i);
    }
  }
  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  extractor.setNegative(true); // true removes the indices, false leaves only the indices
  extractor.filter(*out_clipped_cloud_ptr);
}

2.3 消除雷达近身反射点的影响

具体指标由min_point_distance参数指定。去除过近处区域雷达稀疏点云,避免车身点云的影响:

void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, double in_min_distance,
                                       pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr)
{
  pcl::ExtractIndices<pcl::PointXYZI> extractor;
  extractor.setInputCloud(in_cloud_ptr);
  pcl::PointIndices indices;

#pragma omp for
  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    if (sqrt(in_cloud_ptr->points[i].x * in_cloud_ptr->points[i].x +
             in_cloud_ptr->points[i].y * in_cloud_ptr->points[i].y) < in_min_distance)
    {
      indices.indices.push_back(i);
    }
  }
  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  extractor.setNegative(true); // true removes the indices, false leaves only the indices
  extractor.filter(*out_filtered_cloud_ptr);
}

2.4 角度和距离微分(转换到柱坐标)

Ray Ground Filter算法的核心是以射线(Ray)的形式来组织点云。我们现在将点云的 (x, y, z)三维空间降到(x,y)平面来看,计算每一个点到车辆x正方向的平面夹角 θ, 我们对360度进行微分,分成若干等份,每一份的角度为0.18度,这个微分的等份近似的可以看作一条射线,每条射线上的点又根据水平距离(点到lidar的水平距离,半径)0.001再进行微分。
在这里插入图片描述

为了方便地对点进行半径和夹角的表示,作者自定义了数据结构PointXYZIRTColor来代替pcl::PointCloudXYZI:

struct PointXYZIRTColor
{
    pcl::PointXYZI point;
 
    float radius; //xy平面与雷达的欧氏距离
    float theta;  //xy的角度微分
 
    size_t radial_div;     //线圈的索引
    size_t concentric_div; //扫描线的索引
 
    size_t original_index; //与源雷达点云对应的索引
};

用radial_div和concentric_div分别描述角度微分和距离微分。对点云进行水平角度微分之后,可得到:360/0.18 = 2000条射线,将这些射线中的点按照距离的远近进行排序:

void RayGroundFilter::ConvertXYZIToRTZColor(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
    const std::shared_ptr<PointCloudXYZIRTColor> &out_organized_points,
    const std::shared_ptr<std::vector<pcl::PointIndices>> &out_radial_divided_indices,
    const std::shared_ptr<std::vector<PointCloudXYZIRTColor>> &out_radial_ordered_clouds)
{
  out_organized_points->resize(in_cloud->points.size());
  out_radial_divided_indices->clear();
  out_radial_divided_indices->resize(radial_dividers_num_);
  out_radial_ordered_clouds->resize(radial_dividers_num_);

  // 遍历当前帧的所有点
  for (size_t i = 0; i < in_cloud->points.size(); i++)
  {
    PointXYZIRTColor new_point;
    // 半径和方位角
    auto radius = static_cast<float>(
        sqrt(in_cloud->points[i].x * in_cloud->points[i].x + in_cloud->points[i].y * in_cloud->points[i].y));
    auto theta = static_cast<float>(atan2(in_cloud->points[i].y, in_cloud->points[i].x) * 180 / M_PI);
    if (theta < 0)
    {
      theta += 360;
    }
    if (theta >= 360)
    {
      theta -= 360;
    }
    // 角度的微分
    auto radial_div = (size_t)floor(theta / radial_divider_angle_);
    // 半径的微分
    auto concentric_div = (size_t)floor(fabs(radius / concentric_divider_distance_));

    new_point.point = in_cloud->points[i];
    new_point.radius = radius;
    new_point.theta = theta;
    new_point.radial_div = radial_div;
    new_point.concentric_div = concentric_div;
    new_point.red = (size_t)colors_[new_point.radial_div % color_num_].val[0];
    new_point.green = (size_t)colors_[new_point.radial_div % color_num_].val[1];
    new_point.blue = (size_t)colors_[new_point.radial_div % color_num_].val[2];
    new_point.original_index = i;

    out_organized_points->at(i) = new_point;

    // radial divisions更加角度的微分组织射线
    out_radial_divided_indices->at(radial_div).indices.push_back(i);
    // 每个角度/射线上包含的所有点云
    out_radial_ordered_clouds->at(radial_div).push_back(new_point);
  } // end for

// order radial points on each division
#pragma omp for
  for (size_t i = 0; i < radial_dividers_num_; i++)
  {
    // 将同一根射线上的点按照半径(距离)排序
    std::sort(out_radial_ordered_clouds->at(i).begin(), out_radial_ordered_clouds->at(i).end(),
              [](const PointXYZIRTColor &a, const PointXYZIRTColor &b)
              { return a.radius < b.radius; }); // NOLINT
  }
}

2.5 地面判断(核心部分)

通过判断射线中前后两点的坡度是否大于我们事先设定的坡度阈值,从而判断点是否为地面点:
在这里插入图片描述

void RayGroundFilter::ClassifyPointCloud(const std::vector<PointCloudXYZIRTColor> &in_radial_ordered_clouds,
                                         const pcl::PointIndices::Ptr &out_ground_indices,
                                         const pcl::PointIndices::Ptr &out_no_ground_indices)
{
  out_ground_indices->indices.clear();
  out_no_ground_indices->indices.clear();
#pragma omp for
  for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) // sweep through each radial division
  {
    // 上一个点的半径以及高度
    float prev_radius = 0.f;
    float prev_height = 0.f;
    // 上一个以及当前点是否为地面点
    bool prev_ground = false;
    bool current_ground = false;
    // 遍历一条射线上的每一个点(上面已经按从近到远排序)
    for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) // loop through each point in the radial div
    {
      // 与上一个点水平距离(第一个点是与雷达/base_link的距离)
      float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius;
      // 通过这两个坡度阈值以及当前点的半径(到lidar的水平距离)求得高度阈值,
      // 通过判断当前点的高度(即点的z值)是否在地面加减高度阈值范围内来判断当前点是为地面。
      float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;
      float current_height = in_radial_ordered_clouds[i][j].point.z;
      float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius;

      // for points which are very close causing the height threshold to be tiny, set a minimum value
      // 对于前后两点水平距离较近导致高度阈值较小的点,设置一个阈值最小值
      if (points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_)
      {
        height_threshold = min_height_threshold_;
      }

      // check current point height against the LOCAL threshold (previous point)
      // 在上一个点的高度+/-算出来的局部高度差的高度范围内
      if (current_height <= (prev_height + height_threshold) && current_height >= (prev_height - height_threshold))
      {
        // Check again using general geometry (radius from origin) if previous points wasn't ground
        if (!prev_ground)
        {
            // 在雷达所在地面的高度+/-算出来的全局高度差的高度范围内
          if (current_height <= general_height_threshold && current_height >= -general_height_threshold)
          {
            current_ground = true;
          }
          else
          {
            current_ground = false;
          }
        }
        else
        {
            // 上一个是地面点,当前直接也是
          current_ground = true;
        }
      }
      else
      {
        // check if previous point is too far from previous one, if so classify again
        // 检查是否与前一个点距离太远,若是则再次分类
        if (points_distance > reclass_distance_threshold_ &&
            (current_height <= height_threshold && current_height >= -height_threshold))
        {
          current_ground = true;
        }
        else
        {
          current_ground = false;
        }
      }

      if (current_ground)
      {
           // 记录在原始点云中的索引
        out_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
        prev_ground = true;
      }
      else
      {
        out_no_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
        prev_ground = false;
      }

      prev_radius = in_radial_ordered_clouds[i][j].radius;
      prev_height = in_radial_ordered_clouds[i][j].point.z;
    }
  }
}

这里有两个重要参数,一个是local_max_slope_,是我们设定的同条射线上邻近两点的坡度阈值,一个是general_max_slope_,表示整个地面的坡度阈值,这两个坡度阈值的单位为度(degree),我们通过这两个坡度阈值以及当前点的半径(到lidar的水平距离)求得高度阈值,通过判断当前点的高度(即点的z值)是否在地面加减高度阈值范围内来判断当前点是为地面。效果如下图所示(白色的是地面点,红色的是障碍物):
在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/767535.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

TextView 必填项pro版

优点 基本解决对齐方式,可以设置前缀隐藏和显示 /*** https://blog.csdn.net/u013982652/article/details/94404711* Android自定义TextView实现必填项前面的*号* 另一种实现方式(推荐使用这种,有非必填情况的话不会有对齐问题)* <p>* <cn.mvp.mlibs.weight.MiRequire…

2023年网络安全面试题:详细答案解析与最佳实践分享

如果在数据来源和网络分享方面存在侵权问题&#xff0c;请立即联系我以删除相关内容。 一句话木马 【基本原理】 利用文件上传漏洞&#xff0c;往目标网站中上传一句话木马&#xff0c;然后可以通过中国菜刀chopper.exe来获取和控制整个网站目录。表示后面即使执行错误&…

ZooKeeper原理剖析

1.ZooKeeper简介 ZooKeeper是一个分布式、高可用性的协调服务。在大数据产品中主要提供两个功能&#xff1a; 帮助系统避免单点故障&#xff0c;建立可靠的应用程序。提供分布式协作服务和维护配置信息。 2.ZooKeeper结构 ZooKeeper集群中的节点分为三种角色&#xff1a;Le…

curl: (56) Recv failure : Connection reset by peer

文章目录 背景原因可能如下1. 服务器端关闭了连接2. 网络问题3. 防火墙或代理问题4. 服务器负载过高 解决办法 背景 docker容器里有http服务&#xff0c;今天在docker容器重启时&#xff0c;去调用http接口&#xff0c;出现了以下错误&#xff1a; curl: (56) Recv failure :…

Explain执行计划中各个字段的含义

Explain执行计划中各个字段的含义 1、Explain有什么用2、Explain有哪些信息3、Explain执行计划详解 1、Explain有什么用 当Explain 与 SQL语句一起使用时&#xff0c;MySQL 会显示来自优化器关于SQL执行的信息。也就是说&#xff0c;MySQL解释了它将如何处理该语句&#xff0c…

Docker 架构解析:多角度解析 Docker 引擎与容器运行时

&#x1f337;&#x1f341; 博主 libin9iOak带您 Go to New World.✨&#x1f341; &#x1f984; 个人主页——libin9iOak的博客&#x1f390; &#x1f433; 《面试题大全》 文章图文并茂&#x1f995;生动形象&#x1f996;简单易学&#xff01;欢迎大家来踩踩~&#x1f33…

matlab重名函数调用踩坑记录

我新安装了matlab的robotics toolbox&#xff0c;然而调用的rotx不是我想要的函数。 我上网查了一下资料&#xff0c;知乎和csdn有相关的回答&#xff0c;但是我试了一下还是不行。它们的方法是移除路径再添加路径避免函数的冲突。相关方法放在文末的相关参考1 2。这里建议先用…

疯踏java知识点-进阶精讲篇

该资源是关于Java的师生管理系统&#xff0c;可以学习借鉴一下。 继续进行讲解&#xff0c;如果前面有不懂的&#xff0c;可以翻阅一下同专栏的其他文章&#xff0c;该专栏是针对Java的知识从0开始。 JavaBean 一个Java中的类&#xff0c;其对象可用于程序中封装数据举例&…

【C++】STL——vector的使用、 vector增删查改函数的介绍和使用、push_back和pop_back、operator[]

文章目录 1.vector的使用2.vector的增删查改&#xff08;1&#xff09;push_back 尾插&#xff08;2&#xff09;pop_back 尾删&#xff08;3&#xff09;find 查找&#xff08;4&#xff09;insert 在position之前插入val &#xff08;5&#xff09;erase 删除指定位置的数据&…

EXCEl——单元格移除换行

方法一&#xff1a;使用清除格式功能 步骤如下: 1.选中需要取消换行的单元格 2.在“开始"选项卡中找到"清除”功能&#xff0c;点击下拉菜单中的“清除格式" 3.这时单元格的换行就被取消了。 清除前效果图 清除后效果图 方法一&#xff1a;使用函数功能 步骤…

Camtasia Studio 2023保存为mp4格式的视频的详细教程,Camtasia的视频导出功能

很多用户刚接触Camtasia Studio&#xff0c;不熟悉如何保存mp4格式的视频。在今天的文章中小编为大家带来了Camtasia Studio 2023保存为mp4格式的视频的详细教程介绍。 1、 打开Camtasia Studio。 Camtasia Studio- 2023 win&#xff1a; https://souurl.cn/1JFEsn Camtasia …

HACKATHONCTF_1靶场详解

HACKATHONCTF_1靶场复盘 这个靶场有点CTF感觉&#xff0c;一步一步的没有什么难度。 下载地址&#xff1a;https://download.vulnhub.com/hackathonctf/ctf.zip 扫到ip后对ip进行一个单独扫描&#xff0c;发现开了四个端口&#xff0c;ssh设置到7223上了&#xff0c;这个很重…

SPP、SPPF 、 SimSPPF 、 ASPP、 SPPCSPC详解

分享自&#xff1a;https://blog.csdn.net/weixin_43694096/article/details/126354660 1. 原理 1.1 SPP&#xff08;Spatial Pyramid Pooling&#xff09; SPP 模块是何凯大神在2015年的论文《Spatial Pyramid Pooling in Deep Convolution Networks for Visual Recognitio…

前端工程化第一章:webpack基础(上)

文章目录 1. 什么是webpack&#xff1f;2. webpack使用2.2. 前置知识2.1. 创建一个项目 3. webpack打包3.1. 创建一个webpack.config.js文件3.2. 入口&#xff08;entry&#xff09;3.2.1. webpack.config.js3.2.2. src/index.js3.2.3. package.json 3.3. 输出&#xff08;outp…

C++第三讲

思维导图 手动封装一个顺序栈类&#xff08;数据元素为整形&#xff09;&#xff0c;要求私有成员属性&#xff1a;堆区空间的指针&#xff0c;用于存放数据&#xff0c;和一个指向栈顶元素的变量 /* ---------------------------------author&#xff1a;YoungZorncreated on…

nacos集群地址配置问题

#it is ip #example 127.0.0.1:8848 127.0.0.1:8849 127.0.0.1:8850 上面的配置可能存在配置失败的情况,可以采用下面的配置 127.0.0.1:8845 127.0.0.1.8846 127.0.0.1.8847 该配置在cluster.conf文件里面

还在使用冒泡排序遍历数组?No No No 库函数qsort帮你搞定所有排序还不快学起来!

&#x1f3ac; 鸽芷咕&#xff1a;个人主页 &#x1f525; 个人专栏:《快速入门C语言》《C语言初阶篇》 ⛺️生活的理想&#xff0c;就是为了理想的生活! 文章目录 前言&#x1f4ac; 库函数qsort的介绍&#x1f4ac; 库函数qsort的参数介绍&#x1f4ad; 参数一 (void* base)…

Acwing.908 最大不相交区间数量(贪心)

题目 给定N个闭区间[ai,bi]&#xff0c;请你在数轴上选择若干区间&#xff0c;使得选中的区间之间互不相交&#xff08;包括端点)。输出可选取区间的最大数量。 输入格式 第一行包含整数N&#xff0c;表示区间数。 接下来N行&#xff0c;每行包含两个整数ai , bi&#xff0c…

【基于 GitLab 的 CI/CD 实践】03、GitLab Pipeline 实践(上)

目录 一、GitLab Pipeline 流水线语法有哪些&#xff1f;流水线参数列表 如何检查语法错误&#xff1f;流水线语法检测 二、Pipeline 基础语法 job script before_script after_script stages 未定义 stages ​定义 stages 控制 stage 运行顺序 .pre & .post …

2010年中国生态系统服务空间数据集

摘要 生态系统服务是生态系统形成并维持的人类赖以生存和发展的环境条件与效用&#xff0c;是测度自然生态系统保护价值的重要指标。采用科学方法模拟生态系统服务的空间分布对掌握当前我国生态本底状况&#xff0c;识别生态保护重要区&#xff0c;从而有效支持生态管理决策具…