fix(autoware_image_projection_based_fusion): make optional to conside…

…r lens distortion in the point projection (autowarefoundation#9233)

chore: add point_project_to_unrectified_image parameter to fusion_common.param.yaml

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

perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml

@@ -4,6 +4,7 @@
     timeout_ms: 70.0
     match_threshold_ms: 50.0
     image_buffer_size: 15
+    point_project_to_unrectified_image: false
     debug_mode: false
     filter_scope_min_x: -100.0
     filter_scope_min_y: -100.0

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

@@ -99,6 +99,8 @@ class FusionNode : public rclcpp::Node
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
+  bool point_project_to_unrectified_image_{false};
+
   // camera_info
   std::map<std::size_t, sensor_msgs::msg::CameraInfo> camera_info_map_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;

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

@@ -66,7 +66,8 @@ struct PointData
 bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info);
 
 Eigen::Vector2d calcRawImageProjectedPoint(
-  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d);
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d,
+  const bool & unrectify = false);
 
 std::optional<geometry_msgs::msg::TransformStamped> getTransformStamped(
   const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,

perception/autoware_image_projection_based_fusion/src/fusion_node.cpp

@@ -127,6 +127,8 @@ FusionNode<TargetMsg3D, ObjType, Msg2D>::FusionNode(
     debugger_ =
       std::make_shared<Debugger>(this, rois_number_, image_buffer_size, input_camera_topics_);
   }
+  point_project_to_unrectified_image_ =
+    declare_parameter<bool>("point_project_to_unrectified_image");
 
   // initialize debug tool
   {

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -342,8 +342,8 @@ dc   | dc dc dc  dc ||zc|
       continue;
     }
     // project
-    Eigen::Vector2d projected_point =
-      calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z));
+    Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
+      pinhole_camera_model, cv::Point3d(p_x, p_y, p_z), point_project_to_unrectified_image_);
 
     // iterate 2d bbox
     for (const auto & feature_object : objects) {

perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

@@ -133,8 +133,9 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         continue;
       }
 
-      Eigen::Vector2d projected_point =
-        calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z));
+      Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
+        pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z),
+        point_project_to_unrectified_image_);
       if (
         0 <= static_cast<int>(projected_point.x()) &&
         static_cast<int>(projected_point.x()) <= static_cast<int>(camera_info.width) - 1 &&

perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -149,7 +149,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
       }
 
       Eigen::Vector2d proj_point = calcRawImageProjectedPoint(
-        pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()));
+        pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()),
+        point_project_to_unrectified_image_);
 
       min_x = std::min(proj_point.x(), min_x);
       min_y = std::min(proj_point.y(), min_y);

perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -145,7 +145,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
       continue;
     }
     Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
-      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
+      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z),
+      point_project_to_unrectified_image_);
     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;

perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -137,7 +137,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
     }
 
     Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
-      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
+      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z),
+      point_project_to_unrectified_image_);
 
     bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width &&
                            projected_point.y() > 0 && projected_point.y() < camera_info.height;

perception/autoware_image_projection_based_fusion/src/utils/utils.cpp

@@ -41,10 +41,14 @@ bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info)
 }
 
 Eigen::Vector2d calcRawImageProjectedPoint(
-  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d,
+  const bool & unrectify)
 {
   const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d);
 
+  if (!unrectify) {
+    return Eigen::Vector2d(rectified_image_point.x, rectified_image_point.y);
+  }
   const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point);
 
   return Eigen::Vector2d(raw_image_point.x, raw_image_point.y);