fix: handle projection errors in image fusion nodes

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
autowarefoundation · Aug 14, 2024 · 34ee5f6 · 34ee5f6
1 parent 6b31cf4
commit 34ee5f6
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 11 deletions.
diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
@@ -339,9 +339,16 @@ dc   | dc dc dc  dc ||zc|
     if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) {
       continue;
     }
-    // project
-    Eigen::Vector2d projected_point =
-      calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z));
+
+    // project, try and catch. if it fails, return
+    Eigen::Vector2d projected_point;
+    try {
+      projected_point =
+        calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z));
+    } catch (const cv::Exception & e) {
+      RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what());
+      continue;
+    }
 
     // iterate 2d bbox
     for (const auto & feature_object : objects) {

diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -131,8 +131,16 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         continue;
       }
 
-      Eigen::Vector2d projected_point =
-        calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z));
+      // project, try and catch. if it fails, return
+      Eigen::Vector2d projected_point;
+      try {
+        projected_point =
+          calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z));
+      } catch (const std::exception & e) {
+        RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what());
+        return;
+      }
+
       if (
         0 <= static_cast<int>(projected_point.x()) &&
         static_cast<int>(projected_point.x()) <= static_cast<int>(camera_info.width) - 1 &&

diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
@@ -146,8 +146,15 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
         continue;
       }
 
-      Eigen::Vector2d proj_point = calcRawImageProjectedPoint(
-        pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()));
+      // project, try and catch. if it fails, return
+      Eigen::Vector2d proj_point;
+      try {
+        proj_point = calcRawImageProjectedPoint(
+          pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()));
+      } catch (const std::exception & e) {
+        RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what());
+        return object_roi_map;
+      }
 
       min_x = std::min(proj_point.x(), min_x);
       min_y = std::min(proj_point.y(), min_y);

diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
@@ -142,8 +142,17 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
     if (transformed_z <= 0.0) {
       continue;
     }
-    Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
-      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
+
+    // project, try and catch. if it fails, return
+    Eigen::Vector2d projected_point;
+    try {
+      projected_point = calcRawImageProjectedPoint(
+        pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
+    } catch (const std::exception & e) {
+      RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what());
+      return;
+    }
+
     for (std::size_t i = 0; i < output_objs.size(); ++i) {
       auto & feature_obj = output_objs.at(i);
       const auto & check_roi = feature_obj.feature.roi;

diff --git a/...eption/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/...eption/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -120,8 +120,15 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
       continue;
     }
 
-    Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
-      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
+    // project, try and catch. if it fails, return
+    Eigen::Vector2d projected_point;
+    try {
+      projected_point = calcRawImageProjectedPoint(
+        pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
+    } catch (const std::exception & e) {
+      RCLCPP_WARN_STREAM(this->get_logger(), "Error in projection: " << e.what());
+      return;
+    }
 
     bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width &&
                            projected_point.y() > 0 && projected_point.y() < camera_info.height;