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,