Skip to content

Commit b4593ce

Browse files
KSeangTanpre-commit-ci-lite[bot]amadeuszsz
authored
feat(BEVFusion): add gpu-based image undistortion to the bevfusion node (#12279)
* Update maintaner name in streampetr Signed-off-by: Kok Seang <kseangtan@gmail.com> * Update maintaner name in streampetr Signed-off-by: Kok Seang <kseangtan@gmail.com> * Add camera_data to bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Update CMakeList in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Update CMakeList in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add camera_data in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add camera_data in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add camera_data in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add camera_data in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add camera_data in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add camera_matrices in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add camera_matrices in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add undistrotion_image_buffer_d_ in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Fix json schema in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Fix json schema in bevfusion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Resolve double deletion Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Convert tabs to spaces Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Convert tabs to spaces Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Remove flip_image_channels * Remove flip_image_channels Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Remove lib/camera_matrices.cpp Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add sanity check to check input image shape and ImagePreProcessingParams shape Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add sanity check to check input image shape and ImagePreProcessingParams shape Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Update error message Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add CHECK_CUDA_ERROR for cuda function Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add checkImageCameraMatricesReady and check sensor_fusion before processImages Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add checkImageCameraMatricesReady and check sensor_fusion before processImages Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Remove cv_bridge and opencv hpp Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Fix possible bugs for optional variables Signed-off-by: KSeangTan <kseangtan@gmail.com> * Fix possible bugs for optional variables Signed-off-by: KSeangTan <kseangtan@gmail.com> * style(pre-commit): autofix * Fix possible bugs for optional variables Signed-off-by: KSeangTan <kseangtan@gmail.com> * style(pre-commit): autofix * Add image encoding checking to image topic Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add image encoding checking to image topic Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add image encoding checking to image topic Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> * Add stream to compute_undistorted_map_x_y Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> --------- Signed-off-by: Kok Seang <kseangtan@gmail.com> Signed-off-by: Kok Seang Tan <kseangtan@gmail.com> Signed-off-by: KSeangTan <kseangtan@gmail.com> Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Co-authored-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp>
1 parent e393f23 commit b4593ce

19 files changed

Lines changed: 955 additions & 200 deletions

‎perception/autoware_bevfusion/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND SPCONV_AVAIL)
101101
lib/postprocess/circle_nms_kernel.cu
102102
lib/postprocess/postprocess_kernel.cu
103103
lib/preprocess/preprocess_kernel.cu
104+
lib/camera/camera_preprocess_kernel.cu
104105
)
105106

106107
target_link_libraries(${PROJECT_NAME}_cuda_lib
@@ -120,6 +121,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND SPCONV_AVAIL)
120121
lib/preprocess/precomputed_features.cpp
121122
lib/ros_utils.cpp
122123
lib/bevfusion_trt.cpp
124+
lib/camera/camera_matrices.cpp
125+
lib/camera/camera_data.cpp
123126
)
124127

125128
target_compile_definitions(${PROJECT_NAME}_lib PRIVATE

‎perception/autoware_bevfusion/README.md‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,11 +14,11 @@ Autoware installs it automatically in its setup script. If needed, the user can
1414

1515
### Input
1616

17-
| Name | Type | Description |
18-
| ---------------------- | ------------------------------- | ------------------------- |
19-
| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | Input pointcloud topics. |
20-
| `~/input/image*` | `sensor_msgs::msg::Image` | Input image topics. |
21-
| `~/input/camera_info*` | `sensor_msgs::msg::CameraInfo` | Input camera info topics. |
17+
| Name | Type | Description |
18+
| ---------------------- | ------------------------------- | ------------------------------------------------------------- |
19+
| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | Input pointcloud topics. |
20+
| `~/input/image*` | `sensor_msgs::msg::Image` | Input image topics. The image is assumed to be RGB8 encoding. |
21+
| `~/input/camera_info*` | `sensor_msgs::msg::CameraInfo` | Input camera info topics. |
2222

2323
### Output
2424

‎perception/autoware_bevfusion/config/bevfusion_camera_lidar.param.yaml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@
1818
# pre-process params
1919
densification_num_past_frames: 1
2020
densification_world_frame_id: map
21+
run_image_undistortion: true
22+
2123
# post-process params
2224
circle_nms_dist_threshold: 0.5
2325
iou_nms_search_distance_2d: 10.0

‎perception/autoware_bevfusion/config/bevfusion_lidar.param.yaml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@
1818
# pre-process params
1919
densification_num_past_frames: 1
2020
densification_world_frame_id: map
21+
run_image_undistortion: false
22+
2123
# post-process params
2224
circle_nms_dist_threshold: 0.5
2325
iou_nms_search_distance_2d: 10.0

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

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define AUTOWARE__BEVFUSION__BEVFUSION_NODE_HPP_
1717

1818
#include "autoware/bevfusion/bevfusion_trt.hpp"
19+
#include "autoware/bevfusion/camera/camera_data.hpp"
1920
#include "autoware/bevfusion/detection_class_remapper.hpp"
2021
#include "autoware/bevfusion/postprocess/non_maximum_suppression.hpp"
2122
#include "autoware/bevfusion/preprocess/pointcloud_densification.hpp"
@@ -67,7 +68,8 @@ class BEVFUSION_PUBLIC BEVFusionNode : public rclcpp::Node
6768
void diagnoseProcessingTime(diagnostic_updater::DiagnosticStatusWrapper & stat);
6869

6970
// Helper methods for constructor
70-
void initializeSensorFusionSubscribers(std::int64_t num_cameras);
71+
void initializeSensorFusionSubscribers(
72+
std::int64_t num_cameras, const ImagePreProcessingParams & image_pre_processing_params);
7173
void validateParameters(
7274
const std::vector<float> & point_cloud_range, const std::vector<float> & voxel_size);
7375

@@ -100,6 +102,11 @@ class BEVFUSION_PUBLIC BEVFusionNode : public rclcpp::Node
100102
std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::ConstSharedPtr> camera_info_subs_;
101103
rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_{
102104
nullptr};
105+
// unique_ptr to avoid copying the actual camera data in memory since there's gpu buffer in the
106+
// camera data
107+
std::vector<std::unique_ptr<CameraData>> camera_data_ptrs_;
108+
// One CameraMatrices object can be shared by several CameraData
109+
std::vector<std::shared_ptr<CameraMatrices>> camera_matrices_ptrs_;
103110

104111
tf2_ros::Buffer tf_buffer_;
105112
tf2_ros::TransformListener tf_listener_{tf_buffer_};
@@ -109,9 +116,7 @@ class BEVFUSION_PUBLIC BEVFusionNode : public rclcpp::Node
109116
std::optional<std::string> lidar_frame_;
110117
float max_camera_lidar_delay_;
111118

112-
std::vector<sensor_msgs::msg::Image::ConstSharedPtr> image_msgs_;
113119
std::vector<float> camera_masks_;
114-
std::vector<std::optional<sensor_msgs::msg::CameraInfo>> camera_info_msgs_;
115120
std::vector<std::optional<Matrix4f>> lidar2camera_extrinsics_;
116121

117122
bool sensor_fusion_{false};

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

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef AUTOWARE__BEVFUSION__BEVFUSION_TRT_HPP_
1616
#define AUTOWARE__BEVFUSION__BEVFUSION_TRT_HPP_
1717

18+
#include "autoware/bevfusion/camera/camera_data.hpp"
1819
#include "autoware/bevfusion/postprocess/postprocess_kernel.hpp"
1920
#include "autoware/bevfusion/preprocess/pointcloud_densification.hpp"
2021
#include "autoware/bevfusion/preprocess/preprocess_kernel.hpp"
@@ -87,7 +88,7 @@ class BEVFUSION_PUBLIC BEVFusionTRT
8788

8889
bool detect(
8990
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
90-
const std::vector<sensor_msgs::msg::Image::ConstSharedPtr> & image_msgs,
91+
const std::vector<std::unique_ptr<CameraData>> & camera_data_ptrs,
9192
const std::vector<float> & camera_masks, const tf2_ros::Buffer & tf_buffer,
9293
std::vector<Box3D> & det_boxes3d, std::unordered_map<std::string, double> & proc_timing,
9394
bool & is_num_voxels_within_range);
@@ -102,7 +103,7 @@ class BEVFUSION_PUBLIC BEVFusionTRT
102103

103104
bool preProcess(
104105
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & pc_msg_ptr,
105-
const std::vector<sensor_msgs::msg::Image::ConstSharedPtr> & image_msgs,
106+
const std::vector<std::unique_ptr<CameraData>> & camera_data_ptrs,
106107
const std::vector<float> & camera_masks, const tf2_ros::Buffer & tf_buffer,
107108
bool & is_num_voxels_within_range);
108109

@@ -111,8 +112,11 @@ class BEVFUSION_PUBLIC BEVFusionTRT
111112

112113
void clearDeviceMemory();
113114

114-
void processImages(
115-
const std::vector<sensor_msgs::msg::Image::ConstSharedPtr> & image_msgs,
115+
bool checkImageCameraMatricesReady(
116+
const std::vector<std::unique_ptr<CameraData>> & camera_data_ptrs);
117+
118+
bool processImages(
119+
const std::vector<std::unique_ptr<CameraData>> & camera_data_ptrs,
116120
const std::vector<float> & camera_masks);
117121

118122
std::int64_t processPointCloudVoxelization(
@@ -140,7 +144,6 @@ class BEVFUSION_PUBLIC BEVFusionTRT
140144
std::unique_ptr<PreprocessCuda> pre_ptr_{nullptr};
141145
std::unique_ptr<PostprocessCuda> post_ptr_{nullptr};
142146
cudaStream_t stream_{nullptr};
143-
std::vector<cudaStream_t> camera_streams_{};
144147

145148
BEVFusionConfig config_;
146149
std::vector<int> roi_start_y_vector_;
@@ -171,7 +174,6 @@ class BEVFUSION_PUBLIC BEVFusionTRT
171174

172175
// image buffers
173176
CudaUniquePtr<std::uint8_t[]> roi_tensor_d_{nullptr};
174-
std::vector<CudaUniquePtr<std::uint8_t[]>> image_buffers_d_{};
175177
CudaUniquePtr<float[]> camera_masks_d_{nullptr};
176178

177179
// image feature buffers for fusion model

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

Lines changed: 109 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
// Copyright 2026 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__BEVFUSION__CAMERA__CAMERA_DATA_HPP_
16+
#define AUTOWARE__BEVFUSION__CAMERA__CAMERA_DATA_HPP_
17+
18+
#include "autoware/bevfusion/camera/camera_matrices.hpp"
19+
#include "autoware/bevfusion/camera/camera_preprocess.hpp"
20+
21+
#include <autoware/cuda_utils/cuda_check_error.hpp>
22+
#include <autoware/cuda_utils/cuda_unique_ptr.hpp>
23+
#include <rclcpp/rclcpp.hpp>
24+
25+
#include <sensor_msgs/image_encodings.hpp>
26+
#include <sensor_msgs/msg/camera_info.hpp>
27+
#include <sensor_msgs/msg/compressed_image.hpp>
28+
#include <sensor_msgs/msg/image.hpp>
29+
30+
#include <memory>
31+
#include <string>
32+
33+
namespace autoware::bevfusion
34+
{
35+
using autoware::cuda_utils::CudaUniquePtr;
36+
37+
struct ImagePreProcessingParams
38+
{
39+
std::int64_t original_image_height;
40+
std::int64_t original_image_width;
41+
std::int64_t roi_height;
42+
std::int64_t roi_width;
43+
float image_aug_scale_y;
44+
float image_aug_scale_x;
45+
bool run_image_undistortion;
46+
std::int64_t crop_height;
47+
std::int64_t crop_width;
48+
std::int64_t resized_height;
49+
std::int64_t resized_width;
50+
std::int64_t roi_start_y;
51+
std::int64_t roi_start_x;
52+
53+
ImagePreProcessingParams(
54+
const std::int64_t original_image_height, const std::int64_t original_image_width,
55+
const std::int64_t roi_height, const std::int64_t roi_width, const float image_aug_scale_y,
56+
const float image_aug_scale_x, const bool run_image_undistortion);
57+
};
58+
59+
class CameraData
60+
{
61+
public:
62+
CameraData(
63+
rclcpp::Node * node, const int camera_id,
64+
const ImagePreProcessingParams & image_pre_processing_params,
65+
const std::shared_ptr<CameraMatrices> & camera_matrices_ptr);
66+
~CameraData();
67+
68+
void update_image_msg(const sensor_msgs::msg::Image::ConstSharedPtr & input_camera_image_msg);
69+
void update_camera_info(const sensor_msgs::msg::CameraInfo & input_camera_info_msg);
70+
std::optional<sensor_msgs::msg::CameraInfo> camera_info() const;
71+
sensor_msgs::msg::CameraInfo camera_info_value() const;
72+
73+
sensor_msgs::msg::Image::ConstSharedPtr image_msg() const;
74+
bool is_image_msg_available() const;
75+
bool is_camera_info_available() const;
76+
std::size_t output_img_offset() const;
77+
78+
bool preprocess_image(std::uint8_t * output_img);
79+
bool is_camera_matrices_ready() const;
80+
bool is_image_encoding_supported() const;
81+
cudaError_t sync_cuda_stream();
82+
83+
private:
84+
// Camera info buffer
85+
std::optional<sensor_msgs::msg::CameraInfo> camera_info_;
86+
87+
// Image buffer
88+
// TODO(KokSeang): Remove this once we move preprocessing to subscribers
89+
sensor_msgs::msg::Image::ConstSharedPtr image_msg_;
90+
91+
// GPU Memory for preprocessed image
92+
// image buffers
93+
CudaUniquePtr<std::uint8_t[]> image_buffer_d_{nullptr};
94+
CudaUniquePtr<std::uint8_t[]> undistorted_image_buffer_d_{nullptr};
95+
96+
rclcpp::Logger logger_;
97+
int camera_id_;
98+
ImagePreProcessingParams image_pre_processing_params_;
99+
std::size_t output_img_offset_;
100+
101+
cudaStream_t stream_{nullptr};
102+
std::unique_ptr<CameraPreprocess> camera_preprocess_ptr_{nullptr};
103+
std::shared_ptr<CameraMatrices> camera_matrices_ptr_{nullptr};
104+
105+
// Supported image encoding, only RGB8 is supported for now
106+
const std::string supported_image_encoding_{sensor_msgs::image_encodings::RGB8};
107+
};
108+
} // namespace autoware::bevfusion
109+
#endif // AUTOWARE__BEVFUSION__CAMERA__CAMERA_DATA_HPP_

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

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
// Copyright 2026 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__BEVFUSION__CAMERA__CAMERA_MATRICES_HPP_
16+
#define AUTOWARE__BEVFUSION__CAMERA__CAMERA_MATRICES_HPP_
17+
18+
#include <autoware/cuda_utils/cuda_unique_ptr.hpp>
19+
#include <opencv2/opencv.hpp>
20+
21+
#include <sensor_msgs/msg/camera_info.hpp>
22+
23+
namespace autoware::bevfusion
24+
{
25+
using autoware::cuda_utils::CudaUniquePtr;
26+
27+
// Helper struct for camera matrices
28+
struct CameraMatrices
29+
{
30+
cv::Mat K;
31+
cv::Mat D;
32+
cv::Mat P;
33+
int map_width;
34+
int map_height;
35+
bool matrices_ready{false};
36+
37+
// GPU Memory for undistortion maps and they only need to be computed once for each camera
38+
CudaUniquePtr<float[]> undistorted_map_x_d_{nullptr};
39+
CudaUniquePtr<float[]> undistorted_map_y_d_{nullptr};
40+
41+
CameraMatrices();
42+
void update_camera_matrices(const sensor_msgs::msg::CameraInfo & camera_info);
43+
void compute_undistorted_map_x_y(cudaStream_t stream);
44+
};
45+
} // namespace autoware::bevfusion
46+
#endif // AUTOWARE__BEVFUSION__CAMERA__CAMERA_MATRICES_HPP_

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

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
// Copyright 2026 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__BEVFUSION__CAMERA__CAMERA_PREPROCESS_HPP_
16+
#define AUTOWARE__BEVFUSION__CAMERA__CAMERA_PREPROCESS_HPP_
17+
18+
#include "autoware/bevfusion/utils.hpp"
19+
20+
#include <autoware/cuda_utils/cuda_check_error.hpp>
21+
22+
#include <cstdint>
23+
24+
namespace autoware::bevfusion
25+
{
26+
27+
class CameraPreprocess
28+
{
29+
public:
30+
CameraPreprocess(cudaStream_t stream, const int camera_id);
31+
32+
cudaError_t resizeAndExtractRoi_launch(
33+
const std::uint8_t * input_img, std::uint8_t * output_img, int H,
34+
int W, // Original image dimensions
35+
int H2, int W2, // Resized image dimensions
36+
int H3, int W3, // ROI dimensions
37+
int y_start, int x_start // ROI top-left coordinates in resized image
38+
);
39+
cudaError_t remap_launch(
40+
const std::uint8_t * input_img, std::uint8_t * output_img, int output_height,
41+
int output_width, // Output (destination) image dimensions
42+
int input_height, int input_width, // Input (source) image dimensions
43+
const float * map_x, const float * map_y);
44+
45+
private:
46+
cudaStream_t stream_;
47+
int camera_id_;
48+
};
49+
} // namespace autoware::bevfusion
50+
#endif // AUTOWARE__BEVFUSION__CAMERA__CAMERA_PREPROCESS_HPP_

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

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -59,10 +59,6 @@ class PreprocessCuda
5959
const float * points, unsigned int points_size, float * voxel_features,
6060
std::int32_t * voxel_coords, std::int32_t * num_points_per_voxel);
6161

62-
cudaError_t resizeAndExtractRoi_launch(
63-
const std::uint8_t * input_img, std::uint8_t * output_img, int H, int W, int H2, int W2, int H3,
64-
int W3, int y_start, int x_start, cudaStream_t stream);
65-
6662
private:
6763
BEVFusionConfig config_;
6864
cudaStream_t stream_;

0 commit comments

Comments
 (0)