add camera_projection class and projection cache

Signed-off-by: a-maumau <[email protected]>

perception/autoware_image_projection_based_fusion/CMakeLists.txt

@@ -16,6 +16,7 @@ endif()
 
 # Build non-CUDA dependent nodes
 ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/camera_projection.cpp
   src/fusion_node.cpp
   src/debugger.cpp
   src/utils/geometry.cpp

perception/autoware_image_projection_based_fusion/README.md

@@ -66,6 +66,12 @@ ros2 launch autoware_image_projection_based_fusion pointpainting_fusion.launch.x
 
 The rclcpp::TimerBase timer could not break a for loop, therefore even if time is out when fusing a roi msg at the middle, the program will run until all msgs are fused.
 
+### Approximate camera projection
+
+For algorithms like `pointpainting_fusion`, the computation required to project points onto an unrectified (raw) image can be substantial. To address this, an option is provided to reduce the computational load. Set the [`approximate_camera_projection parameter`](config/fusion_common.param.yaml) to `true` for each camera (ROIs). If the corresponding `point_project_to_unrectified_image` parameter is also set to true, the projections will be pre-cached.
+
+The cached projections are calculated using a grid, with the grid size specified by the `approximation_grid_width_size` and `approximation_grid_height_size` parameters in the [configuration file](config/fusion_common.param.yaml). The centers of the grid are used as the projected points.
+
 ### Detail description of each fusion's algorithm is in the following links
 
 | Fusion Name                | Description                                                                                     | Detail                                       |

perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml

@@ -4,7 +4,8 @@
     timeout_ms: 70.0
     match_threshold_ms: 50.0
     image_buffer_size: 15
-    point_project_to_unrectified_image: false
+    # projection setting for each ROI whether unrectify image
+    point_project_to_unrectified_image: [false, false, false, false, false, false]
     debug_mode: false
     filter_scope_min_x: -100.0
     filter_scope_min_y: -100.0
@@ -13,5 +14,11 @@
     filter_scope_max_y: 100.0
     filter_scope_max_z: 100.0
 
+    # camera cache setting for each ROI
+    approximate_camera_projection: [true, true, true, true, true, true]
+    # grid size in pixels
+    approximation_grid_width_size: 1.0
+    approximation_grid_height_size: 1.0
+
     # debug parameters
     publish_processing_time_detail: false

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

@@ -0,0 +1,88 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_
+#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_
+
+#define EIGEN_MPL2_ONLY
+
+#include <Eigen/Core>
+
+#include <autoware/universe_utils/system/lru_cache.hpp>
+
+#include <sensor_msgs/msg/camera_info.hpp>
+
+#include <opencv2/core/core.hpp>
+
+#include <image_geometry/pinhole_camera_model.h>
+
+#include <map>
+#include <memory>
+
+namespace autoware::image_projection_based_fusion
+{
+struct PixelPos
+{
+  float x;
+  float y;
+};
+
+class CameraProjection
+{
+public:
+  explicit CameraProjection(
+    const sensor_msgs::msg::CameraInfo & camera_info,
+    const float grid_width, const float grid_height,
+    const bool unrectify,
+    const bool use_approximation
+  );
+  CameraProjection(): grid_w_size_(1.0), grid_h_size_(1.0), unrectify_(false) {};
+  void initialize();
+  std::function<bool(const cv::Point3d &, Eigen::Vector2d &)> calcImageProjectedPoint;
+  sensor_msgs::msg::CameraInfo getCameraInfo();
+  bool isOutsideHorizontalView(const float px, const float pz);
+  bool isOutsideVerticalView(const float py, const float pz);
+
+protected:
+  bool calcRectifiedImageProjectedPoint(const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
+  bool calcRawImageProjectedPoint(const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
+  bool calcRawImageProjectedPointWithApproximation(const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
+  void initializeCache();
+
+  sensor_msgs::msg::CameraInfo camera_info_;
+  uint32_t image_h_, image_w_;
+  double tan_h_x_, tan_h_y_;
+
+  uint32_t cache_size_;
+  float grid_w_size_;
+  float grid_h_size_;
+  float half_grid_w_size_;
+  float half_grid_h_size_;
+  float inv_grid_w_size_;
+  float inv_grid_h_size_;
+  uint32_t grid_x_num_;
+  uint32_t grid_y_num_;
+  float index_grid_out_h_;
+  float index_grid_out_w_;
+
+  bool unrectify_;
+  bool use_approximation_;
+
+  std::unique_ptr<PixelPos[]> projection_cache_;
+  image_geometry::PinholeCameraModel camera_model_;
+};
+
+}  // namespace autoware::image_projection_based_fusion
+
+#endif  // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_

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

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_
 #define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_
 
+#include <autoware/image_projection_based_fusion/camera_projection.hpp>
 #include <autoware/image_projection_based_fusion/debugger.hpp>
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
@@ -56,6 +57,7 @@ using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
 using tier4_perception_msgs::msg::DetectedObjectWithFeature;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
 using autoware_perception_msgs::msg::ObjectClassification;
+using autoware::image_projection_based_fusion::CameraProjection;
 
 template <class TargetMsg3D, class ObjType, class Msg2D>
 class FusionNode : public rclcpp::Node
@@ -86,7 +88,7 @@ class FusionNode : public rclcpp::Node
 
   virtual void fuseOnSingleImage(
     const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
-    const sensor_msgs::msg::CameraInfo & camera_info, TargetMsg3D & output_msg) = 0;
+    TargetMsg3D & output_msg) = 0;
 
   // set args if you need
   virtual void postprocess(TargetMsg3D & output_msg);
@@ -100,11 +102,16 @@ class FusionNode : public rclcpp::Node
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
-  bool point_project_to_unrectified_image_{false};
+  std::vector<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_;
+  // camera projection
+  std::vector<CameraProjection> camera_projectors_;
+  std::vector<bool> approx_camera_projection_;
+  float approx_grid_w_size_;
+  float approx_grid_h_size_;
 
   rclcpp::TimerBase::SharedPtr timer_;
   double timeout_ms_{};

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp

@@ -53,15 +53,12 @@ class PointPaintingFusionNode
   void fuseOnSingleImage(
     const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
     const DetectedObjectsWithFeature & input_roi_msg,
-    const sensor_msgs::msg::CameraInfo & camera_info,
     sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
 
   void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
 
   rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
 
-  std::vector<double> tan_h_;  // horizontal field of view
-
   int omp_num_threads_{1};
   float score_threshold_{0.0};
   std::vector<std::string> class_names_;

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp

@@ -38,7 +38,6 @@ class RoiClusterFusionNode
   void fuseOnSingleImage(
     const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id,
     const DetectedObjectsWithFeature & input_roi_msg,
-    const sensor_msgs::msg::CameraInfo & camera_info,
     DetectedObjectsWithFeature & output_cluster_msg) override;
 
   std::string trust_object_iou_mode_{"iou"};

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp

@@ -43,12 +43,11 @@ class RoiDetectedObjectFusionNode
   void fuseOnSingleImage(
     const DetectedObjects & input_object_msg, const std::size_t image_id,
     const DetectedObjectsWithFeature & input_roi_msg,
-    const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override;
+    DetectedObjects & output_object_msg) override;
 
   std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
-    const DetectedObjects & input_object_msg, const double image_width, const double image_height,
-    const Eigen::Affine3d & object2camera_affine,
-    const image_geometry::PinholeCameraModel & pinhole_camera_model);
+    const DetectedObjects & input_object_msg, const std::size_t & image_id,
+    const Eigen::Affine3d & object2camera_affine);
 
   void fuseObjectsOnImage(
     const DetectedObjects & input_object_msg,

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp

@@ -50,7 +50,7 @@ class RoiPointCloudFusionNode
   void fuseOnSingleImage(
     const PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
     const DetectedObjectsWithFeature & input_roi_msg,
-    const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override;
+    PointCloud2 & output_pointcloud_msg) override;
   bool out_of_scope(const DetectedObjectWithFeature & obj) override;
 };
 

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp

@@ -61,7 +61,7 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
 
   void fuseOnSingleImage(
     const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask,
-    const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override;
+    PointCloud2 & output_pointcloud_msg) override;
 
   bool out_of_scope(const PointCloud2 & filtered_cloud) override;
   inline void copyPointCloud(

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

@@ -65,10 +65,6 @@ 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 bool & unrectify = false);
-
 std::optional<geometry_msgs::msg::TransformStamped> getTransformStamped(
   const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
   const std::string & source_frame_id, const rclcpp::Time & time);