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