域描述
Perception子目录分为三个部分:过滤器、分割和跟踪。 filters目录包含节点和库，它们是传感器数据过滤管道的一部分。
该目录中的节点可以作为独立的过滤模块，也可以与处理相同传感器数据类型的其他过滤算法级联。
在分段目录下的包可以将传感器数据分组成簇或感兴趣的对象，这可以为下游流程提供更多关于环境的信息。 过滤器
filter_node_base
目的/用例
该类为 AutowareArchitectureProposal 存储库 FilterNodeBase 中的所有过滤器节点提供了一个基本实现 。
设计
FilterNodeBase 提供了一个通用的过滤器实现，它接受一个输入点云，对点云进行一些操作，并在不同的主题上输出点云。 这允许过滤器实现具有相同的基本 API 集，并标准化用于这些过程的输入和输出的数据类型。
动态参数设置可用于从该基类继承的节点。 为了确保安全地设置参数，对 filter 和 get_node_parameters 方法的调用由互斥体保护。 请参阅下面的 param_callback 和 get_node_parameters 部分。 内部工作/算法
编写从 FilterNodeBase 继承的类的开发人员将需要实现两个方法：
- filter - 处理输入点云并修改参考输出点云
- get_node_parameters - 在参数更改事件期间处理参数
这些虚函数由父类通过以下方法调用：
- pointcloud_callback - 通话 filter
- param_callback - 通话 get_node_parameters
子类的构造函数
继承自FilterNodeBase的子类的构造函数需要 set_param_callback() 在声明完所有参数后调用才能在节点中启用动态参数。 参见 示例 MockFilterNodeBase 。 test/test_filter_node_base.cpp
pointcloud_callback
该 pointcloud_callback 方法在类的构造过程中绑定到订阅者对象。 该方法确定点云的有效性，然后调用该 filter 方法对点云进行处理。
| FILTER_NODE_BASE_LOCAL void pointcloud_callback( const sensor_msgs::msg::PointCloud2::SharedPtr msg) |
筛选
该 filter 方法是FilterNodebase 类中的一个虚方法。 该方法实现了主过滤算法，过滤结果通过 output .
此方法有两个输入，其中一个修改后的输出表示过滤的 PointCloud：
- input - 通过回调提供的引用传递的点云； 未修改。
- output - 此方法中的过滤算法修改此参数，该参数通过该 pointcloud_callback 方法的引用传递
参数回调
该 param_callback 方法绑定到参数服务回调，该回调在节点参数更改时触发。
FILTER_NODE_BASE_LOCAL rcl_interfaces::msg::SetParametersResult param_callback(
常量std::vector<rclcpp::Parameter> & p); |
此方法有一个输入：
该方法返回一个 SetParameterResult 对象，该对象将确定事件是否成功完成。 返回 get_node_parameters 值是由成功的参数检索确定的。
获取节点参数
该 get_node_parameters 方法从输入参数向量中检索节点参数，创建返回结果结构，并返回该结构。 从这个节点创建子类的开发者应该在这个方法中实现所有动态可配置参数的检索。
FILTER_NODE_BASE_LOCAL虚拟rcl_interfaces::msg::SetParametersResult get_node_parameters(
常量std::vector<rclcpp::Parameter> & p) = 0; |
此方法有一个输入：
- p - 在传递的节点命名空间中声明的参数列表 param_callback
该方法返回一个 SetParameterResult 对象，用于表示事件已成功完成。 输入/输出/API
输入
从 autoware::perception::filters::filter_node_base::FilterNodeBase 继承的节点需要设置以下参数：
- max_queue_size - 定义队列的最大大小
继承自 的子类 FilterNodeBase 可以在子类的构造函数中声明附加参数。 声明的任何参数都应在 get_node_parameters 方法中检索 - 除非该参数不打算更改（例如 max_queue_size ）。 假设/已知限制
- 过滤器不需要对帧进行转换，因为这将由一个纯粹用于转换主题的外部节点处理。
相关问题
off_map_obstacles_filter_nodes
这是 off_map_obstacles_filter_nodes 包装的设计文件。
目的/用例
这是停车规划器和障碍物检测限制的临时解决方法。 也就是说，一些不在地图上的障碍物（通常是虚假检测）正在中断停车操作。
真正的解决方案是只对实际动态对象进行检测，并使停车规划器更加稳健（例如，在等待障碍物消失后恢复停车）。
设计
这是纯 off_map_obstacles_filter 包的标准 ROS 级别包装器。
它获取一个lanelet 地图，并过滤所有传入的 BoundingBoxArray 消息。 为了计算它们与地图的重叠，它还通过要求 tf2_ros 具有相同时间戳的转换将它们从它们的原生帧转换为地图帧。 假设/已知限制
假设变换在 100ms 内可用。 输入/输出/API
输入主题是 bounding_boxes_in ，输出主题是 bounding_boxes_out 。 所有设置都是通过参数完成的。 错误检测和处理
如果转换不可用，则不会过滤障碍物，因为这意味着系统将回退到制动以发现潜在的虚假障碍物。
未来的扩展/未实现的部分
地图加载本身也可以更好一些 lanelet2_map_provider 。 但在未来，无论如何我们都应该完全移除这个节点。
off_map_obstacles_filter
这是 off_map_obstacles_filter 包装的设计文件。
目的/用例
提供 off_map_obstacles_filter_nodes 包装的核心功能 - 有关背景，请参见其设计文档。
设计
核心功能以成员函数的形式提供，该成员函数删除与地图没有实质重叠的边界框。
假设/已知限制
一个基本假设是 base_link 和 map 框架的 z 轴是共线的。 另一个假设是边界框仅通过偏航旋转，而不是滚动或俯仰。
输入/输出/API
请参阅API文档。
内部工作/算法
该算法采用边界框，将它们转换为地图框架，然后将它们转换为 lanelet::Polygon2d s。 然后，每个多边形使用边界框相交测试从地图中获取潜在的重叠原始图。 使用精确的算法测试了这些重叠的每个候选物。 lanelet2 与之集成良好 boost::geometry ，因此用于计算重叠百分比。
错误检测和处理
错误检测并没有真正完成。
参考/外部链接
请参阅 LANELET2几何引物 。
未来的扩展/未实现的部分
将来，我们应该完全删除这个包。
相关问题
问题 #840。 outlier_filter
目的/用例
该 outlier_filter 包是一个库包，其中包含来自 TierIV AutowareArchitectureProposal 存储库 的三个异常值过滤器的实现：
- radius_search_2d_filter - 使用 PCL 函数径向搜索一个点周围的邻居
- voxel_grid_outlier_filter - 使用体素来确定特定体素是否包含异常值
- ring_filter （尚未实现）- 使用来自激光雷达的环形信息来确定某个点是否为异常值
设计
最初与 ROS2 节点接口耦合的算法已被分离，以按照 AutowareAuto 中的包设计指南创建两个新包。 此库包包含三种异常值过滤器类型的过滤算法的主要实现。 假设/已知限制
离散化
voxel_grid_outlier_filter 使用离散体素来确定一个点是否为异常值 的实现。 由于离散化算法的性质，可能会引入一个小的近似误差。 radius_search_2d_filter 对于减少近似误差很重要的应用程序，应使用 替代的异常值过滤器实现，例如。
仅 2D 半径搜索
radius_search_2d_filter 实现仅在二维空间中 。 更多详细信息请参见以下部分。
控制对参数的访问
由于参数可能在运行时更改，使用过滤器类的节点类将需要通过互斥锁控制对参数的访问。 需要保护的函数是 update_parameters 和 filter 。 输入/输出/API
构造函数
过滤算法使用的参数最初应该通过类构造函数传入过滤器类。 此包中过滤器类的典型构造函数将采用以下形式：
| 显式 OUTLIER_FILTER_PUBLIC src/perception/filters/outlier_filter/src/voxel_grid_outlier_filter.cpp（ int input_a， int input_b）； |
过滤器功能所需的详尽参数列表 在哪里 input_a 和是。 input_b 过滤器的名称应附加后缀 Filter 。
radius_search_2d_filter
需要以下参数 radius_search_2d_filter ：
- search_radius_ - 搜索邻居点周围的半径（类型： double ，单位：米）
- min_neighbors_ - 点不被视为异常值所需的最小邻居数（类型 int ：）
voxel_grid_outlier_filter
voxel_grid_outlier_filter 类构造函数 需要以下参数：
- voxel_size_x - x 轴上的体素叶大小（类型： float ，单位：米）
- voxel_size_y - y 轴上的体素叶大小（类型： float ，单位：米）
- voxel_size_z - z 轴上的体素叶大小（类型： float ，单位：米）
- voxel_points_threshold - 体素有效的最小体素点数（类型 int ：）
修改参数
这些类允许在运行时修改参数。 使用过滤器库的Node类应该调用该 update_parameters 方法并传入所有参数； 例如：
std::mutex mutex_;
浮动搜索半径 = 1.0;
int min_neighbors = 5;
自动过滤器 = std::make_shared<RadiusSearch2DFilter>(search_radius, min_neighbors);
// 对参数进行一些更新
搜索半径 = 1.5;
{
std::lock_guard<std::mutex> lock(mutex_);
filter->update_parameters(search_radius, min_neighbors);
} |
过滤功能
每个过滤器类都应该包含一些过滤器激活函数，它将在输入点云上运行过滤器。
无效的 OUTLIER_FILTER_PUBLIC 过滤器（
const pcl::PointCloud<pcl::PointXYZ> & 输入，
pcl::PointCloud<pcl::PointXYZ> &输出); |
根据过滤方法，输入中使用的点类型可能会有所不同。 过滤后的点云存储在 output . 内部工作/算法
radius_search_2d_filter
radius_search_2d_filter 过滤方法很简单 。 通过将 PointXYZ 转换为 PointXY 来展平输入点云。 它利用 PCL Search 对象 search_radius_ 在兴趣点周围的 半径（由 定义）中搜索相邻点。 如果找到最小数量的相邻点，则将原始点添加到输出点云中。
voxel_grid_outlier_filter
voxel_grid_outlier_filter 使用 PCL 库 VoxelGrid 对象将点云离散为包含一定数量点的等大小体素 的过滤方法。 然后搜索 3D 网格以确定点是否位于体素内。 返回现有体素的点不被视为异常值，并被添加到输出点云中。 错误检测和处理
不适用
安全注意事项
不适用
外部参考 radius_search_2d_filter
radius_search_2d_filter 可以在 TierIV AutowareArchitectureProposal 存储库中找到 原始实现：
voxel_grid_outlier_filter
voxel_grid_outlier_filter 可以在 TierIV AutowareArchitectureProposal 存储库中找到 原始实现：
outlier_filter_nodes
目的/用例
这个包的目的是为 outlier_filter 库函数提供 ROS2 节点接口。 该库的功能与节点接口一起允许用户在输入点云上应用各种异常值过滤技术。
该 outlier_filter_nodes 软件包包含用于启动的节点可执行文件：
有关确切算法细节的更多信息可以在 Inner-workings / Algorithms 中找到。
设计
这个包使用 filter_node_base 和 outlier_filter 库。 节点从 FilterNodeBase 类扩展而来，并在 ROS2 输入和 outlier_filter 包中的库函数之间提供接口。 假设/已知限制
子类被声明为 final 确保在调用 set_param_callback() 方法后没有声明额外的参数。 输入/输出/API
默认情况下，通过继承 FilterNodeBase 此包中的两个节点订阅输入 sensor_msgs::msg::PointCloud2 消息并 sensor_msgs::msg::PointCloud2 通过发布消息来输出。 更多信息可以在 filter_node_base 中找到。
主题名称如下：
参数
从 FilterNodeBase 中，两个异常值过滤器节点都需要以下参数
- max_queue_size - (int) 定义队列的最大大小
特定于启动节点的参数可以在 outlier_filter 中找到。 这些将需要在发布时指定。 错误检测和处理
不适用
安全注意事项
不适用 point_cloud_filter_transform_nodes
目的/用例
为了正确分离关注点并更容易重用数据（即预先记录的包数据或来自模拟器的数据），我们需要将来自驱动程序的转换和过滤数据分离到单独的节点中。
特别是，我们需要一个围绕我们的算法的样板包装器，它被实现为一个名为 lidar_utils . 这个样板节点包装器允许我们与 Autoware.Auto 的其余部分进行通信。
设计
PointCloud2 消息类型的这些节点有一个实例， 用于可视化和开源支持。
以下组件是 lidar_utils 包的一部分，但 point_cloud_filter_transform 节点使用它们来过滤和转换输入点云数据： 距离过滤器
过滤器类以检查点是否包含在由原点的最小和最大半径定义的范围内。 以正方形形式操作以避免使用 sqrt()。 角度过滤器
过滤器类以检查点是否位于由最小和最大角度定义的范围内。 包含区域由逆时针方向的范围[开始角度，结束角度]定义。 为避免使用反三角函数显式计算角度，使用以下方案：
- 一个被调用的向量 range_normal 被定义为放置在范围中位数的单位向量。
- 最小或最大角度的单位向量投影到此 range_normal 。 投影现在是可接受的角度范围 的一半的余弦因子，即 ( cos(\frac{\theta}{2}) )。 这个值称为阈值。 θ c o s ( θ 2 )
- 如果一个点在范围内，它在范围法线上的投影值应该大于等于这个阈值。 过滤器以正方形形式运行以避免使用 sqrt()
静态变压器
对给定点应用变换。 在计算过程中使用 Eigen 库。
是, , 的 point_cloud_filter_transform_node_exe 包装器 ， 执行顺序如下： DistanceFilter AngleFilter StaticTransformer
- 检查点是否在 和 的范围 DistanceFilter 内 AngleFilter
- 使用转换点 StaticTransformer
- 以PointCloud2 格式 发布转换和过滤后的数据
假设/已知限制
该实现不允许距离和角度过滤器的动态阈值。 它也不支持运行时的动态转换。 阈值和变换参数只能在构建节点时加载。 输入/输出/API
这些节点具有以下基本结构：
输入：
输出：
- points_filtered（消息 PointCloud2 ）
最重要的是，可以通过编程方式或通过构造参数文件配置节点。 错误检测和处理
大多数错误处理发生在 rclcpp .
在这些节点中发生的唯一显式错误处理是在运行时循环中捕获错误。
如果捕获到这样的错误，则节点将简单地转换到“失败状态”，记录这一事实，并尝试继续运行。 安全注意事项
由安全专家待定。
相关问题
- #328：端口转换/过滤节点从 Apex.Auto 到 Autoware.Auto； 从驱动程序中删除过滤器/转换功能
point_cloud_fusion_nodes
目的/用例
我们需要一个能够将两个或多个点云融合成一个点云的节点。
设计
该节点使用 message_filters::Synchronizer 具有同步策略的 message_filters::sync_policies::ApproximateTime 来同步来自不同订阅的消息。 假设/已知限制
最多仅支持 8 个源的点云融合。 这个限制是由于 message_filters 包。 此外，为了使策略起作用， 队列中 ApproximateTime 应该有消息，以防 需要融合消息。 这种限制来自于同步器需要一个参考点才能将大致相似时间的消息组合在一起的事实。 有关详细信息，请参阅 ros1 文档 。 N+1 N 输入/输出/API
这些节点具有以下基本结构：
输入：
相关问题
polygon_remover_nodes
这是 polygon_remover_nodes 包装的设计文件。
目的/用例
此节点允许用户从 PointCloud2 中删除属于自我车辆的点。
它包裹了 `polygon_remover` 库并将其公开为 ROS2 节点。
设计
该节点具有区分多边形如何馈送到节点的工作模式：
假设/已知限制
这里详细解释： `polygon_remover` 输入/输出/API
订户：
- points_xyzi (sensor_msgs::msg::PointCloud2)
出版商：
- cloud_polygon_removed (sensor_msgs::msg::PointCloud2)
- marker_polygon_remover (visualization_msgs::msg::Marker)
- will_visualize 如果设置为 ，将发布多边形的标记 True
设置参数：
cd ~/adehome/Autoware.Auto
源安装/setup.bash
# 使用启动文件
ros2 启动 polygon_remover_nodes polygon_remover_nodes.launch.py
# 或者 ros2 运行
ros2 运行 polygon_remover_nodes polygon_remover_node_exe --ros-args --params-file ~/AutowareAuto/src/perception/filters/polygon_remover_nodes/param/test_params.yaml
# 或者提供参数
ros2 运行 polygon_remover_nodes polygon_remover_node_exe \
--ros-args \
-r "points_xyzi":="/lidar_front/points_xyzi" \
-p 工作模式：=“静态”\
-p 多边形顶点：=[-8.0,-8.0,\
-8.0,8.0,\
8.0,0.0] \
-p will_visualize:=真
|
/polygon_remover_nodes：
ros__参数：
working_mode: "Static" # "Static" 或 "PolygonSub"
# 如果工作模式是“静态”
# x1, y1, x2, y2, ...
多边形顶点：[-6.0，-6.0，
-6.0, 6.0,
6.0, 6.0,
6.0, -6.0 ] # 正方形
will_visualize: 真
|
内部工作/算法
这里详细解释： `polygon_remover` 错误检测和处理
抛出：
- std::runtime_error("Please set working_mode to be one of: " + str_list_of_keys); if working_mode 未设置为 Static or PolygonSub 并退出。
- std::length_error("polygon_vertices has odd number of elements. " "It must have a list of x,y double pairs."); 如果给定的顶点字段计数不是 2 的倍数。（例如 [0.0,0.0, 3.0,0.0, 4.0,0.0, 7.0] ）
- 此外，如果顶点数小于 3，将抛出其他东西（在 `polygon_remover` 中有详细说明）
安全注意事项
待定。
参考/外部链接
无法使用。
未来的扩展/未实现的部分
无法使用。
polygon_remover
这是 polygon_remover 包装的设计文件。
目的/用例
这个包允许用户从给定的 PointCloud2 消息中删除给定的凸面或凹面多边形区域。 该包忽略点的 z 值（高度）并根据 x 和 y 值进行删除。
 
设计
该包包含一个名为 PolygonRemover . 它希望您构造它的一个实例。 生成实例后，用户可以通过以下两种方式使用它：
- 先给它一个多边形，然后给它一个云。 （ update_polygon 然后 remove_updated_polygon_from_cloud ）
- 一起给它一个多边形和云。 remove_polygon_cgal_from_cloud
它将返回一个多边形过滤的点云。
可选地，它还可以为多边形形状可视化提供标记。
假设/已知限制
- 输入是顺时针或逆时针的一系列点。 输入的第一个和最后一个点将被连接。
- 输入点列表应至少有 3 个点。
- 输入点应在输入点云的框架中给出。 该包不会执行帧转换。
- 在给出点云之前或与点云一起提供多边形。 否则，包将返回传入的点云，因为它带有一条 INFO 消息。
- 该代码根本不检查 z 值。 只要点的 x and y 值在多边形内，它就会过滤掉任何点。
- 如果启用了可视化，多边形的标记将从提供的点云中获取其 frame_id。 get_marker 在使用该方法 之前，最好确保至少喂一次云。
输入/输出/API
简单的例子：
polygon_remover::PolygonRemover polygon_remover(true); // will_visualize = true
PointCloud2::SharedPtr cloud_ptr = std::make_shared<Shape>(); // Some point cloud
Shape::SharedPtr shape_ptr = std::make_shared<Shape>(); // And put at least 3 points to the shape
// Either first initialize
polygon_remover.update_polygon(shape_ptr);
// then filter
PointCloud2::SharedPtr cloud_filtered_ptr =
polygon_remover.remove_updated_polygon_from_cloud(cloud_ptr);
// Or provide cloud and polygon together
PointCloud2::SharedPtr cloud_filtered_ptr =
polygon_remover.remove_polygon_geometry_from_cloud(cloud_ptr, shape_ptr);
// If you allowed visualization you can get a Marker for visualization
Marker marker = polygon_remover.get_marker();
// Marker will get its frame_id from the cloud once it has been provided. |
内部工作/算法
该软件包使用 计算几何算法库 (CGAL) 库将多边形构造为 std::vector<CGAL::Exact_predicates_inexact_constructions_kernel::Point_2> 对象。
它使用 光线投射算法 来检查一个点是否位于多边形内，如在 CGAL 中实现的那样。
错误检测和处理
抛出：
- polygon_geometry_to_cgal std::length_error("Polygon vertex count should be larger than 2."); 如果多边形顶点数小于 3， 方法将抛出。
- remove_updated_polygon_from_cloud std::runtime_error( "Shape polygon is not initialized. Please use update_polygon first."); 如果多边形尚未初始化， 方法将抛出。
安全注意事项
待定。
参考/外部链接
未来的扩展/未实现的部分
无法使用。 ray_aggregator
目的/用例
RayGroundClassifier单独对光线 进行 操作。 这以向模块提供射线信息为前提。 虽然这对于扫描 LiDAR 来说是正确的，但在实践中，这种假设并不总是可用的，具体取决于接口定义、驱动程序实现和 LiDAR 扫描模式。
因此，需要一个从非结构化点云生成射线以用于地面分类的组件，以使其 RayGroundClassifier 更普遍地工作。
设计
光线聚合器一般有三个操作：
- 向聚合器添加点
- 检查是否有任何光线准备好
- 获取下一条光线
第二和第三个操作从单个操作中分离出来，通常使 API 更加具体和易于理解。 算法设计
该对象通常以下列方式运行：
- 根据配置设置预先分配缓冲区
- 对于每个点，计算一个角度并因此计算 bin，将该点添加到相应的射线
- 如果任何光线满足给定的标准，那么它（它的索引）被添加到就绪光线列表中
- 当一条射线被看到（得到）时，它会在下一次被触摸添加点时被重置
最后，当前使用的标准是给定射线中的点数。 这是为了简单起见。 当接收到完整扫描时，所有光线都被认为已准备就绪，无论光线中有多少点（如果点数为正数）。 输入/输出/API
有关 autoware::perception::filters::ray_ground_classifier::RayAggregator 更多详细信息，请参阅。 内部工作/算法
一般来说，这 RayAggregator 是相当直截了当的。
但是，它的实现有一个复杂性：
每条射线在内部具有三种状态：
- NOT_READY
- READY
- RESET
当看到射线时设置最后一个状态，并用于在下一次插入射线时重置射线的状态。
安全注意事项
安全专家待定。
参考/外部链接
请参阅原始 Autoware 实现，其中 RayAggregator 隐含定义： CPFL 的 Autoware
未来的扩展/未实现的部分
可以添加和/或评估更奇特的“扫描就绪”标准，例如：
ray_ground_classifier
目的/用例
Autoware.Auto 需要一种方法来区分点云中的地面点和非地面点。 这旨在充当对象检测算法（在非地面点上工作）和自由空间估计算法（在地面点上工作）的过滤步骤。 这种方法非常快，并且允许在扫描数据包仍在进入时暴露过滤后的点云。
设计 算法设计
具体来说，射线地面过滤器的原始实现根据方位角切片（即车架中 x 和 y 之间的角度）对点云中的点进行批处理。 这些通常被分成 0.1 度弧度的角段。 这大致相当于 VLP-16 的一次发射。
一旦点云被重构为射线，射线中的每个点都会用平面距离（仅在 xy 位置上的欧几里德距离，同样在车架中）进行注释。 高度太高或半径太低的点将被忽略。 然后按照径向距离递增的顺序对光线进行排序。
一旦对光线进行排序，算法就会按照径向距离增加的顺序扫描光线中的点。 该算法定义了两个锥体：全局锥体和局部锥体，其中局部锥体具有比全局锥体更宽的孔径。 全局锥植根于点云的足迹（例如向下投影到地平面）。 局部锥体植根于最后处理的点。
射线地面过滤器有两个主要的状态变量：最后处理的点，以及最后一个点是否被标记为地面点。 这些变量被初始化为全局锥的根，最初是假的（不是接地的）。 对于每个后续点，然后使用以下逻辑标记该点：
如果当前点在局部锥内： 如果最后是地面，这也是地面 否则，仅当当前点在全局锥中时才接地 否则，仅当当前点位于全局锥内且径向距离距 最后一点高于阈值。
第一个分支给近点（在 z 意义上）相同的标签。 第二个分支提供了一种切换回接地点的机制。 在最后一个分支中，当前点已经在局部锥体之外（在 z 意义上），因此它必须首先是非局部的（在 r 意义上）才能与全局锥体进行检查。 软件设计决策
添加了一个新的点结构。 这是为了预先计算一个点的投影半径，并在处理一条射线（即标记、排序）期间节省一点计算工作。 输入/输出/API
输入：
至少具有以下字段的点块：
这样具有相同射线的所有点都是连续的。 在单个块内， ray id 不得在不连续的集合中重复。
输出：
两个输出点块（一个用于接地，一个用于非接地）至少具有以下字段：
此外，流由具有空间 ray id velodyne::END OF SCAN ID .
有关更多详细信息，请参阅 API 文档 。 内部工作/算法
上述算法已修改为直接处理来自设备并由 (Velodyne) 驱动程序解析的点流。 Autoware.Auto 执行以下操作：
- 每个点 ray id 根据其在 LiDAR 数据包中的位置在驱动程序级别分配一个
- LiDAR 的单次发射被认为是一条射线
- 数据包被顺序解析，使得一条射线中的所有点在输出点流中都是连续的
- 射线地面过滤器读入点流
- 一旦 ray id 新点的 与当前跟踪的 ID 不同，则对构成当前射线的点的缓冲区进行排序，并按径向顺序通过上述逻辑，或在径向距离相等的情况下按高度递增
- std::partial_sort 用于排序而不是 std::sort 或 std::stable_sort 即使它性能较低，因为它不是递归的
- 标记每个点后，将其推送到适当的输出缓冲区
此外，为了使远距离点的分类更加稳健，在全局高度阈值中添加了一个限制。 添加此功能是为了防止全局高度阈值随着距离的增加而无限增长，从而导致大多数（如果不是全部）远点被归类为地面。
还添加了其他规则以提高算法的性能：
- 如果相对于下一个点有垂直结构，则下一个点和最后一个点是非地面的
- 如果最后一个点是非地面且当前点在最后一个地面点附近，那么如果下一个点是地面，则当前点是地面（假设当前点是地面），否则是非地面
- 如果最后一个点是非地面的，并且当前点垂直靠近最后一个点，那么它也是非地面的
性能表征 时间
过滤单条射线的时间复杂度是 O(k + k log(k) + k) 插入计算半径，按半径排序，然后扫描点标记为地面或非地面。
操作是这样的 O(k log k) 。 k 是激光扫描射线的大小，在此实现中是硬件定义的常数。
因此，过滤整个大小的点云 n 需要 n log k 时间。
空间
该模块的空间复杂度为 O(k)=O(1) ，其中 k 是激光扫描射线的大小，是硬件定义的常数。 状态
核心状态
核心过滤器具有三个浮点状态变量，完全由最后处理的点决定：
- last_point_ground
- last_point_height
- last_point_radius
配置状态
以下配置状态由配置类 Config 引入。
错误检测和处理
主要的潜在故障模式是由于未定义的行为引起的 std::partial_sort ，例如，如果比较器在语义上不正确，以及在数组之外进行索引。 前者应该是正确的，并且有防止后者的检查。
此外， RayGroundClassifier 在函数调用期间在内部使用指针，并在内部使用数组进行存储。 这两个元素带来了潜在的错误，尽管访问这些元素是通过适当的检查来保护的。
接下来，该算法的主要不变量是核心标记函数要求点按径向距离增加的顺序排列。 如果径向距离大致相同，则列表按增加高度排序。 这个不变量在核心标记方法开始时被检查为不被破坏。 值得注意的是，有一些阈值可以补偿浮点错误。 为了处理径向距离大致相等的情况，允许径向距离缩小 [FEPS](autoware::common::types::FEPS)。 虽然这允许破坏不变量，但实际上这不会引起问题，因为如果最大后退发生在 1000 点，净后退将不超过 1 毫米，考虑到 VLP16-HiRes 的精度，这可以忽略不计不大于 1 厘米。
如果配置参数在语义上相互不正确，那么在配置类的构造过程中将抛出错误。
安全注意事项
安全专家待定。
参考/外部链接
射线地面过滤器的设计基于 CPFL 的 Autoware中提供的实现
未来的扩展/未实现的部分
使用最后一帧的全局地平面估计进行校正 + 估计。 ray_ground_classifier_nodes
目的/用例
我们需要一个节点，将原始点组划分为地面集和非地面集。 这种初始预处理使我们能够更有效地使用下游算法。
特别是，我们需要一个围绕我们的算法的样板包装器，它被实现为一个库。 这个样板节点包装器允许我们与 Apex.OS 的其余部分进行通信。
设计
这些节点是 `RayGroundClassifer` 的简单包装器。
提供消息类型的节点用于 PointCloud2 可视化和开源支持。 该 PointCloud2 节点支持非结构化点云。
如果分类器工作所需的光线信息是先验已知的（例如，以如何从传感器接收点的方式进行编码），则认为输入是结构化的。
相反，如果光线信息不是先验已知的，则输入被认为是非结构化的。 这意味着必须做额外的工作来确定这些信息。 默认情况下，点云被认为是非结构化的。
因此，它 RayGroundClassifierCloudNode 有一个实例 RayAggregator 来为非结构化点云提供结构。 假设/已知限制
当前的假设是输入是结构化的。 这个假设将来会被放弃。
另一个限制是它 RayGroundClassifier 可以最大处理 512 大小的光线。
最后，为了简化逻辑，ray id 序列必须是连续的，例如：
1-1-2-2-1-1
不被认为是两条射线，而是三条射线。
有关更多详细信息，请参阅 RayGroundClassifier 设计文档。 输入/输出/API
这些节点具有以下基本结构：
输入：
- raw_points (PointBlock 或 PointCloud2 直接来自驱动节点的消息)
输出：
- points_ground (PointBlock 或 PointCloud2 的消息)
- points_nonground（PointBlock 或 PointCloud2 的消息）
最重要的是，可以通过编程方式或通过构造参数文件配置节点。 错误检测和处理
大多数错误处理发生在 rclcpp 内部、 RayGroundClassifier 自身内部或关联的配置类中。
在这些节点中发生的唯一显式错误处理是在运行时循环中捕获错误。
如果捕获到这样的错误，则节点将简单地转换到“失败状态”，记录这一事实，并尝试继续运行。 安全注意事项
rclcpp::Node 这些组件从和核心 RayGroundClassifier 类 继承安全考虑。
未来的扩展/未实现的部分
相关问题
- #2066：初步实施
- std::chrono #1956：用构造 替换顶点时间。
- #1935：向块节点添加非结构化行为，添加云节点
- #2150：获取结构化块回调处理的标头信息
voxel_grid_filter
体素网格过滤器
体素网格过滤器是一种点云下采样形式，它保留了存储在点云中的大部分信息。
有两种形式：
两种体素网格过滤器都以下列方式运行：
- 对于每个点，计算体素索引
- 这是通过将每个实值笛卡尔坐标转换为整数值坐标来实现的
- 然后将整数坐标转换为单个索引
- 该点存储在适当的数据结构中
近似体素网格滤波器
返回的点不是体素内所有点的质心，而是体素本身的质心。 虽然不太准确，但这消除了对数据的额外扫描。 具体来说，这是通过 pcl 以下方式完成的：
- 考虑点云中的每个点
- 每个点都分配给一个体素
- 如果体素没有分配给它的点，则将体素的质心推到输出点云，否则将被忽略
- 对于非空间项（例如颜色、强度），输出点采用激活所述体素的任何输入点的值
质心体素网格滤波器
质心体素网格过滤器需要额外通过数据来计算分配给体素的每个点的质心。 这样可以更准确地表示下层表面。 具体来说，这是 pcl 通过执行以下操作来完成的：
- 如上所述，每个点都存储在带有附加体素索引字段的向量中
- 然后根据体素索引对数组进行排序
- 然后扫描数组，为每个输出体素计算每个连续点集的质心
修改
进行了以下修改：
- 活动体素存储在哈希图中，以加快平均案例访问时间
- 随着点的接收和索引，体素质心逐渐更新
类似地，对于近似体素网格过滤器，使用哈希图（无序图）来存储活动体素。 但是，不会跟踪质心，并且哈希图仅用于跟踪体素是否处于活动状态。 建筑学
以下架构用于最大化代码重用和提高性能。
Voxel 提供了一个具有通用功能 的核心类。 这个类没有虚方法。 此类以点类型为模板。
类的子 Voxel 实例使用一些额外的、常用的方法来实例化，这些方法可以区分各个体素的行为。
数据结构本身以 VoxelGrid 体素类型为模板。
更进一步，算法的运行时分派可以通过使用非模板化 AlgorithmBase 和模板化子类范式来实现。
最后，关于一些 API 和结构决策的说明：
- An unordered_map 被用作底层数据结构，以提高平均查找时间并匹配数据的稀疏性
- 使用An std::forward_list 是因为该 splice 方法允许我们重用点
- new_voxels 除了 MISRA 考虑之外， 还提供了一个 const 引用作为输出，以防止体素网格的状态意外更改
性能表征
时间
插入体素网格过滤器就是插入哈希图，这是 O(n) 一种对抗情况。
对于质心体素网格过滤器，这导致算法 O(n*n + n) 复杂，其中第一项用于将点插入到体素网格和底层哈希图中，以及更新质心，第二项用于将点冲出体素网格。
这导致了一个算法是 O(n^2)
类似地，对于近似体素网格过滤器，该算法 O(n * n) 用于插入到体素网格中，并将新激活的点发送到下游。
这导致算法是 O(n^2) 。
在实践中，插入 hashmap 是 O(1) ，这会导致 O(n) 两种情况的算法。
空间
每个体素网格的空间复杂度由 O(n) 位于空间中的底层哈希图支配。
状态
体素网格过滤器有两种状态：配置和体素网格本身。 体素状态
- Hashmap<uint32_t, Voxel> ，用于存储稀疏体素网格的主要数据结构，类似于 std::unordered_map
- Voxel , 通过维护两个状态变量来表示一个精确的体素： centroid 和 num_points , 允许质心的增量更新
配置状态
下面定义了配置状态：
- min_point
- max_point , 结合上面定义了一个所有点都将落入的轴对齐边界框（AABB）
- leaf_size , 定义体素每边的长度
为了计算方便，进一步的配置状态变量是从这些预先计算出来的：
- inv_leaf_size ，通过将除法切换为乘法，可以更快地计算体素索引
- leaf_size_2 , 可以快速计算近似体素质心
- x_width , x 方向的体素数，用于快速计算体素指数和近似质心
- xy_width , 每个 z 行中的体素数，用于快速计算体素索引和近似质心
输入
体素网格数据结构的输入是体素模板化的任意点类型。
此体素必须具有以下属性才能成功编译：
- 公共 float32_t 成员 x、y 和 z
- 如果使用 CentroidVoxel , operator+ 必须 operator*(float32_t) 定义
输出
体素网格数据结构支持输出模式：
安全注意事项
安全专家待定。
未来的工作
底层 std::unordered_map 不是静态内存，并且不能使用默认的 STL 功能来实现。 如果可用，则必须使用静态内存分配器 voxel_grid_nodes
立体像素网格过滤器
这是 voxel_grid_nodes 包装的设计文件。
目的/用例
这个包通常实例化 voxel_grid 包中定义的对象和类，用于节点构造和一般的进程间通信。
简而言之，这个包的存在是为了将 voxel_grid 对象族连接到更大的 ROS 2 基础设施。
更具体地说，需要这个包中的功能，因为它允许我们将大点云下采样成更小的点云，同时仍然保留大量的表示能力。
设计
这个包有 3 个部分：
- 通过 PointCloud2 消息的镜头与底层体素网格交互的基本“算法”类
- 不同类型体素网格的基本“算法”类的实例，例如近似或质心
- PointCloud2 算法类 的节点包装器
使用基本“算法”类的动机是，底层 VoxelGrid 类是模板化的，并且具有依赖于模板的 API（就类型而言）。 这意味着为了获得运行时调度/多态性（以保持代码库更轻和更简单），我们必须将数据结构包装在多态基类中。 假设/已知限制
存在以下限制：
- 由于以下元素，节点在运行时分配内存：
- 底层 VoxelGrid 数据结构
- 复制 PointCloud2 标题（由于字符串 frame_id ）
- 一般 pub/sub 分配内存
输入/输出/API
“算法”API 通常很简单，只有一种输入方法和一种输出方法。 有关详细信息，请参阅以下 API 文档：
作为一个节点， `VoxelCloudNode` 有一套 标准的面向公众的方法。
输入是单个 PointCloud2 主题，输出是另一个 PointCloud2 主题。 错误检测和处理
异常通常会在错误时引发。
这个包中的功能通常没有错误处理，因为错误处理被委托给底层数据结构。
安全注意事项 安全专家待定
参考/外部链接
未来的扩展/未实现的部分
|
