From 1afed951e9725876b1c4fffd6db3a2b00c89f50f Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 10 Oct 2023 11:32:55 +0900 Subject: [PATCH 1/9] fix: add 3d validation bind function Signed-off-by: badai-nguyen --- .../debugger.hpp | 6 + .../obstacle_pointcloud_based_validator.hpp | 7 + .../obstacle_pointcloud_based_validator.cpp | 133 +++++++++++++++++- 3 files changed, 139 insertions(+), 7 deletions(-) diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp index 9ef1f75427b65..7b6cf91997682 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp @@ -76,6 +76,12 @@ class Debugger } } + void addNeighborPointcloud(const pcl::PointCloud::Ptr & input) + { + for (const auto & point : *input) { + neighbor_pointcloud_->push_back(point); + } + } void addPointcloudWithinPolygon(const pcl::PointCloud::Ptr & input) { // pcl::PointCloud::Ptr input_xyz = toXYZ(input); diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 5819302664907..3970f9935b5a4 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -61,14 +61,21 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node PointsNumThresholdParam points_num_threshold_param_; std::shared_ptr debugger_; + bool using_2d_validator_; private: void onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); + void on3dObjectsAndObstaclePointCloud( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); std::optional getPointCloudNumWithinPolygon( const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud); + std::optional getPointCloudNumWithinShape( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr pointcloud); std::optional getMaxRadius( const autoware_auto_perception_msgs::msg::DetectedObject & object); }; diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 47640c9332e4d..cfde74d64a8bd 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -85,13 +85,6 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( tf_listener_(tf_buffer_), sync_(SyncPolicy(10), objects_sub_, obstacle_pointcloud_sub_) { - using std::placeholders::_1; - using std::placeholders::_2; - sync_.registerCallback( - std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); - objects_pub_ = create_publisher( - "~/output/objects", rclcpp::QoS{1}); - points_num_threshold_param_.min_points_num = declare_parameter>("min_points_num"); points_num_threshold_param_.max_points_num = @@ -99,10 +92,101 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( points_num_threshold_param_.min_points_and_distance_ratio = declare_parameter>("min_points_and_distance_ratio"); + using_2d_validator_ = declare_parameter("using_2d_validator"); + + using std::placeholders::_1; + using std::placeholders::_2; + + if (using_2d_validator_) { + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); + } else { + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud, this, _1, _2)); + } + + objects_pub_ = create_publisher( + "~/output/objects", rclcpp::QoS{1}); + const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); } +void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) +{ + autoware_auto_perception_msgs::msg::DetectedObjects output, removed_objects; + output.header = input_objects->header; + removed_objects.header = input_objects->header; + + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!object_recognition_utils::transformObjects( + *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_, + transformed_objects)) { + return; + } + + // Convert to PCL + pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); + if (obstacle_pointcloud->empty()) { + return; + } + // Create Kd-tree to search neighbor pointcloud to reduce cost + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + kdtree->setInputCloud(obstacle_pointcloud); + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; + const auto & object = input_objects->objects.at(i); + const auto & transformed_object_position = + transformed_object.kinematics.pose_with_covariance.pose.position; + const auto search_radius = getMaxRadius(transformed_object); + if (!search_radius) { + output.objects.push_back(object); + continue; + } + // Search neighbor pointcloud to reduce cost + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + pcl::PointXYZ( + transformed_object_position.x, transformed_object_position.y, + transformed_object_position.z), + search_radius.value(), indices, distances); + for (const auto & indice : indices) { + neighbor_pointcloud->push_back(obstacle_pointcloud->at(indice)); + } + + if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); + // Filter object that have few pointcloud in them + const auto num = getPointCloudNumWithinShape(transformed_object, neighbor_pointcloud); + const auto object_distance = + std::hypot(transformed_object_position.x, transformed_object_position.y); + size_t min_pointcloud_num = std::clamp( + static_cast( + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); + if (num) { + (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) + : removed_objects.objects.push_back(object); + } else { + output.objects.push_back(object); + } + objects_pub_->publish(output); + if (debugger_) { + debugger_->publishRemovedObjects(removed_objects); + debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); + } + } +} void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) @@ -184,6 +268,41 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); } } +std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinShape( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr pointcloud) +{ + pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); + std::vector vertices_array; + pcl::Vertices vertices; + + auto const & object_position = object.kinematics.pose_with_covariance.pose.position; + auto const object_height = object.shape.dimensions.z; + float z_min = object_position.z - object_height / 2.0f; + float z_max = object_position.z + object_height / 2.0f; + + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + if (bg::is_empty(poly2d)) return std::nullopt; + + pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + + for (size_t i = 0; i < poly2d.outer().size(); ++i) { + vertices.vertices.emplace_back(i); + vertices_array.emplace_back(vertices); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), z_min); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), z_max); + } + pcl::CropHull cropper; + cropper.setInputCloud(pointcloud); + cropper.setDim(3); + cropper.setHullIndices(vertices_array); + cropper.setHullCloud(poly3d); + cropper.setCropOutside(true); + cropper.filter(*cropped_pointcloud); + if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); + return cropped_pointcloud->size(); +} std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon( const autoware_auto_perception_msgs::msg::DetectedObject & object, From c0bca36cbb814725e6e3c72353676d1f799f8301 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 17 Oct 2023 10:43:16 +0900 Subject: [PATCH 2/9] fix: check validation point within shape Signed-off-by: badai-nguyen --- .../obstacle_pointcloud_based_validator.hpp | 4 +- .../obstacle_pointcloud_based_validator.cpp | 63 ++++++++++++------- 2 files changed, 43 insertions(+), 24 deletions(-) diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 3970f9935b5a4..2af96fbba0b0a 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -76,7 +76,9 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::optional getPointCloudNumWithinShape( const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud); - std::optional getMaxRadius( + std::optional getMaxRadius2D( + const autoware_auto_perception_msgs::msg::DetectedObject & object); + std::optional getMaxRadius3D( const autoware_auto_perception_msgs::msg::DetectedObject & object); }; } // namespace obstacle_pointcloud_based_validator diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index cfde74d64a8bd..8994c52a44bb1 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -144,7 +144,7 @@ void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( const auto & object = input_objects->objects.at(i); const auto & transformed_object_position = transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius(transformed_object); + const auto search_radius = getMaxRadius3D(transformed_object); if (!search_radius) { output.objects.push_back(object); continue; @@ -153,11 +153,11 @@ void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); std::vector indices; std::vector distances; - kdtree->radiusSearch( - pcl::PointXYZ( - transformed_object_position.x, transformed_object_position.y, - transformed_object_position.z), - search_radius.value(), indices, distances); + pcl::PointXYZ trans_obj_position_pcl; + trans_obj_position_pcl.x = transformed_object_position.x; + trans_obj_position_pcl.y = transformed_object_position.y; + trans_obj_position_pcl.z = transformed_object_position.z; + kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances); for (const auto & indice : indices) { neighbor_pointcloud->push_back(obstacle_pointcloud->at(indice)); } @@ -180,11 +180,12 @@ void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( } else { output.objects.push_back(object); } - objects_pub_->publish(output); - if (debugger_) { - debugger_->publishRemovedObjects(removed_objects); - debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); - } + } + objects_pub_->publish(output); + if (debugger_) { + debugger_->publishRemovedObjects(removed_objects); + debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); + debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); } } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( @@ -224,7 +225,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const auto & object = input_objects->objects.at(i); const auto & transformed_object_position = transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius(transformed_object); + const auto search_radius = getMaxRadius2D(transformed_object); if (!search_radius) { output.objects.push_back(object); continue; @@ -242,7 +243,6 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); // Filter object that have few pointcloud in them. - // TODO(badai-nguyen) add 3d validator option const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); const auto object_distance = std::hypot(transformed_object_position.x, transformed_object_position.y); @@ -272,14 +272,14 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinSh const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud) { - pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cropped_pointcloud_2d(new pcl::PointCloud); std::vector vertices_array; pcl::Vertices vertices; auto const & object_position = object.kinematics.pose_with_covariance.pose.position; auto const object_height = object.shape.dimensions.z; - float z_min = object_position.z - object_height / 2.0f; - float z_max = object_position.z + object_height / 2.0f; + auto z_min = object_position.z - object_height / 2.0f; + auto z_max = object_position.z + object_height / 2.0f; Polygon2d poly2d = tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); @@ -290,18 +290,25 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinSh for (size_t i = 0; i < poly2d.outer().size(); ++i) { vertices.vertices.emplace_back(i); vertices_array.emplace_back(vertices); - poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), z_min); - poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), z_max); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); } pcl::CropHull cropper; cropper.setInputCloud(pointcloud); - cropper.setDim(3); + cropper.setDim(2); cropper.setHullIndices(vertices_array); cropper.setHullCloud(poly3d); cropper.setCropOutside(true); - cropper.filter(*cropped_pointcloud); - if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); - return cropped_pointcloud->size(); + cropper.filter(*cropped_pointcloud_2d); + + pcl::PointCloud::Ptr cropped_pointcloud_3d(new pcl::PointCloud); + cropped_pointcloud_3d->reserve(cropped_pointcloud_2d->size()); + for (const auto & point : *cropped_pointcloud_2d) { + if (point.z > z_min && point.z < z_max) { + cropped_pointcloud_3d->push_back(point); + } + } + if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud_3d); + return cropped_pointcloud_3d->size(); } std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon( @@ -336,7 +343,7 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPo return cropped_pointcloud->size(); } -std::optional ObstaclePointCloudBasedValidator::getMaxRadius( +std::optional ObstaclePointCloudBasedValidator::getMaxRadius2D( const autoware_auto_perception_msgs::msg::DetectedObject & object) { if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { @@ -353,6 +360,16 @@ std::optional ObstaclePointCloudBasedValidator::getMaxRadius( return std::nullopt; } } +std::optional ObstaclePointCloudBasedValidator::getMaxRadius3D( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const auto max_dist_2d = getMaxRadius2D(object); + if (!max_dist_2d) { + return std::nullopt; + } + const auto object_height = object.shape.dimensions.z; + return std::hypot(max_dist_2d.value(), object_height / 2.0); +} } // namespace obstacle_pointcloud_based_validator From d5b7bed8dbecf18c73d0f3b1afc6d357aa15edd8 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 16 Oct 2023 17:35:59 +0900 Subject: [PATCH 3/9] fix: add 2d validator option param Signed-off-by: badai-nguyen --- .../launch/obstacle_pointcloud_based_validator.launch.xml | 1 + .../src/obstacle_pointcloud_based_validator.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 799b605658345..d661e7db33735 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -11,5 +11,6 @@ + diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 8994c52a44bb1..eff63face1304 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -158,8 +158,8 @@ void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( trans_obj_position_pcl.y = transformed_object_position.y; trans_obj_position_pcl.z = transformed_object_position.z; kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances); - for (const auto & indice : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(indice)); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); } if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); From e3cbf6cb524a34ad3b693a8c0eaa1680e3141f72 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 18 Oct 2023 08:33:02 +0900 Subject: [PATCH 4/9] chore: refactor Signed-off-by: badai-nguyen --- .../obstacle_pointcloud_based_validator.hpp | 11 +- .../obstacle_pointcloud_based_validator.cpp | 298 ++++++++---------- 2 files changed, 135 insertions(+), 174 deletions(-) diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 2af96fbba0b0a..4a0cc496483b5 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -35,6 +35,9 @@ #include namespace obstacle_pointcloud_based_validator { +using Shape = autoware_auto_perception_msgs::msg::Shape; +using Polygon2d = tier4_autoware_utils::Polygon2d; + struct PointsNumThresholdParam { std::vector min_points_num; @@ -67,9 +70,11 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node void onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); - void on3dObjectsAndObstaclePointCloud( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); + void cropPointsInsidePolygon( + const pcl::PointCloud::Ptr & input_points, const Polygon2d & poly2d, + pcl::PointCloud::Ptr & output_points); + size_t getValidationThreshold( + const geometry_msgs::msg::Point & transformed_object_position, const uint8_t object_label_id); std::optional getPointCloudNumWithinPolygon( const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud); diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index eff63face1304..5fa2a5d5d51b2 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -71,8 +71,6 @@ inline pcl::PointCloud::Ptr toXYZ( namespace obstacle_pointcloud_based_validator { namespace bg = boost::geometry; -using Shape = autoware_auto_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const rclcpp::NodeOptions & node_options) @@ -97,97 +95,14 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( using std::placeholders::_1; using std::placeholders::_2; - if (using_2d_validator_) { - sync_.registerCallback( - std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); - } else { - sync_.registerCallback( - std::bind(&ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud, this, _1, _2)); - } - + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); } -void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) -{ - autoware_auto_perception_msgs::msg::DetectedObjects output, removed_objects; - output.header = input_objects->header; - removed_objects.header = input_objects->header; - - // Transform to pointcloud frame - autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( - *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_, - transformed_objects)) { - return; - } - - // Convert to PCL - pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); - if (obstacle_pointcloud->empty()) { - return; - } - - // Create Kd-tree to search neighbor pointcloud to reduce cost - pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); - kdtree->setInputCloud(obstacle_pointcloud); - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { - const auto & transformed_object = transformed_objects.objects.at(i); - const auto object_label_id = transformed_object.classification.front().label; - const auto & object = input_objects->objects.at(i); - const auto & transformed_object_position = - transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius3D(transformed_object); - if (!search_radius) { - output.objects.push_back(object); - continue; - } - // Search neighbor pointcloud to reduce cost - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - pcl::PointXYZ trans_obj_position_pcl; - trans_obj_position_pcl.x = transformed_object_position.x; - trans_obj_position_pcl.y = transformed_object_position.y; - trans_obj_position_pcl.z = transformed_object_position.z; - kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances); - for (const auto & index : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); - } - - if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); - // Filter object that have few pointcloud in them - const auto num = getPointCloudNumWithinShape(transformed_object, neighbor_pointcloud); - const auto object_distance = - std::hypot(transformed_object_position.x, transformed_object_position.y); - size_t min_pointcloud_num = std::clamp( - static_cast( - points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / - object_distance + - 0.5f), - static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), - static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); - if (num) { - (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); - } else { - output.objects.push_back(object); - } - } - objects_pub_->publish(output); - if (debugger_) { - debugger_->publishRemovedObjects(removed_objects); - debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); - debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); - } -} void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) @@ -206,61 +121,102 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } // Convert to PCL - pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); - if (obstacle_pointcloud->empty()) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud"); - // objects_pub_->publish(*input_objects); - return; - } - // Create Kd-tree to search neighbor pointcloud to reduce cost. - pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); - kdtree->setInputCloud(obstacle_pointcloud); - - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { - const auto & transformed_object = transformed_objects.objects.at(i); - const auto object_label_id = transformed_object.classification.front().label; - const auto & object = input_objects->objects.at(i); - const auto & transformed_object_position = - transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius2D(transformed_object); - if (!search_radius) { - output.objects.push_back(object); - continue; + if (using_2d_validator_) { + pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); + if (obstacle_pointcloud->empty()) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud"); + // objects_pub_->publish(*input_objects); + return; } - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(transformed_object_position), search_radius.value(), indices, distances); - for (const auto & index : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); + // Create Kd-tree to search neighbor pointcloud to reduce cost. + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + kdtree->setInputCloud(obstacle_pointcloud); + + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; + const auto & object = input_objects->objects.at(i); + const auto & transformed_object_position = + transformed_object.kinematics.pose_with_covariance.pose.position; + const auto search_radius = getMaxRadius2D(transformed_object); + if (!search_radius) { + output.objects.push_back(object); + continue; + } + + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(transformed_object_position), search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); + } + if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); + + // Filter object that have few pointcloud in them. + const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); + auto validation_threshold = + getValidationThreshold(transformed_object_position, object_label_id); + if (num) { + (validation_threshold <= num.value()) ? output.objects.push_back(object) + : removed_objects.objects.push_back(object); + } else { + output.objects.push_back(object); + } } - if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); - - // Filter object that have few pointcloud in them. - const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); - const auto object_distance = - std::hypot(transformed_object_position.x, transformed_object_position.y); - size_t min_pointcloud_num = std::clamp( - static_cast( - points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / - object_distance + - 0.5f), - static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), - static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); - if (num) { - (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); - } else { - output.objects.push_back(object); + } else { + // Convert to PCL + pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); + if (obstacle_pointcloud->empty()) { + return; } - } + // Create Kd-tree to search neighbor pointcloud to reduce cost + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + kdtree->setInputCloud(obstacle_pointcloud); + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; + const auto & object = input_objects->objects.at(i); + const auto & transformed_object_position = + transformed_object.kinematics.pose_with_covariance.pose.position; + const auto search_radius = getMaxRadius3D(transformed_object); + if (!search_radius) { + output.objects.push_back(object); + continue; + } + // Search neighbor pointcloud to reduce cost + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + pcl::PointXYZ trans_obj_position_pcl = pcl::PointXYZ( + transformed_object_position.x, transformed_object_position.y, + transformed_object_position.z); + kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); + } + if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); + // Filter object that have few pointcloud in them + const auto num = getPointCloudNumWithinShape(transformed_object, neighbor_pointcloud); + auto validation_threshold = + getValidationThreshold(transformed_object_position, object_label_id); + if (num) { + (validation_threshold <= num.value()) ? output.objects.push_back(object) + : removed_objects.objects.push_back(object); + } else { + output.objects.push_back(object); + } + } + } objects_pub_->publish(output); if (debugger_) { debugger_->publishRemovedObjects(removed_objects); @@ -272,34 +228,17 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinSh const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud) { - pcl::PointCloud::Ptr cropped_pointcloud_2d(new pcl::PointCloud); - std::vector vertices_array; - pcl::Vertices vertices; + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + if (bg::is_empty(poly2d)) return std::nullopt; + pcl::PointCloud::Ptr cropped_pointcloud_2d(new pcl::PointCloud); + cropPointsInsidePolygon(pointcloud, poly2d, cropped_pointcloud_2d); auto const & object_position = object.kinematics.pose_with_covariance.pose.position; auto const object_height = object.shape.dimensions.z; auto z_min = object_position.z - object_height / 2.0f; auto z_max = object_position.z + object_height / 2.0f; - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); - if (bg::is_empty(poly2d)) return std::nullopt; - - pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); - - for (size_t i = 0; i < poly2d.outer().size(); ++i) { - vertices.vertices.emplace_back(i); - vertices_array.emplace_back(vertices); - poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); - } - pcl::CropHull cropper; - cropper.setInputCloud(pointcloud); - cropper.setDim(2); - cropper.setHullIndices(vertices_array); - cropper.setHullCloud(poly3d); - cropper.setCropOutside(true); - cropper.filter(*cropped_pointcloud_2d); - pcl::PointCloud::Ptr cropped_pointcloud_3d(new pcl::PointCloud); cropped_pointcloud_3d->reserve(cropped_pointcloud_2d->size()); for (const auto & point : *cropped_pointcloud_2d) { @@ -315,34 +254,36 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPo const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud) { - pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); - std::vector vertices_array; - pcl::Vertices vertices; - Polygon2d poly2d = tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; - pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); + cropPointsInsidePolygon(toXYZ(pointcloud), poly2d, cropped_pointcloud); + if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); + return cropped_pointcloud->size(); +} +void ObstaclePointCloudBasedValidator::cropPointsInsidePolygon( + const pcl::PointCloud::Ptr & input_points, const Polygon2d & poly2d, + pcl::PointCloud::Ptr & output_points) +{ + std::vector vertices_array; + pcl::Vertices vertices; + pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); for (size_t i = 0; i < poly2d.outer().size(); ++i) { vertices.vertices.emplace_back(i); vertices_array.emplace_back(vertices); poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); } - - pcl::CropHull cropper; // don't be implemented PointXY by PCL - cropper.setInputCloud(toXYZ(pointcloud)); + pcl::CropHull cropper; + cropper.setInputCloud(input_points); cropper.setDim(2); cropper.setHullIndices(vertices_array); cropper.setHullCloud(poly3d); cropper.setCropOutside(true); - cropper.filter(*cropped_pointcloud); - - if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); - return cropped_pointcloud->size(); + cropper.filter(*output_points); } - std::optional ObstaclePointCloudBasedValidator::getMaxRadius2D( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -371,6 +312,21 @@ std::optional ObstaclePointCloudBasedValidator::getMaxRadius3D( return std::hypot(max_dist_2d.value(), object_height / 2.0); } +size_t ObstaclePointCloudBasedValidator::getValidationThreshold( + const geometry_msgs::msg::Point & transformed_object_position, const uint8_t object_label_id) +{ + const auto object_distance = + std::hypot(transformed_object_position.x, transformed_object_position.y); + size_t min_pointcloud_num = std::clamp( + static_cast( + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); + return min_pointcloud_num; +} + } // namespace obstacle_pointcloud_based_validator #include From c3317b63547472675d09e7c0d27597d8b7665749 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 18 Oct 2023 11:15:46 +0900 Subject: [PATCH 5/9] chore: refactor Signed-off-by: badai-nguyen --- .../obstacle_pointcloud_based_validator/debugger.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp index 7b6cf91997682..6597475ae297e 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp @@ -71,15 +71,16 @@ class Debugger void addNeighborPointcloud(const pcl::PointCloud::Ptr & input) { pcl::PointCloud::Ptr input_xyz = toXYZ(input); - for (const auto & point : *input_xyz) { - neighbor_pointcloud_->push_back(point); - } + addNeighborPointcloud(input_xyz); } void addNeighborPointcloud(const pcl::PointCloud::Ptr & input) { - for (const auto & point : *input) { - neighbor_pointcloud_->push_back(point); + if (!input->empty()) { + neighbor_pointcloud_->reserve(neighbor_pointcloud_->size() + input->size()); + for (const auto & point : *input) { + neighbor_pointcloud_->push_back(point); + } } } void addPointcloudWithinPolygon(const pcl::PointCloud::Ptr & input) From e3389ee544d4e47730e922b3fe5f971bec7aa69d Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 30 Oct 2023 18:19:11 +0900 Subject: [PATCH 6/9] Revert "chore: refactor" This reverts commit e3cbf6cb524a34ad3b693a8c0eaa1680e3141f72. --- .../obstacle_pointcloud_based_validator.hpp | 11 +- .../obstacle_pointcloud_based_validator.cpp | 298 ++++++++++-------- 2 files changed, 174 insertions(+), 135 deletions(-) diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 4a0cc496483b5..2af96fbba0b0a 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -35,9 +35,6 @@ #include namespace obstacle_pointcloud_based_validator { -using Shape = autoware_auto_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; - struct PointsNumThresholdParam { std::vector min_points_num; @@ -70,11 +67,9 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node void onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); - void cropPointsInsidePolygon( - const pcl::PointCloud::Ptr & input_points, const Polygon2d & poly2d, - pcl::PointCloud::Ptr & output_points); - size_t getValidationThreshold( - const geometry_msgs::msg::Point & transformed_object_position, const uint8_t object_label_id); + void on3dObjectsAndObstaclePointCloud( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); std::optional getPointCloudNumWithinPolygon( const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud); diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 5fa2a5d5d51b2..eff63face1304 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -71,6 +71,8 @@ inline pcl::PointCloud::Ptr toXYZ( namespace obstacle_pointcloud_based_validator { namespace bg = boost::geometry; +using Shape = autoware_auto_perception_msgs::msg::Shape; +using Polygon2d = tier4_autoware_utils::Polygon2d; ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const rclcpp::NodeOptions & node_options) @@ -95,15 +97,21 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( using std::placeholders::_1; using std::placeholders::_2; - sync_.registerCallback( - std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); + if (using_2d_validator_) { + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); + } else { + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud, this, _1, _2)); + } + objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); } -void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( +void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) { @@ -116,107 +124,143 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( if (!object_recognition_utils::transformObjects( *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_, transformed_objects)) { - // objects_pub_->publish(*input_objects); return; } // Convert to PCL + pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); + if (obstacle_pointcloud->empty()) { + return; + } - if (using_2d_validator_) { - pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); - if (obstacle_pointcloud->empty()) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud"); - // objects_pub_->publish(*input_objects); - return; + // Create Kd-tree to search neighbor pointcloud to reduce cost + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + kdtree->setInputCloud(obstacle_pointcloud); + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; + const auto & object = input_objects->objects.at(i); + const auto & transformed_object_position = + transformed_object.kinematics.pose_with_covariance.pose.position; + const auto search_radius = getMaxRadius3D(transformed_object); + if (!search_radius) { + output.objects.push_back(object); + continue; + } + // Search neighbor pointcloud to reduce cost + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + pcl::PointXYZ trans_obj_position_pcl; + trans_obj_position_pcl.x = transformed_object_position.x; + trans_obj_position_pcl.y = transformed_object_position.y; + trans_obj_position_pcl.z = transformed_object_position.z; + kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); } - // Create Kd-tree to search neighbor pointcloud to reduce cost. - pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); - kdtree->setInputCloud(obstacle_pointcloud); - - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { - const auto & transformed_object = transformed_objects.objects.at(i); - const auto object_label_id = transformed_object.classification.front().label; - const auto & object = input_objects->objects.at(i); - const auto & transformed_object_position = - transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius2D(transformed_object); - if (!search_radius) { - output.objects.push_back(object); - continue; - } - - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(transformed_object_position), search_radius.value(), indices, distances); - for (const auto & index : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); - } - if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); - - // Filter object that have few pointcloud in them. - const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); - auto validation_threshold = - getValidationThreshold(transformed_object_position, object_label_id); - if (num) { - (validation_threshold <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); - } else { - output.objects.push_back(object); - } + if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); + // Filter object that have few pointcloud in them + const auto num = getPointCloudNumWithinShape(transformed_object, neighbor_pointcloud); + const auto object_distance = + std::hypot(transformed_object_position.x, transformed_object_position.y); + size_t min_pointcloud_num = std::clamp( + static_cast( + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); + if (num) { + (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) + : removed_objects.objects.push_back(object); + } else { + output.objects.push_back(object); } - } else { - // Convert to PCL - pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); - if (obstacle_pointcloud->empty()) { - return; + } + objects_pub_->publish(output); + if (debugger_) { + debugger_->publishRemovedObjects(removed_objects); + debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); + debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); + } +} +void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) +{ + autoware_auto_perception_msgs::msg::DetectedObjects output, removed_objects; + output.header = input_objects->header; + removed_objects.header = input_objects->header; + + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!object_recognition_utils::transformObjects( + *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + return; + } + + // Convert to PCL + pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); + if (obstacle_pointcloud->empty()) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud"); + // objects_pub_->publish(*input_objects); + return; + } + + // Create Kd-tree to search neighbor pointcloud to reduce cost. + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + kdtree->setInputCloud(obstacle_pointcloud); + + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; + const auto & object = input_objects->objects.at(i); + const auto & transformed_object_position = + transformed_object.kinematics.pose_with_covariance.pose.position; + const auto search_radius = getMaxRadius2D(transformed_object); + if (!search_radius) { + output.objects.push_back(object); + continue; } - // Create Kd-tree to search neighbor pointcloud to reduce cost - pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); - kdtree->setInputCloud(obstacle_pointcloud); - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { - const auto & transformed_object = transformed_objects.objects.at(i); - const auto object_label_id = transformed_object.classification.front().label; - const auto & object = input_objects->objects.at(i); - const auto & transformed_object_position = - transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius3D(transformed_object); - if (!search_radius) { - output.objects.push_back(object); - continue; - } - // Search neighbor pointcloud to reduce cost - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - pcl::PointXYZ trans_obj_position_pcl = pcl::PointXYZ( - transformed_object_position.x, transformed_object_position.y, - transformed_object_position.z); - kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances); - for (const auto & index : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); - } - if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); - // Filter object that have few pointcloud in them - const auto num = getPointCloudNumWithinShape(transformed_object, neighbor_pointcloud); - auto validation_threshold = - getValidationThreshold(transformed_object_position, object_label_id); - if (num) { - (validation_threshold <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); - } else { - output.objects.push_back(object); - } + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(transformed_object_position), search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); + } + if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); + + // Filter object that have few pointcloud in them. + const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); + const auto object_distance = + std::hypot(transformed_object_position.x, transformed_object_position.y); + size_t min_pointcloud_num = std::clamp( + static_cast( + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); + if (num) { + (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) + : removed_objects.objects.push_back(object); + } else { + output.objects.push_back(object); } } + objects_pub_->publish(output); if (debugger_) { debugger_->publishRemovedObjects(removed_objects); @@ -228,17 +272,34 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinSh const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud) { - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); - if (bg::is_empty(poly2d)) return std::nullopt; - pcl::PointCloud::Ptr cropped_pointcloud_2d(new pcl::PointCloud); - cropPointsInsidePolygon(pointcloud, poly2d, cropped_pointcloud_2d); + std::vector vertices_array; + pcl::Vertices vertices; + auto const & object_position = object.kinematics.pose_with_covariance.pose.position; auto const object_height = object.shape.dimensions.z; auto z_min = object_position.z - object_height / 2.0f; auto z_max = object_position.z + object_height / 2.0f; + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + if (bg::is_empty(poly2d)) return std::nullopt; + + pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + + for (size_t i = 0; i < poly2d.outer().size(); ++i) { + vertices.vertices.emplace_back(i); + vertices_array.emplace_back(vertices); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); + } + pcl::CropHull cropper; + cropper.setInputCloud(pointcloud); + cropper.setDim(2); + cropper.setHullIndices(vertices_array); + cropper.setHullCloud(poly3d); + cropper.setCropOutside(true); + cropper.filter(*cropped_pointcloud_2d); + pcl::PointCloud::Ptr cropped_pointcloud_3d(new pcl::PointCloud); cropped_pointcloud_3d->reserve(cropped_pointcloud_2d->size()); for (const auto & point : *cropped_pointcloud_2d) { @@ -254,36 +315,34 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPo const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud) { + pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); + std::vector vertices_array; + pcl::Vertices vertices; + Polygon2d poly2d = tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; - pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); - cropPointsInsidePolygon(toXYZ(pointcloud), poly2d, cropped_pointcloud); - if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); - return cropped_pointcloud->size(); -} - -void ObstaclePointCloudBasedValidator::cropPointsInsidePolygon( - const pcl::PointCloud::Ptr & input_points, const Polygon2d & poly2d, - pcl::PointCloud::Ptr & output_points) -{ - std::vector vertices_array; - pcl::Vertices vertices; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + for (size_t i = 0; i < poly2d.outer().size(); ++i) { vertices.vertices.emplace_back(i); vertices_array.emplace_back(vertices); poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); } - pcl::CropHull cropper; - cropper.setInputCloud(input_points); + + pcl::CropHull cropper; // don't be implemented PointXY by PCL + cropper.setInputCloud(toXYZ(pointcloud)); cropper.setDim(2); cropper.setHullIndices(vertices_array); cropper.setHullCloud(poly3d); cropper.setCropOutside(true); - cropper.filter(*output_points); + cropper.filter(*cropped_pointcloud); + + if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); + return cropped_pointcloud->size(); } + std::optional ObstaclePointCloudBasedValidator::getMaxRadius2D( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -312,21 +371,6 @@ std::optional ObstaclePointCloudBasedValidator::getMaxRadius3D( return std::hypot(max_dist_2d.value(), object_height / 2.0); } -size_t ObstaclePointCloudBasedValidator::getValidationThreshold( - const geometry_msgs::msg::Point & transformed_object_position, const uint8_t object_label_id) -{ - const auto object_distance = - std::hypot(transformed_object_position.x, transformed_object_position.y); - size_t min_pointcloud_num = std::clamp( - static_cast( - points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / - object_distance + - 0.5f), - static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), - static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); - return min_pointcloud_num; -} - } // namespace obstacle_pointcloud_based_validator #include From 7ad22a7f5553fd95d6574202c16468349019a68a Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 30 Oct 2023 19:03:33 +0900 Subject: [PATCH 7/9] chore: update docs and param file Signed-off-by: badai-nguyen --- .../config/obstacle_pointcloud_based_validator.param.yaml | 2 ++ .../launch/obstacle_pointcloud_based_validator.launch.xml | 1 - .../obstacle-pointcloud-based-validator.md | 1 + .../src/obstacle_pointcloud_based_validator.cpp | 1 + 4 files changed, 4 insertions(+), 1 deletion(-) diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index 745a0fd6591a9..f7a27a52bfa8a 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -11,3 +11,5 @@ min_points_and_distance_ratio: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] + + using_2d_validator: false diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index d661e7db33735..799b605658345 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -11,6 +11,5 @@ - diff --git a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md index dd67aab3db0c9..36b3815e7e689 100644 --- a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md +++ b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md @@ -28,6 +28,7 @@ In the debug image above, the red DetectedObject is the validated object. The bl | Name | Type | Description | | ------------------------------- | ----- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `using_2d_validator` | bool | The xy-plane projected (2D) obstacle point clouds will be used for validation | | `min_points_num` | int | The minimum number of obstacle point clouds in DetectedObjects | | `max_points_num` | int | The max number of obstacle point clouds in DetectedObjects | | `min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index eff63face1304..db149a8b82ed2 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -97,6 +97,7 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( using std::placeholders::_1; using std::placeholders::_2; + // TODO(badai-nguyen): change to single bind function if (using_2d_validator_) { sync_.registerCallback( std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); From 731e768c7065357080abccdeeaca66a2b9f996b6 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 13 Nov 2023 11:46:18 +0900 Subject: [PATCH 8/9] refactor: change to abstract class Signed-off-by: badai-nguyen --- .../obstacle_pointcloud_based_validator.hpp | 95 +++- .../obstacle_pointcloud_based_validator.cpp | 490 +++++++++--------- 2 files changed, 317 insertions(+), 268 deletions(-) diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 2af96fbba0b0a..e7df2c409b18f 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -25,8 +25,13 @@ #include #include #include +#include +#include #include #include +#include +#include +#include #include #include @@ -35,12 +40,88 @@ #include namespace obstacle_pointcloud_based_validator { + struct PointsNumThresholdParam { std::vector min_points_num; std::vector max_points_num; std::vector min_points_and_distance_ratio; }; + +class Validator +{ +private: + PointsNumThresholdParam points_num_threshold_param_; + +protected: + pcl::PointCloud::Ptr cropped_pointcloud_; + +public: + explicit Validator(PointsNumThresholdParam & points_num_threshold_param); + inline pcl::PointCloud::Ptr getDebugPointCloudWithinObject() + { + return cropped_pointcloud_; + } + + virtual bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud) = 0; + virtual bool validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object) = 0; + virtual std::optional getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object) = 0; + size_t getThresholdPointCloud(const autoware_auto_perception_msgs::msg::DetectedObject & object); + virtual pcl::PointCloud::Ptr getDebugNeighborPointCloud() = 0; +}; + +class Validator2D : public Validator +{ +private: + pcl::PointCloud::Ptr obstacle_pointcloud_; + pcl::PointCloud::Ptr neighbor_pointcloud_; + pcl::search::Search::Ptr kdtree_; + +public: + explicit Validator2D(PointsNumThresholdParam & points_num_threshold_param); + + pcl::PointCloud::Ptr convertToXYZ( + const pcl::PointCloud::Ptr & pointcloud_xy); + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + { + return convertToXYZ(neighbor_pointcloud_); + } + + bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); + bool validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object); + std::optional getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object); + std::optional getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr neighbor_pointcloud); +}; +class Validator3D : public Validator +{ +private: + pcl::PointCloud::Ptr obstacle_pointcloud_; + pcl::PointCloud::Ptr neighbor_pointcloud_; + pcl::search::Search::Ptr kdtree_; + +public: + explicit Validator3D(PointsNumThresholdParam & points_num_threshold_param); + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + { + return neighbor_pointcloud_; + } + bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); + bool validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object); + std::optional getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object); + std::optional getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr neighbor_pointcloud); +}; + class ObstaclePointCloudBasedValidator : public rclcpp::Node { public: @@ -62,24 +143,12 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; + std::unique_ptr validator_; private: void onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); - void on3dObjectsAndObstaclePointCloud( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); - std::optional getPointCloudNumWithinPolygon( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr pointcloud); - std::optional getPointCloudNumWithinShape( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr pointcloud); - std::optional getMaxRadius2D( - const autoware_auto_perception_msgs::msg::DetectedObject & object); - std::optional getMaxRadius3D( - const autoware_auto_perception_msgs::msg::DetectedObject & object); }; } // namespace obstacle_pointcloud_based_validator diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index db149a8b82ed2..3249581513dd9 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -19,12 +19,6 @@ #include -#include -#include -#include -#include -#include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -35,44 +29,247 @@ #include #include -namespace +namespace obstacle_pointcloud_based_validator { -inline pcl::PointXY toPCL(const double x, const double y) +namespace bg = boost::geometry; +using Shape = autoware_auto_perception_msgs::msg::Shape; +using Polygon2d = tier4_autoware_utils::Polygon2d; + +Validator::Validator(PointsNumThresholdParam & points_num_threshold_param) { - pcl::PointXY pcl_point; - pcl_point.x = x; - pcl_point.y = y; - return pcl_point; + points_num_threshold_param_.min_points_num = points_num_threshold_param.min_points_num; + points_num_threshold_param_.max_points_num = points_num_threshold_param.max_points_num; + points_num_threshold_param_.min_points_and_distance_ratio = + points_num_threshold_param.min_points_and_distance_ratio; } -inline pcl::PointXY toPCL(const geometry_msgs::msg::Point & point) +size_t Validator::getThresholdPointCloud( + const autoware_auto_perception_msgs::msg::DetectedObject & object) { - return toPCL(point.x, point.y); + const auto object_label_id = object.classification.front().label; + const auto object_distance = std::hypot( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + size_t threshold_pc = std::clamp( + static_cast( + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); + return threshold_pc; } -inline pcl::PointXYZ toXYZ(const pcl::PointXY & point) +Validator2D::Validator2D(PointsNumThresholdParam & points_num_threshold_param) +: Validator(points_num_threshold_param) { - return pcl::PointXYZ(point.x, point.y, 0.0); } -inline pcl::PointCloud::Ptr toXYZ( - const pcl::PointCloud::Ptr & pointcloud) +bool Validator2D::setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) +{ + obstacle_pointcloud_.reset(new pcl::PointCloud); + pcl::fromROSMsg(*input_cloud, *obstacle_pointcloud_); + if (obstacle_pointcloud_->empty()) { + return false; + } + + kdtree_ = pcl::make_shared>(false); + kdtree_->setInputCloud(obstacle_pointcloud_); + + return true; +} +pcl::PointCloud::Ptr Validator2D::convertToXYZ( + const pcl::PointCloud::Ptr & pointcloud_xy) { pcl::PointCloud::Ptr pointcloud_xyz(new pcl::PointCloud); - pointcloud_xyz->reserve(pointcloud->size()); - for (const auto & point : *pointcloud) { - pointcloud_xyz->push_back(toXYZ(point)); + pointcloud_xyz->reserve(pointcloud_xy->size()); + for (const auto & point : *pointcloud_xy) { + pointcloud_xyz->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); } return pointcloud_xyz; } -} // namespace +std::optional Validator2D::getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr pointcloud) +{ + std::vector vertices_array; + pcl::Vertices vertices; + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + if (bg::is_empty(poly2d)) return std::nullopt; -namespace obstacle_pointcloud_based_validator + pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + + for (size_t i = 0; i < poly2d.outer().size(); ++i) { + vertices.vertices.emplace_back(i); + vertices_array.emplace_back(vertices); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); + } + cropped_pointcloud_.reset(new pcl::PointCloud); + pcl::CropHull cropper; // don't be implemented PointXY by PCL + cropper.setInputCloud(convertToXYZ(pointcloud)); + cropper.setDim(2); + cropper.setHullIndices(vertices_array); + cropper.setHullCloud(poly3d); + cropper.setCropOutside(true); + cropper.filter(*cropped_pointcloud_); + return cropped_pointcloud_->size(); +} + +bool Validator2D::validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object) { -namespace bg = boost::geometry; -using Shape = autoware_auto_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; + // get neighbor_pointcloud of object + neighbor_pointcloud_.reset(new pcl::PointCloud); + std::vector indices; + std::vector distances; + const auto search_radius = getMaxRadius(transformed_object); + if (!search_radius) { + return false; + } + kdtree_->radiusSearch( + pcl::PointXY( + transformed_object.kinematics.pose_with_covariance.pose.position.x, + transformed_object.kinematics.pose_with_covariance.pose.position.y), + search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud_->push_back(obstacle_pointcloud_->at(index)); + } + const auto num = getPointCloudWithinObject(transformed_object, neighbor_pointcloud_); + if (!num) return true; + + size_t threshold_pointcloud_num = getThresholdPointCloud(transformed_object); + if (num.value() > threshold_pointcloud_num) { + return true; + } + return false; // remove object +} + +std::optional Validator2D::getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { + return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + } else if (object.shape.type == Shape::POLYGON) { + float max_dist = 0.0; + for (const auto & point : object.shape.footprint.points) { + const float dist = std::hypot(point.x, point.y); + max_dist = max_dist < dist ? dist : max_dist; + } + return max_dist; + } else { + return std::nullopt; + } +} + +Validator3D::Validator3D(PointsNumThresholdParam & points_num_threshold_param) +: Validator(points_num_threshold_param) +{ +} +bool Validator3D::setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) +{ + obstacle_pointcloud_.reset(new pcl::PointCloud); + pcl::fromROSMsg(*input_cloud, *obstacle_pointcloud_); + if (obstacle_pointcloud_->empty()) { + return false; + } + // setup kdtree_ + kdtree_ = pcl::make_shared>(false); + kdtree_->setInputCloud(obstacle_pointcloud_); + return true; +} +std::optional Validator3D::getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { + auto square_radius = (object.shape.dimensions.x * 0.5f) * (object.shape.dimensions.x * 0.5f) + + (object.shape.dimensions.y * 0.5f) * (object.shape.dimensions.y * 0.5f) + + (object.shape.dimensions.z * 0.5f) * (object.shape.dimensions.z * 0.5f); + return std::sqrt(square_radius); + } else if (object.shape.type == Shape::POLYGON) { + float max_dist = 0.0; + for (const auto & point : object.shape.footprint.points) { + const float dist = std::hypot(point.x, point.y); + max_dist = max_dist < dist ? dist : max_dist; + } + return std::hypot(max_dist, object.shape.dimensions.z * 0.5f); + } else { + return std::nullopt; + } +} + +std::optional Validator3D::getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr neighbor_pointcloud) +{ + std::vector vertices_array; + pcl::Vertices vertices; + auto const & object_position = object.kinematics.pose_with_covariance.pose.position; + auto const object_height = object.shape.dimensions.x; + auto z_min = object_position.z - object_height / 2.0f; + auto z_max = object_position.z + object_height / 2.0f; + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + if (bg::is_empty(poly2d)) return std::nullopt; + + pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + for (size_t i = 0; i < poly2d.outer().size(); ++i) { + vertices.vertices.emplace_back(i); + vertices_array.emplace_back(vertices); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); + } + + pcl::PointCloud::Ptr cropped_pointcloud_2d(new pcl::PointCloud); + pcl::CropHull cropper; + cropper.setInputCloud(neighbor_pointcloud); + cropper.setDim(2); + cropper.setHullIndices(vertices_array); + cropper.setHullCloud(poly3d); + cropper.setCropOutside(true); + cropper.filter(*cropped_pointcloud_2d); + + cropped_pointcloud_.reset(new pcl::PointCloud); + cropped_pointcloud_->reserve(cropped_pointcloud_2d->size()); + for (const auto & point : *cropped_pointcloud_2d) { + if (point.z > z_min && point.z < z_max) { + cropped_pointcloud_->push_back(point); + } + } + return cropped_pointcloud_->size(); +} + +bool Validator3D::validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object) +{ + neighbor_pointcloud_.reset(new pcl::PointCloud); + std::vector indices; + std::vector distances; + const auto search_radius = getMaxRadius(transformed_object); + if (!search_radius) { + return false; + } + kdtree_->radiusSearch( + pcl::PointXYZ( + transformed_object.kinematics.pose_with_covariance.pose.position.x, + transformed_object.kinematics.pose_with_covariance.pose.position.y, + transformed_object.kinematics.pose_with_covariance.pose.position.z), + search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud_->push_back(obstacle_pointcloud_->at(index)); + } + + const auto num = getPointCloudWithinObject(transformed_object, neighbor_pointcloud_); + if (!num) return true; + + size_t threshold_pointcloud_num = getThresholdPointCloud(transformed_object); + if (num.value() > threshold_pointcloud_num) { + return true; + } + return false; // remove object +} ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const rclcpp::NodeOptions & node_options) @@ -97,13 +294,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( using std::placeholders::_1; using std::placeholders::_2; - // TODO(badai-nguyen): change to single bind function + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); if (using_2d_validator_) { - sync_.registerCallback( - std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); + validator_ = std::make_unique(points_num_threshold_param_); } else { - sync_.registerCallback( - std::bind(&ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud, this, _1, _2)); + validator_ = std::make_unique(points_num_threshold_param_); } objects_pub_ = create_publisher( @@ -112,83 +308,6 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); } -void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) -{ - autoware_auto_perception_msgs::msg::DetectedObjects output, removed_objects; - output.header = input_objects->header; - removed_objects.header = input_objects->header; - - // Transform to pointcloud frame - autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( - *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_, - transformed_objects)) { - return; - } - - // Convert to PCL - pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); - if (obstacle_pointcloud->empty()) { - return; - } - - // Create Kd-tree to search neighbor pointcloud to reduce cost - pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); - kdtree->setInputCloud(obstacle_pointcloud); - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { - const auto & transformed_object = transformed_objects.objects.at(i); - const auto object_label_id = transformed_object.classification.front().label; - const auto & object = input_objects->objects.at(i); - const auto & transformed_object_position = - transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius3D(transformed_object); - if (!search_radius) { - output.objects.push_back(object); - continue; - } - // Search neighbor pointcloud to reduce cost - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - pcl::PointXYZ trans_obj_position_pcl; - trans_obj_position_pcl.x = transformed_object_position.x; - trans_obj_position_pcl.y = transformed_object_position.y; - trans_obj_position_pcl.z = transformed_object_position.z; - kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances); - for (const auto & index : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); - } - - if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); - // Filter object that have few pointcloud in them - const auto num = getPointCloudNumWithinShape(transformed_object, neighbor_pointcloud); - const auto object_distance = - std::hypot(transformed_object_position.x, transformed_object_position.y); - size_t min_pointcloud_num = std::clamp( - static_cast( - points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / - object_distance + - 0.5f), - static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), - static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); - if (num) { - (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); - } else { - output.objects.push_back(object); - } - } - objects_pub_->publish(output); - if (debugger_) { - debugger_->publishRemovedObjects(removed_objects); - debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); - debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); - } -} void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) @@ -205,60 +324,23 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( // objects_pub_->publish(*input_objects); return; } - - // Convert to PCL - pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); - if (obstacle_pointcloud->empty()) { + if (!validator_->setKdtreeInputCloud(input_obstacle_pointcloud)) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud"); - // objects_pub_->publish(*input_objects); return; } - // Create Kd-tree to search neighbor pointcloud to reduce cost. - pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); - kdtree->setInputCloud(obstacle_pointcloud); - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); - const auto object_label_id = transformed_object.classification.front().label; const auto & object = input_objects->objects.at(i); - const auto & transformed_object_position = - transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius2D(transformed_object); - if (!search_radius) { - output.objects.push_back(object); - continue; - } - - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(transformed_object_position), search_radius.value(), indices, distances); - for (const auto & index : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); + const auto validated = validator_->validate_object(transformed_object); + if (debugger_) { + debugger_->addNeighborPointcloud(validator_->getDebugNeighborPointCloud()); + debugger_->addPointcloudWithinPolygon(validator_->getDebugPointCloudWithinObject()); } - if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); - - // Filter object that have few pointcloud in them. - const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); - const auto object_distance = - std::hypot(transformed_object_position.x, transformed_object_position.y); - size_t min_pointcloud_num = std::clamp( - static_cast( - points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / - object_distance + - 0.5f), - static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), - static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); - if (num) { - (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); - } else { + if (validated) { output.objects.push_back(object); + } else { + removed_objects.objects.push_back(object); } } @@ -269,108 +351,6 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); } } -std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinShape( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr pointcloud) -{ - pcl::PointCloud::Ptr cropped_pointcloud_2d(new pcl::PointCloud); - std::vector vertices_array; - pcl::Vertices vertices; - - auto const & object_position = object.kinematics.pose_with_covariance.pose.position; - auto const object_height = object.shape.dimensions.z; - auto z_min = object_position.z - object_height / 2.0f; - auto z_max = object_position.z + object_height / 2.0f; - - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); - if (bg::is_empty(poly2d)) return std::nullopt; - - pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); - - for (size_t i = 0; i < poly2d.outer().size(); ++i) { - vertices.vertices.emplace_back(i); - vertices_array.emplace_back(vertices); - poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); - } - pcl::CropHull cropper; - cropper.setInputCloud(pointcloud); - cropper.setDim(2); - cropper.setHullIndices(vertices_array); - cropper.setHullCloud(poly3d); - cropper.setCropOutside(true); - cropper.filter(*cropped_pointcloud_2d); - - pcl::PointCloud::Ptr cropped_pointcloud_3d(new pcl::PointCloud); - cropped_pointcloud_3d->reserve(cropped_pointcloud_2d->size()); - for (const auto & point : *cropped_pointcloud_2d) { - if (point.z > z_min && point.z < z_max) { - cropped_pointcloud_3d->push_back(point); - } - } - if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud_3d); - return cropped_pointcloud_3d->size(); -} - -std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr pointcloud) -{ - pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); - std::vector vertices_array; - pcl::Vertices vertices; - - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); - if (bg::is_empty(poly2d)) return std::nullopt; - - pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); - - for (size_t i = 0; i < poly2d.outer().size(); ++i) { - vertices.vertices.emplace_back(i); - vertices_array.emplace_back(vertices); - poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); - } - - pcl::CropHull cropper; // don't be implemented PointXY by PCL - cropper.setInputCloud(toXYZ(pointcloud)); - cropper.setDim(2); - cropper.setHullIndices(vertices_array); - cropper.setHullCloud(poly3d); - cropper.setCropOutside(true); - cropper.filter(*cropped_pointcloud); - - if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); - return cropped_pointcloud->size(); -} - -std::optional ObstaclePointCloudBasedValidator::getMaxRadius2D( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { - return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - } else if (object.shape.type == Shape::POLYGON) { - float max_dist = 0.0; - for (const auto & point : object.shape.footprint.points) { - const float dist = std::hypot(point.x, point.y); - max_dist = max_dist < dist ? dist : max_dist; - } - return max_dist; - } else { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type"); - return std::nullopt; - } -} -std::optional ObstaclePointCloudBasedValidator::getMaxRadius3D( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - const auto max_dist_2d = getMaxRadius2D(object); - if (!max_dist_2d) { - return std::nullopt; - } - const auto object_height = object.shape.dimensions.z; - return std::hypot(max_dist_2d.value(), object_height / 2.0); -} } // namespace obstacle_pointcloud_based_validator From be156719e7bb8b859aae79516149f400f1d3803d Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 15 Nov 2023 23:28:38 +0900 Subject: [PATCH 9/9] chore: add maintainer Signed-off-by: badai-nguyen --- perception/detected_object_validation/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index e19b2b1c399b2..31bb633294748 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -6,6 +6,7 @@ The ROS 2 detected_object_validation package Yukihiro Saito Shunsuke Miura + badai nguyen Apache License 2.0 ament_cmake_auto