feat(BEVFusion): add gpu-based image undistortion to the bevfusion node

perception/autoware_bevfusion/CMakeLists.txt

@@ -101,6 +101,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND SPCONV_AVAIL)
     lib/postprocess/circle_nms_kernel.cu
     lib/postprocess/postprocess_kernel.cu
     lib/preprocess/preprocess_kernel.cu
+	lib/camera/camera_preprocess_kernel.cu
   )
 
   target_link_libraries(${PROJECT_NAME}_cuda_lib
@@ -120,6 +121,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND SPCONV_AVAIL)
     lib/preprocess/precomputed_features.cpp
     lib/ros_utils.cpp
     lib/bevfusion_trt.cpp
+	lib/camera/camera_matrices.cpp
+	lib/camera/camera_data.cpp
   )
 
   target_compile_definitions(${PROJECT_NAME}_lib PRIVATE

perception/autoware_bevfusion/config/bevfusion_camera_lidar.param.yaml

@@ -18,6 +18,8 @@
     # pre-process params
     densification_num_past_frames: 1
     densification_world_frame_id: map
+    run_image_undistortion: true
+
     # post-process params
     circle_nms_dist_threshold: 0.5
     iou_nms_search_distance_2d: 10.0

perception/autoware_bevfusion/config/bevfusion_lidar.param.yaml

@@ -18,6 +18,8 @@
     # pre-process params
     densification_num_past_frames: 1
     densification_world_frame_id: map
+    run_image_undistortion: false
+
     # post-process params
     circle_nms_dist_threshold: 0.5
     iou_nms_search_distance_2d: 10.0

perception/autoware_bevfusion/config/ml_package_bevfusion_camera_lidar.param.yaml

@@ -24,3 +24,4 @@
     features_width: 72
     num_depth_features: 95
     image_feature_channel: 256
+    flip_image_channels: true

perception/autoware_bevfusion/config/ml_package_bevfusion_lidar.param.yaml

@@ -24,3 +24,4 @@
     features_width: 0
     num_depth_features: 0
     image_feature_channel: 0
+    flip_image_channels: false

perception/autoware_bevfusion/include/autoware/bevfusion/bevfusion_node.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE__BEVFUSION__BEVFUSION_NODE_HPP_
 
 #include "autoware/bevfusion/bevfusion_trt.hpp"
+#include "autoware/bevfusion/camera/camera_data.hpp"
 #include "autoware/bevfusion/detection_class_remapper.hpp"
 #include "autoware/bevfusion/postprocess/non_maximum_suppression.hpp"
 #include "autoware/bevfusion/preprocess/pointcloud_densification.hpp"
@@ -67,7 +68,8 @@ class BEVFUSION_PUBLIC BEVFusionNode : public rclcpp::Node
   void diagnoseProcessingTime(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   // Helper methods for constructor
-  void initializeSensorFusionSubscribers(std::int64_t num_cameras);
+  void initializeSensorFusionSubscribers(
+    std::int64_t num_cameras, const ImagePreProcessingParams & image_pre_processing_params);
   void validateParameters(
     const std::vector<float> & point_cloud_range, const std::vector<float> & voxel_size);
 
@@ -100,6 +102,11 @@ class BEVFUSION_PUBLIC BEVFusionNode : public rclcpp::Node
   std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::ConstSharedPtr> camera_info_subs_;
   rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_{
     nullptr};
+  // unique_ptr to avoid copying the actual camera data in memory since there's gpu buffer in the
+  // camera data
+  std::vector<std::unique_ptr<CameraData>> camera_data_ptrs_;
+  // One CameraMatrices object can be shared by several CameraData
+  std::vector<std::shared_ptr<CameraMatrices>> camera_matrices_ptrs_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
@@ -109,9 +116,7 @@ class BEVFUSION_PUBLIC BEVFusionNode : public rclcpp::Node
   std::optional<std::string> lidar_frame_;
   float max_camera_lidar_delay_;
 
-  std::vector<sensor_msgs::msg::Image::ConstSharedPtr> image_msgs_;
   std::vector<float> camera_masks_;
-  std::vector<std::optional<sensor_msgs::msg::CameraInfo>> camera_info_msgs_;
   std::vector<std::optional<Matrix4f>> lidar2camera_extrinsics_;
 
   bool sensor_fusion_{false};

perception/autoware_bevfusion/include/autoware/bevfusion/bevfusion_trt.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__BEVFUSION__BEVFUSION_TRT_HPP_
 #define AUTOWARE__BEVFUSION__BEVFUSION_TRT_HPP_
 
+#include "autoware/bevfusion/camera/camera_data.hpp"
 #include "autoware/bevfusion/postprocess/postprocess_kernel.hpp"
 #include "autoware/bevfusion/preprocess/pointcloud_densification.hpp"
 #include "autoware/bevfusion/preprocess/preprocess_kernel.hpp"
@@ -87,7 +88,7 @@ class BEVFUSION_PUBLIC BEVFusionTRT
 
   bool detect(
     const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
-    const std::vector<sensor_msgs::msg::Image::ConstSharedPtr> & image_msgs,
+    const std::vector<std::unique_ptr<CameraData>> & camera_data_ptrs,
     const std::vector<float> & camera_masks, const tf2_ros::Buffer & tf_buffer,
     std::vector<Box3D> & det_boxes3d, std::unordered_map<std::string, double> & proc_timing,
     bool & is_num_voxels_within_range);
@@ -102,7 +103,7 @@ class BEVFUSION_PUBLIC BEVFusionTRT
 
   bool preProcess(
     const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & pc_msg_ptr,
-    const std::vector<sensor_msgs::msg::Image::ConstSharedPtr> & image_msgs,
+    const std::vector<std::unique_ptr<CameraData>> & camera_data_ptrs,
     const std::vector<float> & camera_masks, const tf2_ros::Buffer & tf_buffer,
     bool & is_num_voxels_within_range);
 
@@ -112,7 +113,7 @@ class BEVFUSION_PUBLIC BEVFusionTRT
   void clearDeviceMemory();
 
   void processImages(
-    const std::vector<sensor_msgs::msg::Image::ConstSharedPtr> & image_msgs,
+    const std::vector<std::unique_ptr<CameraData>> & camera_data_ptrs,
     const std::vector<float> & camera_masks);
 
   std::int64_t processPointCloudVoxelization(
@@ -140,7 +141,6 @@ class BEVFUSION_PUBLIC BEVFusionTRT
   std::unique_ptr<PreprocessCuda> pre_ptr_{nullptr};
   std::unique_ptr<PostprocessCuda> post_ptr_{nullptr};
   cudaStream_t stream_{nullptr};
-  std::vector<cudaStream_t> camera_streams_{};
 
   BEVFusionConfig config_;
   std::vector<int> roi_start_y_vector_;
@@ -171,7 +171,6 @@ class BEVFUSION_PUBLIC BEVFusionTRT
 
   // image buffers
   CudaUniquePtr<std::uint8_t[]> roi_tensor_d_{nullptr};
-  std::vector<CudaUniquePtr<std::uint8_t[]>> image_buffers_d_{};
   CudaUniquePtr<float[]> camera_masks_d_{nullptr};
 
   // image feature buffers for fusion model

perception/autoware_bevfusion/include/autoware/bevfusion/camera/camera_data.hpp

@@ -0,0 +1,106 @@
+// Copyright 2026 TIER IV
+//
+// 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__BEVFUSION__CAMERA__CAMERA_DATA_HPP_
+#define AUTOWARE__BEVFUSION__CAMERA__CAMERA_DATA_HPP_
+
+#include "autoware/bevfusion/camera/camera_matrices.hpp"
+#include "autoware/bevfusion/camera/camera_preprocess.hpp"
+
+#include <autoware/cuda_utils/cuda_check_error.hpp>
+#include <autoware/cuda_utils/cuda_unique_ptr.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <memory>
+#include <string>
+
+namespace autoware::bevfusion
+{
+using autoware::cuda_utils::CudaUniquePtr;
+
+struct ImagePreProcessingParams
+{
+  std::int64_t original_image_height;
+  std::int64_t original_image_width;
+  std::int64_t roi_height;
+  std::int64_t roi_width;
+  float image_aug_scale_y;
+  float image_aug_scale_x;
+  bool run_image_undistortion;
+  bool flip_image_channels;
+  std::int64_t crop_height;
+  std::int64_t crop_width;
+  std::int64_t resized_height;
+  std::int64_t resized_width;
+  std::int64_t roi_start_y;
+  std::int64_t roi_start_x;
+
+  ImagePreProcessingParams(
+    const std::int64_t original_image_height, const std::int64_t original_image_width,
+    const std::int64_t roi_height, const std::int64_t roi_width, const float image_aug_scale_y,
+    const float image_aug_scale_x, const bool run_image_undistortion,
+    const bool flip_image_channels);
+};
+
+class CameraData
+{
+public:
+  CameraData(
+    rclcpp::Node * node, const int camera_id,
+    const ImagePreProcessingParams & image_pre_processing_params,
+    const std::shared_ptr<CameraMatrices> & camera_matrices_ptr);
+  ~CameraData();
+
+  void update_image_msg(const sensor_msgs::msg::Image::ConstSharedPtr & input_camera_image_msg);
+  void update_camera_info(const sensor_msgs::msg::CameraInfo & input_camera_info_msg);
+  std::optional<sensor_msgs::msg::CameraInfo> camera_info() const;
+  sensor_msgs::msg::CameraInfo camera_info_value() const;
+
+  sensor_msgs::msg::Image::ConstSharedPtr image_msg() const;
+  bool is_image_msg_available() const;
+  bool is_camera_info_available() const;
+  std::size_t output_img_offset() const;
+
+  void preprocess_image(std::uint8_t * output_img);
+  bool is_camera_matrices_ready() const;
+  cudaError_t sync_cuda_stream();
+
+private:
+  // Camera info buffer
+  std::optional<sensor_msgs::msg::CameraInfo> camera_info_;
+
+  // Image buffer
+  // TODO(KokSeang): Remove this once we move preprocessing to subscribers
+  sensor_msgs::msg::Image::ConstSharedPtr image_msg_;
+
+  // GPU Memory for preprocessed image
+  // image buffers
+  CudaUniquePtr<std::uint8_t[]> image_buffer_d_{nullptr};
+  CudaUniquePtr<std::uint8_t[]> undistorted_image_buffer_d_{nullptr};
+
+  rclcpp::Logger logger_;
+  int camera_id_;
+  ImagePreProcessingParams image_pre_processing_params_;
+  std::size_t output_img_offset_;
+
+  cudaStream_t stream_{nullptr};
+  std::unique_ptr<CameraPreprocess> camera_preprocess_ptr_{nullptr};
+  std::shared_ptr<CameraMatrices> camera_matrices_ptr_{nullptr};
+};
+}  // namespace autoware::bevfusion
+#endif  // AUTOWARE__BEVFUSION__CAMERA__CAMERA_DATA_HPP_

perception/autoware_bevfusion/include/autoware/bevfusion/camera/camera_matrices.hpp

@@ -0,0 +1,52 @@
+// Copyright 2026 TIER IV
+//
+// 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__BEVFUSION__CAMERA__CAMERA_MATRICES_HPP_
+#define AUTOWARE__BEVFUSION__CAMERA__CAMERA_MATRICES_HPP_
+
+#include <autoware/cuda_utils/cuda_unique_ptr.hpp>
+
+#include <sensor_msgs/msg/camera_info.hpp>
+
+#if __has_include(<cv_bridge/cv_bridge.hpp>)
+#include <cv_bridge/cv_bridge.hpp>
+#else
+#include <cv_bridge/cv_bridge.h>
+#endif
+#include <opencv2/opencv.hpp>
+
+namespace autoware::bevfusion
+{
+using autoware::cuda_utils::CudaUniquePtr;
+
+// Helper struct for camera matrices
+struct CameraMatrices
+{
+  cv::Mat K;
+  cv::Mat D;
+  cv::Mat P;
+  int map_width;
+  int map_height;
+  bool matrices_ready{false};
+
+  // GPU Memory for undistortion maps and they only need to be computed once for each camera
+  CudaUniquePtr<float[]> undistorted_map_x_d_{nullptr};
+  CudaUniquePtr<float[]> undistorted_map_y_d_{nullptr};
+
+  CameraMatrices();
+  void update_camera_matrices(const sensor_msgs::msg::CameraInfo & camera_info);
+  void compute_undistorted_map_x_y();
+};
+}  // namespace autoware::bevfusion
+#endif  // AUTOWARE__BEVFUSION__CAMERA__CAMERA_MATRICES_HPP_

perception/autoware_bevfusion/include/autoware/bevfusion/camera/camera_preprocess.hpp

@@ -0,0 +1,53 @@
+// Copyright 2026 TIER IV
+//
+// 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__BEVFUSION__CAMERA__CAMERA_PREPROCESS_HPP_
+#define AUTOWARE__BEVFUSION__CAMERA__CAMERA_PREPROCESS_HPP_
+
+#include "autoware/bevfusion/utils.hpp"
+
+#include <autoware/cuda_utils/cuda_check_error.hpp>
+
+#include <cstdint>
+
+namespace autoware::bevfusion
+{
+
+class CameraPreprocess
+{
+public:
+  CameraPreprocess(cudaStream_t stream, const int camera_id);
+  ~CameraPreprocess();
+
+  cudaError_t resizeAndExtractRoi_launch(
+    const std::uint8_t * input_img, std::uint8_t * output_img, int H,
+    int W,                     // Original image dimensions
+    int H2, int W2,            // Resized image dimensions
+    int H3, int W3,            // ROI dimensions
+    int y_start, int x_start,  // ROI top-left coordinates in resized image
+    bool flip_image_channels   // Set to True if the image channels are to be flipped, for example,
+                               // BGR to RGB
+  );
+  cudaError_t remap_launch(
+    const std::uint8_t * input_img, std::uint8_t * output_img, int output_height,
+    int output_width,                   // Output (destination) image dimensions
+    int input_height, int input_width,  // Input (source) image dimensions
+    const float * map_x, const float * map_y);
+
+private:
+  cudaStream_t stream_;
+  int camera_id_;
+};
+}  // namespace autoware::bevfusion
+#endif  // AUTOWARE__BEVFUSION__CAMERA__CAMERA_PREPROCESS_HPP_

perception/autoware_bevfusion/include/autoware/bevfusion/preprocess/preprocess_kernel.hpp

@@ -59,10 +59,6 @@ class PreprocessCuda
     const float * points, unsigned int points_size, float * voxel_features,
     std::int32_t * voxel_coords, std::int32_t * num_points_per_voxel);
 
-  cudaError_t resizeAndExtractRoi_launch(
-    const std::uint8_t * input_img, std::uint8_t * output_img, int H, int W, int H2, int W2, int H3,
-    int W3, int y_start, int x_start, cudaStream_t stream);
-
 private:
   BEVFusionConfig config_;
   cudaStream_t stream_;

perception/autoware_bevfusion/launch/bevfusion.launch.xml

@@ -74,18 +74,18 @@
       <param from="$(var class_remapper_param_path)"/>
       <param from="$(var common_param_path)"/>
 
-      <remap from="~/input/camera_info0" to="$(var camera_info0)"/>
-      <remap from="~/input/camera_info1" to="$(var camera_info1)"/>
-      <remap from="~/input/camera_info2" to="$(var camera_info2)"/>
-      <remap from="~/input/camera_info3" to="$(var camera_info3)"/>
-      <remap from="~/input/camera_info4" to="$(var camera_info4)"/>
-      <remap from="~/input/camera_info5" to="$(var camera_info5)"/>
-      <remap from="~/input/image0" to="$(var image0)"/>
-      <remap from="~/input/image1" to="$(var image1)"/>
-      <remap from="~/input/image2" to="$(var image2)"/>
-      <remap from="~/input/image3" to="$(var image3)"/>
-      <remap from="~/input/image4" to="$(var image4)"/>
-      <remap from="~/input/image5" to="$(var image5)"/>
+      <remap from="~/input/camera_info0" to="$(var bevfusion_camera_info0)"/>
+      <remap from="~/input/camera_info1" to="$(var bevfusion_camera_info1)"/>
+      <remap from="~/input/camera_info2" to="$(var bevfusion_camera_info2)"/>
+      <remap from="~/input/camera_info3" to="$(var bevfusion_camera_info3)"/>
+      <remap from="~/input/camera_info4" to="$(var bevfusion_camera_info4)"/>
+      <remap from="~/input/camera_info5" to="$(var bevfusion_camera_info5)"/>
+      <remap from="~/input/image0" to="$(var bevfusion_image0)"/>
+      <remap from="~/input/image1" to="$(var bevfusion_image1)"/>
+      <remap from="~/input/image2" to="$(var bevfusion_image2)"/>
+      <remap from="~/input/image3" to="$(var bevfusion_image3)"/>
+      <remap from="~/input/image4" to="$(var bevfusion_image4)"/>
+      <remap from="~/input/image5" to="$(var bevfusion_image5)"/>
       <param name="use_compressed_images" value="$(var use_compressed_images)"/>
       <!-- This parameter shall NOT be included in param file. See also: https://github.com/autowarefoundation/autoware.universe/pull/6167 -->
       <param name="build_only" value="$(var build_only)"/>