From 3b147dd540395c8dfa5bbd72628398b9406fe3b9 Mon Sep 17 00:00:00 2001 From: Samrat Thapa <38401989+SamratThapa120@users.noreply.github.com> Date: Thu, 19 Sep 2024 08:41:13 +0900 Subject: [PATCH] fix(autoware_lidar_apollo_instance_segmentation): added existence probability (#8862) * added existence probability Signed-off-by: Samrat Thapa * style(pre-commit): autofix --------- Signed-off-by: Samrat Thapa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/cluster2d.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/cluster2d.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/cluster2d.cpp index 0450eca4e3759..d883d93fee2c8 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/cluster2d.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/cluster2d.cpp @@ -72,6 +72,11 @@ geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double return tf2::toMsg(q); } +double sigmoid(const double x) +{ + return 1.0 / (1.0 + std::exp(-x)); +} + Cluster2D::Cluster2D(const int rows, const int cols, const float range) { rows_ = rows; @@ -220,7 +225,7 @@ void Cluster2D::filter(const float * inferred_data) double vec_x = 0.0; double vec_y = 0.0; for (int grid : obs->grids) { - score += static_cast(confidence_pt_data[grid]); + score += sigmoid(static_cast(confidence_pt_data[grid])); height += static_cast(height_pt_data[grid]); vec_x += heading_pt_x_data[grid]; vec_y += heading_pt_y_data[grid]; @@ -265,6 +270,8 @@ tier4_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObjec tier4_perception_msgs::msg::DetectedObjectWithFeature resulting_object; // pcl::PointCloud in_cluster = *(in_obstacle.cloud_ptr); + resulting_object.object.existence_probability = in_obstacle.score; + resulting_object.object.classification.emplace_back( autoware_perception_msgs::build() .label(ObjectClassification::UNKNOWN)