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/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp index 9ef1f75427b65..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,11 +71,18 @@ 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) + { + 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) { // 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..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: @@ -61,16 +142,13 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node PointsNumThresholdParam points_num_threshold_param_; 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); - std::optional getPointCloudNumWithinPolygon( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr pointcloud); - std::optional getMaxRadius( - const autoware_auto_perception_msgs::msg::DetectedObject & object); }; } // namespace obstacle_pointcloud_based_validator 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/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 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..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) @@ -85,13 +282,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 +289,25 @@ 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; + + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); + if (using_2d_validator_) { + validator_ = std::make_unique(points_num_threshold_param_); + } else { + validator_ = std::make_unique(points_num_threshold_param_); + } + + 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( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) @@ -119,61 +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 = 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( - 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. - // 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); - 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); } } @@ -185,56 +352,6 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } } -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::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 { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type"); - return std::nullopt; - } -} - } // namespace obstacle_pointcloud_based_validator #include