fix(image_projection_based_fusion): handle projection errors in image…

… fusion nodes (#7747)

* fix: add check for camera distortion model

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat(utils): add const qualifier to local variables in checkCameraInfo function

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* style(pre-commit): autofix

* chore(utils): update checkCameraInfo function to use RCLCPP_ERROR_STREAM for unsupported distortion model and coefficients size

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp

@@ -41,6 +41,7 @@
 
 #include "autoware_perception_msgs/msg/shape.hpp"
 #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
+#include <sensor_msgs/msg/camera_info.hpp>
 
 #include <image_geometry/pinhole_camera_model.h>
 #include <pcl/point_cloud.h>
@@ -62,6 +63,8 @@ struct PointData
   size_t orig_index;
 };
 
+bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info);
+
 Eigen::Vector2d calcRawImageProjectedPoint(
   const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d);
 

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -271,6 +271,8 @@ void PointPaintingFusionNode::fuseOnSingleImage(
     return;
   }
 
+  if (!checkCameraInfo(camera_info)) return;
+
   std::vector<sensor_msgs::msg::RegionOfInterest> debug_image_rois;
   std::vector<Eigen::Vector2d> debug_image_points;
 

perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

@@ -83,6 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
 {
+  if (!checkCameraInfo(camera_info)) return;
+
   image_geometry::PinholeCameraModel pinhole_camera_model;
   pinhole_camera_model.fromCameraInfo(camera_info);
 

perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -75,6 +75,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
   const sensor_msgs::msg::CameraInfo & camera_info,
   DetectedObjects & output_object_msg __attribute__((unused)))
 {
+  if (!checkCameraInfo(camera_info)) return;
+
   Eigen::Affine3d object2camera_affine;
   {
     const auto transform_stamped_optional = getTransformStamped(

perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -82,6 +82,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     return;
   }
+  if (!checkCameraInfo(camera_info)) return;
+
   std::vector<DetectedObjectWithFeature> output_objs;
   // select ROIs for fusion
   for (const auto & feature_obj : input_roi_msg.feature_objects) {

perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -57,6 +57,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     return;
   }
+  if (!checkCameraInfo(camera_info)) return;
+
   cv_bridge::CvImagePtr in_image_ptr;
   try {
     in_image_ptr = cv_bridge::toCvCopy(

perception/autoware_image_projection_based_fusion/src/utils/utils.cpp

@@ -14,8 +14,32 @@
 
 #include "autoware/image_projection_based_fusion/utils/utils.hpp"
 
+#include <sensor_msgs/distortion_models.hpp>
+
 namespace autoware::image_projection_based_fusion
 {
+bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info)
+{
+  const bool is_supported_model =
+    (camera_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
+     camera_info.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL);
+  if (!is_supported_model) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("image_projection_based_fusion"),
+      "checkCameraInfo: Unsupported distortion model: " << camera_info.distortion_model);
+    return false;
+  }
+  const bool is_supported_distortion_param =
+    (camera_info.d.size() == 5 || camera_info.d.size() == 8);
+  if (!is_supported_distortion_param) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("image_projection_based_fusion"),
+      "checkCameraInfo: Unsupported distortion coefficients size: " << camera_info.d.size());
+    return false;
+  }
+  return true;
+}
+
 Eigen::Vector2d calcRawImageProjectedPoint(
   const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
 {