From 8fd1707b149c28cb7fefc417797f276b4d9b6e55 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Wed, 27 Dec 2023 16:28:19 +0900 Subject: [PATCH 1/7] improve perfomance Signed-off-by: Taiga Takano --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 104 ++++++++++++------ 1 file changed, 72 insertions(+), 32 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 691c13a6d4701..c046bdfc8282d 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -135,8 +135,7 @@ void RadiusSearch2dFilter::filter( const int min_points_threshold = std::min( std::max(static_cast(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_distances, min_points_threshold); + const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices, k_distances); if (min_points_threshold <= points_num) { output.points.push_back(xyz_cloud.points.at(i)); @@ -150,43 +149,62 @@ void RadiusSearch2dFilter::filter( const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier) { - const auto & high_conf_xyz_cloud = high_conf_input; - const auto & low_conf_xyz_cloud = low_conf_input; // check the limit points number - if (low_conf_xyz_cloud.points.size() > max_filter_points_nb_) { + if (low_conf_input.points.size() > max_filter_points_nb_) { RCLCPP_WARN( rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"), "Skip outlier filter since too much low_confidence pointcloud!"); return; } - pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); - 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; + pcl::PointCloud::Ptr low_conf_xy_cloud(new pcl::PointCloud); + low_conf_xy_cloud->points.resize(low_conf_input.points.size()); + for (size_t i = 0; i < low_conf_input.points.size(); ++i) { + low_conf_xy_cloud->points[i].x = low_conf_input.points[i].x; + low_conf_xy_cloud->points[i].y = low_conf_input.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; + + // Low conf cloud check + std::vector k_indices_low(low_conf_xy_cloud->points.size()); + std::vector k_distances_low(low_conf_xy_cloud->points.size()); + kd_tree_->setInputCloud(low_conf_xy_cloud); + for (size_t i = 0; i < low_conf_input.points.size(); ++i) { + const float distance = std::hypot(low_conf_xy_cloud->points[i].x - pose.position.x, low_conf_xy_cloud->points[i].y - pose.position.y); + const int min_points_threshold = std::min( + std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), + max_points_); + const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_low, k_distances_low); + if (min_points_threshold <= points_num) { + output.points.push_back(low_conf_input.points.at(i)); + } else { + outlier.points.push_back(low_conf_input.points.at(i)); + } } - std::vector k_indices(xy_cloud->points.size()); - std::vector k_distances(xy_cloud->points.size()); - kd_tree_->setInputCloud(xy_cloud); - for (size_t i = 0; i < low_conf_xyz_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); + // High conf cloud check + if(outlier.points.size() == 0){return;} + + pcl::PointCloud::Ptr high_conf_xy_cloud(new pcl::PointCloud); + high_conf_xy_cloud->points.resize(high_conf_input.points.size()); + for (size_t i = 0; i < high_conf_input.points.size(); ++i) { + high_conf_xy_cloud->points[i].x = high_conf_input.points[i].x; + high_conf_xy_cloud->points[i].y = high_conf_input.points[i].y; + } + + std::vector k_indices_high(high_conf_xy_cloud->points.size()); + std::vector k_distances_high(high_conf_xy_cloud->points.size()); + kd_tree_->setInputCloud(high_conf_xy_cloud); + for (size_t i = 0; i < outlier.points.size(); ++i) { + const float distance = std::hypot(high_conf_xy_cloud->points[i].x - pose.position.x, high_conf_xy_cloud->points[i].y - pose.position.y); const int min_points_threshold = std::min( std::max(static_cast(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_distances, min_points_threshold); + const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_high, k_distances_high); if (min_points_threshold <= points_num) { - output.points.push_back(low_conf_xyz_cloud.points.at(i)); + output.points.push_back(outlier.points.at(i)); } else { - outlier.points.push_back(low_conf_xyz_cloud.points.at(i)); + outlier.points.erase(outlier.points.begin() + i); } } } @@ -239,19 +257,41 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc) { - PclPointCloud tmp_behind_pc; - PclPointCloud tmp_front_pc; - for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"), y(*input_pc, "y"), - z(*input_pc, "z"); - x != x.end(); ++x, ++y, ++z) { + size_t front_count = 0; + size_t behind_count = 0; + + for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"); x != x.end(); ++x) { if (*x < 0.0) { - tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + behind_count++; } else { - tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + front_count++; } } - pcl::toROSMsg(tmp_front_pc, front_pc); - pcl::toROSMsg(tmp_behind_pc, behind_pc); + + sensor_msgs::PointCloud2Modifier front_pc_modfier(front_pc); + sensor_msgs::PointCloud2Modifier behind_pc_modfier(behind_pc); + front_pc_modfier.setPointCloud2FieldsByString(1,"xyz"); + behind_pc_modfier.setPointCloud2FieldsByString(1,"xyz"); + front_pc_modfier.resize(front_count); + behind_pc_modfier.resize(behind_count); + + sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); + sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); + + for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); in_iter != in_iter.end(); ++in_iter) { + if (*in_iter < 0.0) { + *be_iter = in_iter[0]; + be_iter[1] = in_iter[1]; + be_iter[2] = in_iter[2]; + ++be_iter; + } else { + *fr_iter = in_iter[0]; + fr_iter[1] = in_iter[1]; + fr_iter[2] = in_iter[2]; + ++fr_iter; + } + } + front_pc.header = input_pc->header; behind_pc.header = input_pc->header; } From a4b15601ab9eb9ce0060a6795dbdc67aad74cf5f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 27 Dec 2023 08:39:17 +0000 Subject: [PATCH 2/7] style(pre-commit): autofix Signed-off-by: Taiga Takano --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 32 ++++++++++++------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index c046bdfc8282d..f5e96d52efee0 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -135,7 +135,8 @@ void RadiusSearch2dFilter::filter( const int min_points_threshold = std::min( std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), max_points_); - const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices, k_distances); + const int points_num = + kd_tree_->nearestKSearch(i, min_points_threshold, k_indices, k_distances); if (min_points_threshold <= points_num) { output.points.push_back(xyz_cloud.points.at(i)); @@ -169,11 +170,14 @@ void RadiusSearch2dFilter::filter( std::vector k_distances_low(low_conf_xy_cloud->points.size()); kd_tree_->setInputCloud(low_conf_xy_cloud); for (size_t i = 0; i < low_conf_input.points.size(); ++i) { - const float distance = std::hypot(low_conf_xy_cloud->points[i].x - pose.position.x, low_conf_xy_cloud->points[i].y - pose.position.y); + const float distance = std::hypot( + low_conf_xy_cloud->points[i].x - pose.position.x, + low_conf_xy_cloud->points[i].y - pose.position.y); const int min_points_threshold = std::min( std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), max_points_); - const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_low, k_distances_low); + const int points_num = + kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_low, k_distances_low); if (min_points_threshold <= points_num) { output.points.push_back(low_conf_input.points.at(i)); } else { @@ -181,8 +185,10 @@ void RadiusSearch2dFilter::filter( } } - // High conf cloud check - if(outlier.points.size() == 0){return;} + // High conf cloud check + if (outlier.points.size() == 0) { + return; + } pcl::PointCloud::Ptr high_conf_xy_cloud(new pcl::PointCloud); high_conf_xy_cloud->points.resize(high_conf_input.points.size()); @@ -195,11 +201,14 @@ void RadiusSearch2dFilter::filter( std::vector k_distances_high(high_conf_xy_cloud->points.size()); kd_tree_->setInputCloud(high_conf_xy_cloud); for (size_t i = 0; i < outlier.points.size(); ++i) { - const float distance = std::hypot(high_conf_xy_cloud->points[i].x - pose.position.x, high_conf_xy_cloud->points[i].y - pose.position.y); + const float distance = std::hypot( + high_conf_xy_cloud->points[i].x - pose.position.x, + high_conf_xy_cloud->points[i].y - pose.position.y); const int min_points_threshold = std::min( std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), max_points_); - const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_high, k_distances_high); + const int points_num = + kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_high, k_distances_high); if (min_points_threshold <= points_num) { output.points.push_back(outlier.points.at(i)); @@ -259,7 +268,7 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( { size_t front_count = 0; size_t behind_count = 0; - + for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"); x != x.end(); ++x) { if (*x < 0.0) { behind_count++; @@ -270,15 +279,16 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( sensor_msgs::PointCloud2Modifier front_pc_modfier(front_pc); sensor_msgs::PointCloud2Modifier behind_pc_modfier(behind_pc); - front_pc_modfier.setPointCloud2FieldsByString(1,"xyz"); - behind_pc_modfier.setPointCloud2FieldsByString(1,"xyz"); + front_pc_modfier.setPointCloud2FieldsByString(1, "xyz"); + behind_pc_modfier.setPointCloud2FieldsByString(1, "xyz"); front_pc_modfier.resize(front_count); behind_pc_modfier.resize(behind_count); sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); - for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); in_iter != in_iter.end(); ++in_iter) { + for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); + in_iter != in_iter.end(); ++in_iter) { if (*in_iter < 0.0) { *be_iter = in_iter[0]; be_iter[1] = in_iter[1]; From 136672747943af7b774af00ed688174b8083059b Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Mon, 5 Feb 2024 18:31:37 +0900 Subject: [PATCH 3/7] fixed the bug and corrected the spelling mistake. Signed-off-by: Taiga Takano --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 44 ++++++++++--------- 1 file changed, 24 insertions(+), 20 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index f5e96d52efee0..e2acd36f65d37 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -120,16 +120,18 @@ void RadiusSearch2dFilter::filter( { const auto & xyz_cloud = input; pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); - xy_cloud->points.resize(xyz_cloud.points.size()); - for (size_t i = 0; i < xyz_cloud.points.size(); ++i) { + auto xyz_cloud_points_size = xyz_cloud.points.size(); + xy_cloud->points.resize(xyz_cloud_points_size); + for (size_t i = 0; i < xyz_cloud_points_size; ++i) { xy_cloud->points[i].x = xyz_cloud.points[i].x; xy_cloud->points[i].y = xyz_cloud.points[i].y; } - std::vector k_indices(xy_cloud->points.size()); - std::vector k_distances(xy_cloud->points.size()); + auto xy_cloud_points_size = xy_cloud->points.size(); + std::vector k_indices(xy_cloud_points_size); + std::vector k_distances(xy_cloud_points_size); kd_tree_->setInputCloud(xy_cloud); - for (size_t i = 0; i < xy_cloud->points.size(); ++i) { + 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( @@ -150,8 +152,9 @@ void RadiusSearch2dFilter::filter( const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier) { + auto low_conf_input_points_size = low_conf_input.points.size(); // check the limit points number - if (low_conf_input.points.size() > max_filter_points_nb_) { + if (low_conf_input_points_size > max_filter_points_nb_) { RCLCPP_WARN( rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"), "Skip outlier filter since too much low_confidence pointcloud!"); @@ -159,8 +162,8 @@ void RadiusSearch2dFilter::filter( } pcl::PointCloud::Ptr low_conf_xy_cloud(new pcl::PointCloud); - low_conf_xy_cloud->points.resize(low_conf_input.points.size()); - for (size_t i = 0; i < low_conf_input.points.size(); ++i) { + low_conf_xy_cloud->points.resize(low_conf_input_points_size); + for (size_t i = 0; i < low_conf_input_points_size; ++i) { low_conf_xy_cloud->points[i].x = low_conf_input.points[i].x; low_conf_xy_cloud->points[i].y = low_conf_input.points[i].y; } @@ -169,7 +172,7 @@ void RadiusSearch2dFilter::filter( std::vector k_indices_low(low_conf_xy_cloud->points.size()); std::vector k_distances_low(low_conf_xy_cloud->points.size()); kd_tree_->setInputCloud(low_conf_xy_cloud); - for (size_t i = 0; i < low_conf_input.points.size(); ++i) { + for (size_t i = 0; i < low_conf_input_points_size; ++i) { const float distance = std::hypot( low_conf_xy_cloud->points[i].x - pose.position.x, low_conf_xy_cloud->points[i].y - pose.position.y); @@ -184,9 +187,9 @@ void RadiusSearch2dFilter::filter( outlier.points.push_back(low_conf_input.points.at(i)); } } - + auto outlier_points_size = outlier.points.size(); // High conf cloud check - if (outlier.points.size() == 0) { + if (outlier_points_size == 0) { return; } @@ -200,7 +203,7 @@ void RadiusSearch2dFilter::filter( std::vector k_indices_high(high_conf_xy_cloud->points.size()); std::vector k_distances_high(high_conf_xy_cloud->points.size()); kd_tree_->setInputCloud(high_conf_xy_cloud); - for (size_t i = 0; i < outlier.points.size(); ++i) { + for (size_t i = 0; i < outlier_points_size; ++i) { const float distance = std::hypot( high_conf_xy_cloud->points[i].x - pose.position.x, high_conf_xy_cloud->points[i].y - pose.position.y); @@ -212,10 +215,11 @@ void RadiusSearch2dFilter::filter( if (min_points_threshold <= points_num) { output.points.push_back(outlier.points.at(i)); - } else { - outlier.points.erase(outlier.points.begin() + i); + outlier.points.push_back(outlier.points.at(i)); } } + + outlier.points.erase(outlier.points.begin(),outlier.points.begin() + outlier_points_size); } OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( @@ -277,12 +281,12 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( } } - sensor_msgs::PointCloud2Modifier front_pc_modfier(front_pc); - sensor_msgs::PointCloud2Modifier behind_pc_modfier(behind_pc); - front_pc_modfier.setPointCloud2FieldsByString(1, "xyz"); - behind_pc_modfier.setPointCloud2FieldsByString(1, "xyz"); - front_pc_modfier.resize(front_count); - behind_pc_modfier.resize(behind_count); + sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc); + sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc); + front_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + front_pc_modifier.resize(front_count); + behind_pc_modifier.resize(behind_count); sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); From 2caa5807d214881cfbd198b2cd1ccc22e24c66ab Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 5 Feb 2024 09:33:37 +0000 Subject: [PATCH 4/7] style(pre-commit): autofix Signed-off-by: Taiga Takano --- .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index e2acd36f65d37..51e88277214f3 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -219,7 +219,7 @@ void RadiusSearch2dFilter::filter( } } - outlier.points.erase(outlier.points.begin(),outlier.points.begin() + outlier_points_size); + outlier.points.erase(outlier.points.begin(), outlier.points.begin() + outlier_points_size); } OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( From 5689a1f30830465e7ed0aa2274a9b040c67f2985 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Mon, 5 Feb 2024 19:02:12 +0900 Subject: [PATCH 5/7] fix bug Signed-off-by: Taiga Takano --- .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 51e88277214f3..1f36889201d9a 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -215,7 +215,8 @@ void RadiusSearch2dFilter::filter( if (min_points_threshold <= points_num) { output.points.push_back(outlier.points.at(i)); - outlier.points.push_back(outlier.points.at(i)); + }else{ + outlier.points.push_back(outlier.points.at(i)); } } From 2bfc2b392946cdf619aac447cd892718592649f6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 5 Feb 2024 10:04:07 +0000 Subject: [PATCH 6/7] style(pre-commit): autofix Signed-off-by: Taiga Takano --- .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 1f36889201d9a..2351ab4f99301 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -215,8 +215,8 @@ void RadiusSearch2dFilter::filter( if (min_points_threshold <= points_num) { output.points.push_back(outlier.points.at(i)); - }else{ - outlier.points.push_back(outlier.points.at(i)); + } else { + outlier.points.push_back(outlier.points.at(i)); } } From 68b16b667cf94794579e3f5f68db6f7c4b730776 Mon Sep 17 00:00:00 2001 From: Taiga Takano Date: Sun, 18 Feb 2024 22:16:04 +0900 Subject: [PATCH 7/7] change filter Signed-off-by: Taiga Takano --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 84 ++++++------------- 1 file changed, 26 insertions(+), 58 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 2351ab4f99301..78f76d4f980c3 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -120,25 +120,23 @@ void RadiusSearch2dFilter::filter( { const auto & xyz_cloud = input; pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); - auto xyz_cloud_points_size = xyz_cloud.points.size(); - xy_cloud->points.resize(xyz_cloud_points_size); - for (size_t i = 0; i < xyz_cloud_points_size; ++i) { + xy_cloud->points.resize(xyz_cloud.points.size()); + for (size_t i = 0; i < xyz_cloud.points.size(); ++i) { xy_cloud->points[i].x = xyz_cloud.points[i].x; xy_cloud->points[i].y = xyz_cloud.points[i].y; } - auto xy_cloud_points_size = xy_cloud->points.size(); - std::vector k_indices(xy_cloud_points_size); - std::vector k_distances(xy_cloud_points_size); + std::vector k_indices(xy_cloud->points.size()); + std::vector k_distances(xy_cloud->points.size()); kd_tree_->setInputCloud(xy_cloud); - for (size_t i = 0; i < xy_cloud_points_size; ++i) { + 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(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), max_points_); const int points_num = - kd_tree_->nearestKSearch(i, min_points_threshold, k_indices, k_distances); + kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold); if (min_points_threshold <= points_num) { output.points.push_back(xyz_cloud.points.at(i)); @@ -152,75 +150,45 @@ void RadiusSearch2dFilter::filter( const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier) { - auto low_conf_input_points_size = low_conf_input.points.size(); + const auto & high_conf_xyz_cloud = high_conf_input; + const auto & low_conf_xyz_cloud = low_conf_input; // check the limit points number - if (low_conf_input_points_size > max_filter_points_nb_) { + if (low_conf_xyz_cloud.points.size() > max_filter_points_nb_) { RCLCPP_WARN( rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"), "Skip outlier filter since too much low_confidence pointcloud!"); return; } - pcl::PointCloud::Ptr low_conf_xy_cloud(new pcl::PointCloud); - low_conf_xy_cloud->points.resize(low_conf_input_points_size); - for (size_t i = 0; i < low_conf_input_points_size; ++i) { - low_conf_xy_cloud->points[i].x = low_conf_input.points[i].x; - low_conf_xy_cloud->points[i].y = low_conf_input.points[i].y; - } - - // Low conf cloud check - std::vector k_indices_low(low_conf_xy_cloud->points.size()); - std::vector k_distances_low(low_conf_xy_cloud->points.size()); - kd_tree_->setInputCloud(low_conf_xy_cloud); - for (size_t i = 0; i < low_conf_input_points_size; ++i) { - const float distance = std::hypot( - low_conf_xy_cloud->points[i].x - pose.position.x, - low_conf_xy_cloud->points[i].y - pose.position.y); - const int min_points_threshold = std::min( - std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), - max_points_); - const int points_num = - kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_low, k_distances_low); - if (min_points_threshold <= points_num) { - output.points.push_back(low_conf_input.points.at(i)); - } else { - outlier.points.push_back(low_conf_input.points.at(i)); - } - } - auto outlier_points_size = outlier.points.size(); - // High conf cloud check - if (outlier_points_size == 0) { - return; + pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); + 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; } - - pcl::PointCloud::Ptr high_conf_xy_cloud(new pcl::PointCloud); - high_conf_xy_cloud->points.resize(high_conf_input.points.size()); - for (size_t i = 0; i < high_conf_input.points.size(); ++i) { - high_conf_xy_cloud->points[i].x = high_conf_input.points[i].x; - high_conf_xy_cloud->points[i].y = high_conf_input.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; } - std::vector k_indices_high(high_conf_xy_cloud->points.size()); - std::vector k_distances_high(high_conf_xy_cloud->points.size()); - kd_tree_->setInputCloud(high_conf_xy_cloud); - for (size_t i = 0; i < outlier_points_size; ++i) { - const float distance = std::hypot( - high_conf_xy_cloud->points[i].x - pose.position.x, - high_conf_xy_cloud->points[i].y - pose.position.y); + std::vector k_indices(xy_cloud->points.size()); + std::vector k_distances(xy_cloud->points.size()); + kd_tree_->setInputCloud(xy_cloud); + for (size_t i = 0; i < low_conf_xyz_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(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), max_points_); const int points_num = - kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_high, k_distances_high); + kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold); if (min_points_threshold <= points_num) { - output.points.push_back(outlier.points.at(i)); + output.points.push_back(low_conf_xyz_cloud.points.at(i)); } else { - outlier.points.push_back(outlier.points.at(i)); + outlier.points.push_back(low_conf_xyz_cloud.points.at(i)); } } - - outlier.points.erase(outlier.points.begin(), outlier.points.begin() + outlier_points_size); } OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(