From 21de9e5dd54c6a697a953e5bff45e3c4472e62c9 Mon Sep 17 00:00:00 2001
From: Yukihiro Saito <yukky.saito@gmail.com>
Date: Fri, 26 Apr 2024 14:05:19 +0900
Subject: [PATCH] calc unrectified point

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>
---
 .../image_projection_based_fusion/package.xml |  1 +
 .../src/roi_cluster_fusion/node.cpp           | 64 +++++++++++++------
 2 files changed, 45 insertions(+), 20 deletions(-)

diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml
index a5b41a30be166..ee4c777d05a5d 100644
--- a/perception/image_projection_based_fusion/package.xml
+++ b/perception/image_projection_based_fusion/package.xml
@@ -21,6 +21,7 @@
   <depend>cv_bridge</depend>
   <depend>euclidean_cluster</depend>
   <depend>image_transport</depend>
+  <depend>image_geometry</depend>
   <depend>lidar_centerpoint</depend>
   <depend>message_filters</depend>
   <depend>object_recognition_utils</depend>
diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
index feaf1ad495ce8..591040bd404b3 100644
--- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
+++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -28,6 +28,28 @@
 #include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 #endif
 
+#include <image_geometry/pinhole_camera_model.h>
+
+namespace
+{
+Eigen::Vector2d calcRawImagePointFromPoint3D(
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
+{
+  cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d);
+
+  cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point);
+  return Eigen::Vector2d(raw_image_point.x, raw_image_point.y);
+}
+
+Eigen::Vector2d calcRectifiedImagePointFromPoint3D(
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
+{
+  cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d);
+  return Eigen::Vector2d(rectified_image_point.x, rectified_image_point.y);
+}
+
+}  // namespace
+
 namespace image_projection_based_fusion
 {
 
@@ -83,10 +105,8 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
 {
-  Eigen::Matrix4d projection;
-  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
-    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
 
   // get transform from cluster frame id to camera optical frame id
   geometry_msgs::msg::TransformStamped transform_stamped;
@@ -133,23 +153,27 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         continue;
       }
 
-      Eigen::Vector4d projected_point =
-        projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0);
-      Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
-        projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
+      Eigen::Vector2d projected_point;
+
+      // if (camera_info.do_rectify) {
+      projected_point =
+        calcRawImagePointFromPoint3D(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z));
+      // } else {
+      //   projected_point = calcRectifiedImagePointFromPoint3D(
+      //     pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z));
+      // }
+
       if (
-        0 <= static_cast<int>(normalized_projected_point.x()) &&
-        static_cast<int>(normalized_projected_point.x()) <=
-          static_cast<int>(camera_info.width) - 1 &&
-        0 <= static_cast<int>(normalized_projected_point.y()) &&
-        static_cast<int>(normalized_projected_point.y()) <=
-          static_cast<int>(camera_info.height) - 1) {
-        min_x = std::min(static_cast<int>(normalized_projected_point.x()), min_x);
-        min_y = std::min(static_cast<int>(normalized_projected_point.y()), min_y);
-        max_x = std::max(static_cast<int>(normalized_projected_point.x()), max_x);
-        max_y = std::max(static_cast<int>(normalized_projected_point.y()), max_y);
-        projected_points.push_back(normalized_projected_point);
-        if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point);
+        0 <= static_cast<int>(projected_point.x()) &&
+        static_cast<int>(projected_point.x()) <= static_cast<int>(camera_info.width) - 1 &&
+        0 <= static_cast<int>(projected_point.y()) &&
+        static_cast<int>(projected_point.y()) <= static_cast<int>(camera_info.height) - 1) {
+        min_x = std::min(static_cast<int>(projected_point.x()), min_x);
+        min_y = std::min(static_cast<int>(projected_point.y()), min_y);
+        max_x = std::max(static_cast<int>(projected_point.x()), max_x);
+        max_y = std::max(static_cast<int>(projected_point.y()), max_y);
+        projected_points.push_back(projected_point);
+        if (debugger_) debugger_->obstacle_points_.push_back(projected_point);
       }
     }
     if (projected_points.empty()) {