From 9ef23ab08c39f841184fb9897bafd46bd0ff0c7a Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 23 Aug 2024 11:41:14 +0900 Subject: [PATCH] fix(autoware_image_projection_based_fusion): fix unusedFunction (#8567) fix:unusedFunction Signed-off-by: kobayu858 --- .../utils/utils.hpp | 8 -- .../src/utils/utils.cpp | 95 ------------------- 2 files changed, 103 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 9a0f4838d1fac..058a5994ab8ad 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -74,9 +74,6 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); -void convertCluster2FeatureObject( - const std_msgs::msg::Header & header, const PointCloud & cluster, - DetectedObjectWithFeature & feature_obj); void closest_cluster( const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, const pcl::PointXYZ & center, PointCloud2 & out_cluster); @@ -90,11 +87,6 @@ void updateOutputFusedObjects( geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); -pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); -void addShapeAndKinematic( - const pcl::PointCloud & cluster, - tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); - } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 1b303e3f01ff5..1f67d2e1cc86f 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -72,18 +72,6 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } -void convertCluster2FeatureObject( - const std_msgs::msg::Header & header, const PointCloud & cluster, - DetectedObjectWithFeature & feature_obj) -{ - PointCloud2 ros_cluster; - pcl::toROSMsg(cluster, ros_cluster); - ros_cluster.header = header; - feature_obj.feature.cluster = ros_cluster; - feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); - feature_obj.object.existence_probability = 1.0f; -} - void closest_cluster( const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, const pcl::PointXYZ & center, PointCloud2 & out_cluster) @@ -198,73 +186,6 @@ void updateOutputFusedObjects( } } -void addShapeAndKinematic( - const pcl::PointCloud & cluster, - tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) -{ - if (cluster.empty()) { - return; - } - pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); - float max_z = -1e6; - float min_z = 1e6; - for (const auto & point : cluster) { - centroid.x += point.x; - centroid.y += point.y; - centroid.z += point.z; - max_z = max_z < point.z ? point.z : max_z; - min_z = min_z > point.z ? point.z : min_z; - } - centroid.x = centroid.x / static_cast(cluster.size()); - centroid.y = centroid.y / static_cast(cluster.size()); - centroid.z = centroid.z / static_cast(cluster.size()); - - std::vector cluster2d; - std::vector cluster2d_convex; - - for (size_t i = 0; i < cluster.size(); ++i) { - cluster2d.push_back( - cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); - } - cv::convexHull(cluster2d, cluster2d_convex); - if (cluster2d_convex.empty()) { - return; - } - pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); - for (size_t i = 0; i < cluster2d_convex.size(); ++i) { - polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; - polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; - } - polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); - polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); - - autoware_perception_msgs::msg::Shape shape; - for (size_t i = 0; i < cluster2d_convex.size(); ++i) { - geometry_msgs::msg::Point32 point; - point.x = cluster2d_convex.at(i).x / 1000.0; - point.y = cluster2d_convex.at(i).y / 1000.0; - point.z = 0.0; - shape.footprint.points.push_back(point); - } - shape.type = autoware_perception_msgs::msg::Shape::POLYGON; - constexpr float eps = 0.01; - shape.dimensions.x = 0; - shape.dimensions.y = 0; - shape.dimensions.z = std::max((max_z - min_z), eps); - feature_obj.object.shape = shape; - feature_obj.object.kinematics.pose_with_covariance.pose.position.x = - centroid.x + polygon_centroid.x; - feature_obj.object.kinematics.pose_with_covariance.pose.position.y = - centroid.y + polygon_centroid.y; - feature_obj.object.kinematics.pose_with_covariance.pose.position.z = - min_z + shape.dimensions.z * 0.5; - feature_obj.object.existence_probability = 1.0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; -} - geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { geometry_msgs::msg::Point centroid; @@ -285,20 +206,4 @@ geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & poin return centroid; } -pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) -{ - pcl::PointXYZ closest_point; - double min_dist = 1e6; - pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); - for (std::size_t i = 0; i < cluster.points.size(); ++i) { - pcl::PointXYZ point = cluster.points.at(i); - double dist_closest_point = autoware::universe_utils::calcDistance2d(point, orig_point); - if (min_dist > dist_closest_point) { - min_dist = dist_closest_point; - closest_point = pcl::PointXYZ(point.x, point.y, point.z); - } - } - return closest_point; -} - } // namespace autoware::image_projection_based_fusion