|
21 | 21 |
|
22 | 22 | #include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>
|
23 | 23 |
|
| 24 | +#include <pcl/filters/passthrough.h> |
24 | 25 | #include <pcl/filters/voxel_grid.h>
|
25 | 26 | #include <tf2/utils.h>
|
26 | 27 |
|
@@ -128,6 +129,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
|
128 | 129 | publish_debug_pointcloud_ = declare_parameter<bool>("publish_debug_pointcloud");
|
129 | 130 | use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
|
130 | 131 | use_imu_path_ = declare_parameter<bool>("use_imu_path");
|
| 132 | + detection_range_min_height_ = declare_parameter<double>("detection_range_min_height"); |
131 | 133 | voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
|
132 | 134 | voxel_grid_y_ = declare_parameter<double>("voxel_grid_y");
|
133 | 135 | voxel_grid_z_ = declare_parameter<double>("voxel_grid_z");
|
@@ -221,9 +223,17 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
|
221 | 223 | pcl::fromROSMsg(transformed_points, *pointcloud_ptr);
|
222 | 224 | }
|
223 | 225 |
|
| 226 | + // apply z-axis filter for removing False Positive points |
| 227 | + PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud); |
| 228 | + pcl::PassThrough<pcl::PointXYZ> height_filter; |
| 229 | + height_filter.setInputCloud(pointcloud_ptr); |
| 230 | + height_filter.setFilterFieldName("z"); |
| 231 | + height_filter.setFilterLimits(detection_range_min_height_, 5.0); |
| 232 | + height_filter.filter(*height_filtered_pointcloud_ptr); |
| 233 | + |
224 | 234 | pcl::VoxelGrid<pcl::PointXYZ> filter;
|
225 | 235 | PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud);
|
226 |
| - filter.setInputCloud(pointcloud_ptr); |
| 236 | + filter.setInputCloud(height_filtered_pointcloud_ptr); |
227 | 237 | filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_);
|
228 | 238 | filter.filter(*no_height_filtered_pointcloud_ptr);
|
229 | 239 |
|
|
0 commit comments