From e3cbf6cb524a34ad3b693a8c0eaa1680e3141f72 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 18 Oct 2023 08:33:02 +0900 Subject: [PATCH] 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