autoware.universe源码略读(3.17)--perception:occupancy_grid_map_outlier_filter

autoware.universe源码略读3.17--perception:occupancy_grid_map_outlier_filter

  • Overview
  • (Class)RadiusSearch2dfilter
    • (Class Constructor)RadiusSearch2dfilter::RadiusSearch2dfilter
    • (mFunc)RadiusSearch2dfilter::filter
  • (Class)OccupancyGridMapOutlierFilterComponent
    • (Class Constructor)OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent
    • (mFunc)OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2
    • (mFunc)OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap
    • (Func)getCost

Overview

这个模块是根据占据栅格地图occupancy_grid_map来对点云进行滤波的,根据占用网格图的实现方式,它也可以被称为时间序列中的离群值滤波器,因为占用网格图表达的是时间序列中的占用概率。 关于这里涉及到的方法,在官方文档中有简单的介绍occupancy_grid_map_outlier_filter,甚至附上了两个YouTube上的视频,简单来说就是把点云根据occupancy grid map分成了两部分 (low occupancy probability和high occupancy probability) ,这里还有一点就是不是所有的low occupancy probability都是outliers,还会根据距离信息多一步判断

在这里插入图片描述

(Class)RadiusSearch2dfilter

看名字这个是利用半径进行2D滤波的,应该涉及到了前面提到的针对low occupancy probability的情况

(Class Constructor)RadiusSearch2dfilter::RadiusSearch2dfilter

这里面涉及到了几个参数的加载

NameTypeDescription
radius_search_2d_filter/search_radiusfloatRadius when calculating the density
radius_search_2d_filter/min_points_and_distance_ratiofloatThreshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink.
radius_search_2d_filter/min_pointsintMinimum number of point clouds per radius
radius_search_2d_filter/max_pointsintMaximum number of point clouds per radius

然后还实例化了一个很常用的PCL的KD树对象,应该在滤波的时候会用到

(mFunc)RadiusSearch2dfilter::filter

这个函数分成了两种参数输入方式,第一种的话输入包括了输入的点云,一个位置;然后输出的就是满足条件的点和离散点。具体的话就是找到给定位置Pose的距离满足要求的点

kd_tree_->setInputCloud(xy_cloud);
for (size_t i = 0; i < xy_cloud->points.size(); ++i) {const float distance =std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y);const int min_points_threshold = std::min(std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),max_points_);const int points_num =kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists, min_points_threshold);if (min_points_threshold <= points_num) {output.points.push_back(xyz_cloud.points.at(i));} else {outlier.points.push_back(xyz_cloud.points.at(i));}
}

第二种和第一种的方法是一样的,不过这里是直接输入了high_conf_input和low_conf_input两个点云,然后相当于给一起处理了

xy_cloud->points.resize(low_conf_xyz_cloud.points.size() + high_conf_xyz_cloud.points.size());
for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) {xy_cloud->points[i].x = low_conf_xyz_cloud.points[i].x;xy_cloud->points[i].y = low_conf_xyz_cloud.points[i].y;
}
for (size_t i = low_conf_xyz_cloud.points.size(); i < xy_cloud->points.size(); ++i) {xy_cloud->points[i].x = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].x;xy_cloud->points[i].y = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].y;
}

(Class)OccupancyGridMapOutlierFilterComponent

本模块对应的主要节点函数

(Class Constructor)OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent

构造函数里加载了另一部分参数

NameTypeDescription
map_framestringmap frame id
base_link_framestringbase link frame id
cost_thresholdintCost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle.
enable_debuggerboolWhether to output the point cloud for debugging.
use_radius_search_2d_filterboolWhether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map.

然后这里因为输入的点云必须和栅格地图匹配上执行后边的步骤才有意义,所以这里也是使用到了之前用到过的那种同步技巧

pointcloud_sub_.subscribe(this, "~/input/pointcloud", rmw_qos_profile_sensor_data);
occupancy_grid_map_sub_.subscribe(this, "~/input/occupancy_grid_map", rclcpp::QoS{1}.get_rmw_qos_profile());
sync_ptr_ = std::make_shared<Sync>(SyncPolicy(5), occupancy_grid_map_sub_, pointcloud_sub_
sync_ptr_->registerCallback(std::bind(&OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2, this,std::placeholders::_1, std::placeholders::_2));

(mFunc)OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2

这里滤波的过程是通过调用filterByOccupancyGridMap函数实现的

filterByOccupancyGridMap(*input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc);

然后根据参数配置,来决定要不要执行距离的滤波

if (radius_search_2d_filter_ptr_) {auto pc_frame_pose_stamped = getPoseStamped(*tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp);radius_search_2d_filter_ptr_->filter(high_confidence_pc, low_confidence_pc, pc_frame_pose_stamped.pose, filtered_low_confidence_pc,outlier_pc);
} else {outlier_pc = low_confidence_pc;
}

然后就是对要发布的点云坐标转换再发布就好了

(mFunc)OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap

这个里面其实也很简单,就是根据阈值和对应栅格的值来判断要存进哪个里面,也没什么特殊的

for (sensor_msgs::PointCloud2ConstIterator<float> x(pointcloud, "x"), y(pointcloud, "y"),z(pointcloud, "z");x != x.end(); ++x, ++y, ++z) {const auto cost = getCost(occupancy_grid_map, *x, *y);if (cost) {if (cost_threshold_ < *cost) {high_confidence.push_back(pcl::PointXYZ(*x, *y, *z));} else {low_confidence.push_back(pcl::PointXYZ(*x, *y, *z));}} else {high_confidence.push_back(pcl::PointXYZ(*x, *y, *z));}
}

(Func)getCost

根据输入的网格和对应的xy索引,返回cost这么一个值的过程,其实主要的过程是计算xy对应的网格index,具体的cost值就是nav_msgs::msg::OccupancyGrid类型中对应的data

if (map_min_x < x && x < map_max_x && map_min_y < y && y < map_max_y) {unsigned int map_cell_x{};unsigned int map_cell_y{};map_cell_x = std::floor((x - map_position.x) / map_resolution);map_cell_y = std::floor((y - map_position.y) / map_resolution);size_t index = map_cell_y * map.info.width + map_cell_x;return map.data.at(index);
}

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

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

相关文章

2月科研——arcgis计算植被差异

ArcGIS中&#xff0c;设置高于或低于某个值的像元为 -9999&#xff0c;然后将这些地方设为空——目的&#xff1a;去除异常值和黑色背景值 Con(("T_std ano7.tif" > 2) | ("T_std ano7.tif" < - 2), - 9999,"T_std ano7.tif") SetNull(&…

python基础语法 007 文件操作-1读取写入

1 文件操作 1.1 什么时候用文件操作&#xff1f; 打开文档写东西看东西拿文档做统计 在python 文档操作作用 存储数据读取数据 打开文件有什么用&#xff1f; 读取数据&#xff0c;写入数据不管什么数据都可以用open打开&#xff0c;如可复制一张图片 1.2 open() 读取,…

c++中的冒泡排序(Bubble Sort),插入排序(Insertion Sort)和选择排序(Selection Sort)

前言 hello大家好啊&#xff0c;这里是文宇&#xff0c;不是文字&#xff0c;是文宇哦。今天开始爆更 冒泡排序&#xff08;Bubble Sort&#xff09; 冒泡排序&#xff08;Bubble Sort&#xff09;是一种简单的排序算法&#xff0c;它属于比较排序算法的一种。冒泡排序的基本…

Java开发利器:深入浅出`java.util.Objects`

在Java开发过程中&#xff0c;java.util.Objects作为一个不可或缺的工具类&#xff0c;为开发者提供了诸多实用方法&#xff0c;用以简化对象操作、提升代码健壮性和可读性。本文将详尽介绍Objects类中的几个核心方法及其应用场景&#xff0c;帮助你更好地驾驭Java编程。 前言…

C++顶层const和底层const

《Cprimer》中写到&#xff1a;顶层const表示指针本身是个常量&#xff0c;底层const表示指针所指的对象是一个常量。 顶层const可以表示任意的对象是常量&#xff0c;这一点对任何数据类型都适用。底层const则与指针和引用等复合类型的基本类型部分有关&#xff0c;比较特殊的…

【嵌入式DIY实例-ESP8266篇】-LCD ST7789显示BMP280传感器数据

LCD ST7789显示BMP280传感器数据 文章目录 LCD ST7789显示BMP280传感器数据1、硬件准备与接线2、代码实现在本文中,将介绍如何通过 ESP8266 NodeMCU 开发板 (ESP-12E) 与 ST7789 TFT 显示屏连接。 在此项目中,我们将了解如何将 ESP8266 NodeMCU 板与 Bosch Sensortec 的 BMP2…

OpenAI发布迷你AI模型GPT-4o mini

本心、输入输出、结果 文章目录 OpenAI发布迷你AI模型GPT-4o mini前言OpenAI发布迷你AI模型GPT-4o mini英伟达联合发布 Mistral-NeMo AI 模型:120 亿参数、上下文窗口 12.8 万个 tokenOpenAI发布迷你AI模型GPT-4o mini 编辑 | 简简单单 Online zuozuo 地址 | https://blog.csd…

多视角多对学习方式的相关论文解读

目标解决问题&#xff1a; 1&#xff1b;地理分布的空气质量和天气站是异质的空间对象&#xff0c;它们监测的条件不同。现有的方法通常设计用于同质的空间对象&#xff0c;不适合联合预测。 2&#xff1b;观察误差累积&#xff1a;监测站的观测数据往往因传感器误差和环境干扰…

如何在 Nginx 中配置访问日志的格式?

文章目录 如何在 Nginx 中配置访问日志的格式&#xff1f;一、Nginx 访问日志的重要性二、Nginx 访问日志的默认格式三、自定义 Nginx 访问日志格式四、配置访问日志的存储路径五、实际应用场景与示例场景一&#xff1a;电商网站场景二&#xff1a;多语言网站场景三&#xff1a…

Win10+Docker配置TensorRT环境

1.Docker下载和安装 Docker下载:Install Docker Desktop on Windows Docker安装: 勾选直接下一步就行,安装完成后需要电脑重启。 重启后,选择Accept—>Continue without signing in—>skip survey. 可以进入下面页面,并且左下角是绿色的,显示e…

用Python写一个视频采集脚本,对某网站进行批量采集

最近某牙上又出现一批高质量视频&#xff0c;听说删的很快&#xff0c;还好我会Python&#xff0c;赶紧采集下来保存&#xff01; 准备工作 环境使用 Python 3.10 解释器 Pycharm 编辑器 模块使用 requests >>> 数据请求模块 re <正则表达式模块> os <文…

unittest框架和pytest框架区别及示例

unittest框架和pytest框架区别及示例 类型unittest框架pytest框架unittest框架示例pytest框架示例安装python内置的一个单元测试框架,标准库&#xff0c;不需要安装第三方单元测试库&#xff0c;需要安装使用时直接引用 import unittest安装命令&#xff1a;pip3 install pyte…

matlab 声音信号希尔伯特黄变换

1、内容简介 略 91-可以交流、咨询、答疑 2、内容说明 略 Hilbert-Huang变换&#xff08;HHT&#xff09;是一种基于经验的数据分析方法 方法。它的扩展基础是自适应的&#xff0c;因此它可以从非线性和非平稳过程中产生具有物理意义的数据表示。这个 适应性的优势是有代价…

加入更多的功能,我需要随便输入一个 我今天的工作 ,它能自动分类到 其中一个,例如 “编程” 它会自动分类到 工作

为了实现更智能的分类&#xff0c;我们需要改进分类器&#xff0c;使其能够处理用户输入的任意文本&#xff0c;并自动分类到相应的类别。可以通过增加训练数据并改进数据预处理和分类器训练来实现这一点。 改进步骤 增加训练数据&#xff1a;增加更多样化的训练数据&#xf…

算法——双指针(day4)

15.三数之和 15. 三数之和 - 力扣&#xff08;LeetCode&#xff09; 题目解析&#xff1a; 这道题目说是三数之和&#xff0c;其实这和我们之前做过的两数之和是一个规律的~无非就是我们需要实时改动target的值。先排好序&#xff0c;然后固定一个数取其负值作target&#xf…

【MySQL进阶之路 | 高级篇】GROUP BY优化,分页查询优化

1. GROUP BY优化 group by使用的索引的原则几乎跟order by一致&#xff0c;即使没有过滤条件用到索引&#xff0c;也可以直接使用索引。 group by先排序再分组&#xff0c;遵循索引建立的最佳左前缀原则 当无法使用索引列&#xff0c;增大max_length_for_sort_data和sort_buf…

【Linux 驱动】IMX6ULL eLCDIF驱动

1. eLCDIF设备树 lcdif: lcdif021c8000 {compatible "fsl,imx6ul-lcdif", "fsl,imx28-lcdif"; //属性reg <0x021c8000 0x4000>; //起始地址 地址大小interrupts <GIC_SPI 5 IRQ_TYPE_LEVEL_HIGH>; …

Linux存储管理-逻辑卷管理(LVM)

逻辑卷管理&#xff08;LVM&#xff09;流程 物理磁盘/分区 -> 物理卷&#xff08;PV&#xff09;-> 加入卷组&#xff08;VG&#xff09;-> 卷组调配空间&#xff0c;制作逻辑卷&#xff08;LV&#xff09;-> 格式化 -> 挂载 -> 使用 为什么需要逻辑卷管理…

Web 性能入门指南-3.5 优化单页应用程序 (SPA)

&#x1f338; 欢迎来到前端后花园&#xff01;这里是一个温馨的小角落&#xff0c;专为热爱前端技术的你打造。没有华丽的辞藻&#xff0c;只有真诚的分享。希望你能在这里找到实用的内容&#xff0c;学到新知识&#xff0c;同时也欢迎你畅所欲言&#xff0c;分享你的思考和见…

【Linux 13】文件系统

文章目录 &#x1f308; 一、前言&#x1f308; 二、文件操作的系统接口⭐ 1. 打开文件 open⭐ 2. 关闭文件 close⭐ 3. 写入文件 write⭐ 4. 读取文件 read &#x1f308; 三、文件描述符⭐ 1. 文件描述符介绍⭐ 2. 提前被分配的文件描述符 0 1 2⭐ 3. 文件描述符的分配规则 &…