diff --git a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml index 99d85089befb8..662362c4fd7c5 100644 --- a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml +++ b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml @@ -4,6 +4,7 @@ timeout_ms: 70.0 match_threshold_ms: 50.0 image_buffer_size: 15 + point_project_to_unrectified_image: false debug_mode: false filter_scope_min_x: -100.0 filter_scope_min_y: -100.0 diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index d4e498212ac36..c9b4f1f7b903f 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -99,6 +99,8 @@ class FusionNode : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + bool point_project_to_unrectified_image_{false}; + // camera_info std::map camera_info_map_; std::vector::SharedPtr> camera_info_subs_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 058a5994ab8ad..44679f7509687 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -66,7 +66,8 @@ 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 image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d, + const bool & unrectify = false); std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 6cc4a0c23adcc..16a089fcc6d09 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -127,6 +127,8 @@ FusionNode::FusionNode( debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } + point_project_to_unrectified_image_ = + declare_parameter("point_project_to_unrectified_image"); // initialize debug tool { diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 03a950f68bbd2..e581149f8e018 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -342,8 +342,8 @@ dc | dc dc dc dc ||zc| continue; } // project - Eigen::Vector2d projected_point = - calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z)); + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(p_x, p_y, p_z), point_project_to_unrectified_image_); // iterate 2d bbox for (const auto & feature_object : objects) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 0a69959aecf5c..e925efdc0afcb 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -133,8 +133,9 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = - calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z)); + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z), + point_project_to_unrectified_image_); if ( 0 <= static_cast(projected_point.x()) && static_cast(projected_point.x()) <= static_cast(camera_info.width) - 1 && diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 39835b465d8af..4e224c554adac 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -149,7 +149,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( } Eigen::Vector2d proj_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z())); + pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()), + point_project_to_unrectified_image_); min_x = std::min(proj_point.x(), min_x); min_y = std::min(proj_point.y(), min_y); diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index e10c7e94f4dae..bc7832a15741e 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -145,7 +145,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( continue; } Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), + point_project_to_unrectified_image_); for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); const auto & check_roi = feature_obj.feature.roi; diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 1965aca86074b..430b452b391f6 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -137,7 +137,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), + point_project_to_unrectified_image_); bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && projected_point.y() > 0 && projected_point.y() < camera_info.height; diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 1f67d2e1cc86f..b991d206eceb4 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -41,10 +41,14 @@ bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) } Eigen::Vector2d calcRawImageProjectedPoint( - const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d) + const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d, + const bool & unrectify) { const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d); + if (!unrectify) { + return Eigen::Vector2d(rectified_image_point.x, rectified_image_point.y); + } const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point); return Eigen::Vector2d(raw_image_point.x, raw_image_point.y);