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 32c6bde3daf3f..fb89a7f4a4b15 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 @@ -63,9 +63,6 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::optional getPointCloudNumWithinPolygon( const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud); - void toPolygon2d( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr & polygon); std::optional getMaxRadius( const autoware_auto_perception_msgs::msg::DetectedObject & object); }; diff --git a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index 5602b5ee2eb27..f62b0d866dc3a 100644 --- a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -62,9 +62,6 @@ class OccupancyGridBasedValidator : public rclcpp::Node std::optional getMask( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const autoware_auto_perception_msgs::msg::DetectedObject & object, cv::Mat mask); - void toPolygon2d( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - std::vector & vertices); void showDebugImage( const nav_msgs::msg::OccupancyGrid & ros_occ_grid, const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid); 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 c8e5cce142010..447cd72495636 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -17,6 +17,8 @@ #include #include +#include + #include #include #include @@ -68,7 +70,9 @@ 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) @@ -170,22 +174,29 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPo const autoware_auto_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr pointcloud) { - pcl::PointCloud::Ptr polygon(new pcl::PointCloud); pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); std::vector vertices_array; pcl::Vertices vertices; - toPolygon2d(object, polygon); - if (polygon->empty()) return std::nullopt; + 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 < polygon->size(); ++i) vertices.vertices.push_back(i); - vertices_array.push_back(vertices); + size_t idx = 0; + for (const auto & p : poly2d.outer()) { + vertices.vertices.emplace_back(idx); + vertices_array.emplace_back(vertices); + poly3d->emplace_back(p.x(), p.y(), 0.0); + idx++; + } pcl::CropHull cropper; // don't be implemented PointXY by PCL cropper.setInputCloud(toXYZ(pointcloud)); cropper.setDim(2); cropper.setHullIndices(vertices_array); - cropper.setHullCloud(toXYZ(polygon)); + cropper.setHullCloud(poly3d); cropper.setCropOutside(true); cropper.filter(*cropped_pointcloud); @@ -193,56 +204,6 @@ std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPo return cropped_pointcloud->size(); } -void ObstaclePointCloudBasedValidator::toPolygon2d( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr & polygon) -{ - if (object.shape.type == Shape::BOUNDING_BOX) { - const auto & pose = object.kinematics.pose_with_covariance.pose; - double yaw = tf2::getYaw(pose.orientation); - Eigen::Matrix2d rotation; - rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); - Eigen::Vector2d offset0, offset1, offset2, offset3; - offset0 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - offset1 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); - offset2 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); - offset3 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - polygon->push_back( - pcl::PointXY(toPCL(pose.position.x + offset0.x(), pose.position.y + offset0.y()))); - polygon->push_back( - pcl::PointXY(toPCL(pose.position.x + offset1.x(), pose.position.y + offset1.y()))); - polygon->push_back( - pcl::PointXY(toPCL(pose.position.x + offset2.x(), pose.position.y + offset2.y()))); - polygon->push_back( - pcl::PointXY(toPCL(pose.position.x + offset3.x(), pose.position.y + offset3.y()))); - } else if (object.shape.type == Shape::CYLINDER) { - const auto & center = object.kinematics.pose_with_covariance.pose.position; - const auto & radius = object.shape.dimensions.x * 0.5; - constexpr int n = 6; - for (int i = 0; i < n; ++i) { - Eigen::Vector2d point; - point.x() = std::cos( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.x; - point.y() = std::sin( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.y; - polygon->push_back(toPCL(point.x(), point.y())); - } - } else if (object.shape.type == Shape::POLYGON) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "POLYGON type is not supported"); - } -} - std::optional ObstaclePointCloudBasedValidator::getMaxRadius( const autoware_auto_perception_msgs::msg::DetectedObject & object) { diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 6db8eda20fdfb..4ebdb2ca0f1b8 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -33,6 +33,7 @@ namespace occupancy_grid_based_validator { using Shape = autoware_auto_perception_msgs::msg::Shape; +using Polygon2d = tier4_autoware_utils::Polygon2d; OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options) : rclcpp::Node("occupancy_grid_based_validator", node_options), @@ -103,14 +104,14 @@ std::optional OccupancyGridBasedValidator::getMask( { const auto & resolution = occupancy_grid.info.resolution; const auto & origin = occupancy_grid.info.origin; - std::vector vertices; std::vector pixel_vertices; - toPolygon2d(object, vertices); + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); bool is_polygon_within_image = true; - for (const auto & vertex : vertices) { - const float px = (vertex.x - origin.position.x) / resolution; - const float py = (vertex.y - origin.position.y) / resolution; + for (const auto & p : poly2d.outer()) { + const float px = (p.x() - origin.position.x) / resolution; + const float py = (p.y() - origin.position.y) / resolution; const bool is_point_within_image = (0 <= px && px < mask.cols && 0 <= py && py < mask.rows); if (!is_point_within_image) is_polygon_within_image = false; @@ -141,37 +142,6 @@ cv::Mat OccupancyGridBasedValidator::fromOccupancyGrid( return cv_occ_grid; } -void OccupancyGridBasedValidator::toPolygon2d( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - std::vector & vertices) -{ - if (object.shape.type == Shape::BOUNDING_BOX) { - const auto & pose = object.kinematics.pose_with_covariance.pose; - double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); - Eigen::Matrix2d rotation; - rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); - Eigen::Vector2d offset0, offset1, offset2, offset3; - offset0 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - offset1 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); - offset2 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); - offset3 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - vertices.push_back(cv::Point2f(pose.position.x + offset0.x(), pose.position.y + offset0.y())); - vertices.push_back(cv::Point2f(pose.position.x + offset1.x(), pose.position.y + offset1.y())); - vertices.push_back(cv::Point2f(pose.position.x + offset2.x(), pose.position.y + offset2.y())); - vertices.push_back(cv::Point2f(pose.position.x + offset3.x(), pose.position.y + offset3.y())); - } else if (object.shape.type == Shape::CYLINDER) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "CYLINDER type is not supported"); - } else if (object.shape.type == Shape::POLYGON) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "POLYGON type is not supported"); - } -} - void OccupancyGridBasedValidator::showDebugImage( const nav_msgs::msg::OccupancyGrid & ros_occ_grid, const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid)