Skip to content

Commit

Permalink
fix(autoware_image_projection_based_fusion): fix unusedFunction (auto…
Browse files Browse the repository at this point in the history 
…warefoundation#8567)

fix:unusedFunction

Signed-off-by: kobayu858 <[email protected]>
  • Loading branch information
@kobayu858 @ktro2828
kobayu858 authored and ktro2828 committed Sep 18, 2024
1 parent d1ba5cd commit 9ef23ab
Show file tree
Hide file tree
Showing 2 changed files with 0 additions and 103 deletions.
8 changes: 0 additions & 8 deletions ...ge_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,9 +74,6 @@ std::optional<geometry_msgs::msg::TransformStamped> 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);
Expand All @@ -90,11 +87,6 @@ void updateOutputFusedObjects(

geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud);

pcl::PointXYZ getClosestPoint(const pcl::PointCloud<pcl::PointXYZ> & cluster);
void addShapeAndKinematic(
const pcl::PointCloud<pcl::PointXYZ> & cluster,
tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj);

} // namespace autoware::image_projection_based_fusion

#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_
95 changes: 0 additions & 95 deletions perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -198,73 +186,6 @@ void updateOutputFusedObjects(
}
}

void addShapeAndKinematic(
const pcl::PointCloud<pcl::PointXYZ> & 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<double>(cluster.size());
centroid.y = centroid.y / static_cast<double>(cluster.size());
centroid.z = centroid.z / static_cast<double>(cluster.size());

std::vector<cv::Point> cluster2d;
std::vector<cv::Point> 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<double>(cluster2d_convex.at(i).x) / 1000.0;
polygon_centroid.y += static_cast<double>(cluster2d_convex.at(i).y) / 1000.0;
}
polygon_centroid.x = polygon_centroid.x / static_cast<double>(cluster2d_convex.size());
polygon_centroid.y = polygon_centroid.y / static_cast<double>(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;
Expand All @@ -285,20 +206,4 @@ geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & poin
return centroid;
}

pcl::PointXYZ getClosestPoint(const pcl::PointCloud<pcl::PointXYZ> & 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

0 comments on commit 9ef23ab

Please sign in to comment.