Skip to content

Commit ca3afdd

Browse files
a-maumaupre-commit-ci[bot]technolojinbadai-nguyen
authored
feat(image_projection_based_fusion): add cache for camera projection (#9635)
* add camera_projection class and projection cache Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * style(pre-commit): autofix * fix FOV filtering Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * style(pre-commit): autofix * remove unused includes Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * update schema Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * fix cpplint error Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * apply suggestion: more simple and effcient computation Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * correct terminology Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * style(pre-commit): autofix * fix comments Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * fix var name Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Co-authored-by: Taekjin LEE <technolojin@gmail.com> * fix variable names Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Co-authored-by: Taekjin LEE <technolojin@gmail.com> * fix variable names Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Co-authored-by: Taekjin LEE <technolojin@gmail.com> * fix variable names Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Co-authored-by: Taekjin LEE <technolojin@gmail.com> * fix variable names Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Co-authored-by: Taekjin LEE <technolojin@gmail.com> * fix variable names Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * fix initialization Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * add verification to point_project_to_unrectified_image when loading Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * chore: add option description to project points to unrectified image Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Taekjin LEE <technolojin@gmail.com> Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Co-authored-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 308eefb commit ca3afdd

File tree

20 files changed

+522
-167
lines changed

20 files changed

+522
-167
lines changed

‎perception/autoware_image_projection_based_fusion/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ endif()
1616

1717
# Build non-CUDA dependent nodes
1818
ament_auto_add_library(${PROJECT_NAME} SHARED
19+
src/camera_projection.cpp
1920
src/fusion_node.cpp
2021
src/debugger.cpp
2122
src/utils/geometry.cpp

‎perception/autoware_image_projection_based_fusion/README.md

+6
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,12 @@ ros2 launch autoware_image_projection_based_fusion pointpainting_fusion.launch.x
6666

6767
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.
6868

69+
### Approximate camera projection
70+
71+
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.
72+
73+
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.
74+
6975
### Detail description of each fusion's algorithm is in the following links
7076

7177
| Fusion Name | Description | Detail |

‎perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml

+8-1
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,8 @@
44
timeout_ms: 70.0
55
match_threshold_ms: 50.0
66
image_buffer_size: 15
7-
point_project_to_unrectified_image: false
7+
# projection setting for each ROI whether unrectify image
8+
point_project_to_unrectified_image: [false, false, false, false, false, false]
89
debug_mode: false
910
filter_scope_min_x: -100.0
1011
filter_scope_min_y: -100.0
@@ -13,5 +14,11 @@
1314
filter_scope_max_y: 100.0
1415
filter_scope_max_z: 100.0
1516

17+
# camera cache setting for each ROI
18+
approximate_camera_projection: [true, true, true, true, true, true]
19+
# grid size in pixels
20+
approximation_grid_cell_width: 1.0
21+
approximation_grid_cell_height: 1.0
22+
1623
# debug parameters
1724
publish_processing_time_detail: false

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

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright 2024 TIER IV, Inc.
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__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_
16+
#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_
17+
18+
#define EIGEN_MPL2_ONLY
19+
20+
#include <Eigen/Core>
21+
#include <opencv2/core/core.hpp>
22+
23+
#include <sensor_msgs/msg/camera_info.hpp>
24+
25+
#include <image_geometry/pinhole_camera_model.h>
26+
27+
#include <memory>
28+
29+
namespace autoware::image_projection_based_fusion
30+
{
31+
struct PixelPos
32+
{
33+
float x;
34+
float y;
35+
};
36+
37+
class CameraProjection
38+
{
39+
public:
40+
explicit CameraProjection(
41+
const sensor_msgs::msg::CameraInfo & camera_info, const float grid_cell_width,
42+
const float grid_cell_height, const bool unrectify, const bool use_approximation);
43+
CameraProjection() : cell_width_(1.0), cell_height_(1.0), unrectify_(false) {}
44+
void initialize();
45+
std::function<bool(const cv::Point3d &, Eigen::Vector2d &)> calcImageProjectedPoint;
46+
sensor_msgs::msg::CameraInfo getCameraInfo();
47+
bool isOutsideHorizontalView(const float px, const float pz);
48+
bool isOutsideVerticalView(const float py, const float pz);
49+
bool isOutsideFOV(const float px, const float py, const float pz);
50+
51+
protected:
52+
bool calcRectifiedImageProjectedPoint(
53+
const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
54+
bool calcRawImageProjectedPoint(const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
55+
bool calcRawImageProjectedPointWithApproximation(
56+
const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
57+
void initializeCache();
58+
59+
sensor_msgs::msg::CameraInfo camera_info_;
60+
uint32_t image_height_, image_width_;
61+
double tan_h_x_, tan_h_y_;
62+
double fov_left_, fov_right_, fov_top_, fov_bottom_;
63+
64+
uint32_t cache_size_;
65+
float cell_width_;
66+
float cell_height_;
67+
float inv_cell_width_;
68+
float inv_cell_height_;
69+
int grid_width_;
70+
int grid_height_;
71+
72+
bool unrectify_;
73+
bool use_approximation_;
74+
75+
std::unique_ptr<PixelPos[]> projection_cache_;
76+
image_geometry::PinholeCameraModel camera_model_;
77+
};
78+
79+
} // namespace autoware::image_projection_based_fusion
80+
81+
#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_

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

+9-2
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_
1616
#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_
1717

18+
#include <autoware/image_projection_based_fusion/camera_projection.hpp>
1819
#include <autoware/image_projection_based_fusion/debugger.hpp>
1920
#include <autoware/universe_utils/ros/debug_publisher.hpp>
2021
#include <autoware/universe_utils/system/stop_watch.hpp>
@@ -55,6 +56,7 @@ using sensor_msgs::msg::PointCloud2;
5556
using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
5657
using tier4_perception_msgs::msg::DetectedObjectWithFeature;
5758
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
59+
using autoware::image_projection_based_fusion::CameraProjection;
5860
using autoware_perception_msgs::msg::ObjectClassification;
5961

6062
template <class TargetMsg3D, class ObjType, class Msg2D>
@@ -86,7 +88,7 @@ class FusionNode : public rclcpp::Node
8688

8789
virtual void fuseOnSingleImage(
8890
const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
89-
const sensor_msgs::msg::CameraInfo & camera_info, TargetMsg3D & output_msg) = 0;
91+
TargetMsg3D & output_msg) = 0;
9092

9193
// set args if you need
9294
virtual void postprocess(TargetMsg3D & output_msg);
@@ -100,11 +102,16 @@ class FusionNode : public rclcpp::Node
100102
tf2_ros::Buffer tf_buffer_;
101103
tf2_ros::TransformListener tf_listener_;
102104

103-
bool point_project_to_unrectified_image_{false};
105+
std::vector<bool> point_project_to_unrectified_image_;
104106

105107
// camera_info
106108
std::map<std::size_t, sensor_msgs::msg::CameraInfo> camera_info_map_;
107109
std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
110+
// camera projection
111+
std::vector<CameraProjection> camera_projectors_;
112+
std::vector<bool> approx_camera_projection_;
113+
float approx_grid_cell_w_size_;
114+
float approx_grid_cell_h_size_;
108115

109116
rclcpp::TimerBase::SharedPtr timer_;
110117
double timeout_ms_{};

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

-3
Original file line numberDiff line numberDiff line change
@@ -53,15 +53,12 @@ class PointPaintingFusionNode
5353
void fuseOnSingleImage(
5454
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
5555
const DetectedObjectsWithFeature & input_roi_msg,
56-
const sensor_msgs::msg::CameraInfo & camera_info,
5756
sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
5857

5958
void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
6059

6160
rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
6261

63-
std::vector<double> tan_h_; // horizontal field of view
64-
6562
int omp_num_threads_{1};
6663
float score_threshold_{0.0};
6764
std::vector<std::string> class_names_;

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

-1
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,6 @@ class RoiClusterFusionNode
3838
void fuseOnSingleImage(
3939
const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id,
4040
const DetectedObjectsWithFeature & input_roi_msg,
41-
const sensor_msgs::msg::CameraInfo & camera_info,
4241
DetectedObjectsWithFeature & output_cluster_msg) override;
4342

4443
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

+3-5
Original file line numberDiff line numberDiff line change
@@ -42,13 +42,11 @@ class RoiDetectedObjectFusionNode
4242

4343
void fuseOnSingleImage(
4444
const DetectedObjects & input_object_msg, const std::size_t image_id,
45-
const DetectedObjectsWithFeature & input_roi_msg,
46-
const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override;
45+
const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override;
4746

4847
std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
49-
const DetectedObjects & input_object_msg, const double image_width, const double image_height,
50-
const Eigen::Affine3d & object2camera_affine,
51-
const image_geometry::PinholeCameraModel & pinhole_camera_model);
48+
const DetectedObjects & input_object_msg, const std::size_t & image_id,
49+
const Eigen::Affine3d & object2camera_affine);
5250

5351
void fuseObjectsOnImage(
5452
const DetectedObjects & input_object_msg,

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

+1-2
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,7 @@ class RoiPointCloudFusionNode
4949

5050
void fuseOnSingleImage(
5151
const PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
52-
const DetectedObjectsWithFeature & input_roi_msg,
53-
const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override;
52+
const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override;
5453
bool out_of_scope(const DetectedObjectWithFeature & obj) override;
5554
};
5655

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
6161

6262
void fuseOnSingleImage(
6363
const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask,
64-
const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override;
64+
PointCloud2 & output_pointcloud_msg) override;
6565

6666
bool out_of_scope(const PointCloud2 & filtered_cloud) override;
6767
inline void copyPointCloud(

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

-4
Original file line numberDiff line numberDiff line change
@@ -65,10 +65,6 @@ struct PointData
6565

6666
bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info);
6767

68-
Eigen::Vector2d calcRawImageProjectedPoint(
69-
const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d,
70-
const bool & unrectify = false);
71-
7268
std::optional<geometry_msgs::msg::TransformStamped> getTransformStamped(
7369
const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
7470
const std::string & source_frame_id, const rclcpp::Time & time);

‎perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json

+20
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,11 @@
3131
"default": 15,
3232
"minimum": 1
3333
},
34+
"point_project_to_unrectified_image": {
35+
"type": "array",
36+
"description": "An array of options indicating whether to project point to unrectified image or not.",
37+
"default": [false, false, false, false, false, false]
38+
},
3439
"debug_mode": {
3540
"type": "boolean",
3641
"description": "Whether to debug or not.",
@@ -65,6 +70,21 @@
6570
"type": "number",
6671
"description": "Maximum z position [m].",
6772
"default": 100.0
73+
},
74+
"approximate_camera_projection": {
75+
"type": "array",
76+
"description": "An array of options indicating whether to use approximated projection for each camera.",
77+
"default": [true, true, true, true, true, true]
78+
},
79+
"approximation_grid_cell_width": {
80+
"type": "number",
81+
"description": "The width of grid cell used in approximated projection [pixel].",
82+
"default": 1.0
83+
},
84+
"approximation_grid_cell_height": {
85+
"type": "number",
86+
"description": "The height of grid cell used in approximated projection [pixel].",
87+
"default": 1.0
6888
}
6989
}
7090
}

0 commit comments

Comments
 (0)