From f72d5ea681a3d4e0ed45ca406cc5fb0127df3dc6 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Thu, 12 Dec 2024 15:07:47 +0900 Subject: [PATCH 1/4] add timekeeper Signed-off-by: a-maumau --- .../config/fusion_common.param.yaml | 3 + .../fusion_node.hpp | 6 + .../roi_cluster_fusion/node.hpp | 1 + .../roi_pointcloud_fusion/node.hpp | 2 + .../segmentation_pointcloud_fusion/node.hpp | 1 + .../src/fusion_node.cpp | 129 +++++++---- .../src/pointpainting_fusion/node.cpp | 109 +++++---- .../src/roi_cluster_fusion/node.cpp | 216 ++++++++++-------- .../src/roi_detected_object_fusion/node.cpp | 25 +- .../src/roi_pointcloud_fusion/node.cpp | 130 +++++++---- .../segmentation_pointcloud_fusion/node.cpp | 83 ++++--- 11 files changed, 445 insertions(+), 260 deletions(-) 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 662362c4fd7c5..347cb57b484e9 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 @@ -12,3 +12,6 @@ filter_scope_max_x: 100.0 filter_scope_max_y: 100.0 filter_scope_max_z: 100.0 + + # debug parameters + publish_processing_time_detail: false 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 c9b4f1f7b903f..04ecf89faa0d4 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 @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -142,6 +143,11 @@ class FusionNode : public rclcpp::Node /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + + // timekeeper + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr time_keeper_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 2c0283a190023..be05c0a1c4bc6 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -53,6 +53,7 @@ class RoiClusterFusionNode double fusion_distance_; double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj) override; double cal_iou_by_mode( diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 4de49df61a071..9baf754e224a7 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -19,8 +19,10 @@ #include +#include #include #include + namespace autoware::image_projection_based_fusion { class RoiPointCloudFusionNode diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index b09e9521bdcdb..0bec3195bb402 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include 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 60406652310dd..0b4fac76d30be 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -42,6 +42,7 @@ static double processing_time_ms = 0; namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; template FusionNode::FusionNode( @@ -130,6 +131,17 @@ FusionNode::FusionNode( debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } + + // time keeper + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); + } + point_project_to_unrectified_image_ = declare_parameter("point_project_to_unrectified_image"); @@ -170,6 +182,9 @@ template void FusionNode::subCallback( const typename TargetMsg3D::ConstSharedPtr input_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); timer_->cancel(); @@ -217,60 +232,68 @@ void FusionNode::subCallback( int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; - // if matching rois exist, fuseOnSingle - // please ask maintainers before parallelize this loop because debugger is not thread safe - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - continue; - } - - if ((cached_roi_msgs_.at(roi_i)).size() > 0) { - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - - for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); - int64_t interval = abs(static_cast(k) - new_stamp); - - if ( - interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = k; - } else if ( - static_cast(k) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(k)); - } - } + { // if matching rois exist, fuseOnSingle + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("check matched rois", *time_keeper_); - // remove outdated stamps - for (auto stamp : outdate_stamps) { - (cached_roi_msgs_.at(roi_i)).erase(stamp); + // please ask maintainers before parallelize this loop because debugger is not thread safe + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); + continue; } - // fuseOnSingle - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); + if ((cached_roi_msgs_.at(roi_i)).size() > 0) { + int64_t min_interval = 1e9; + int64_t matched_stamp = -1; + std::list outdate_stamps; + + for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { + int64_t new_stamp = + timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); + int64_t interval = abs(static_cast(k) - new_stamp); + + if ( + interval <= min_interval && + interval <= match_threshold_ms_ * static_cast(1e6)) { + min_interval = interval; + matched_stamp = k; + } else if ( + static_cast(k) < new_stamp && + interval > match_threshold_ms_ * static_cast(1e6)) { + outdate_stamps.push_back(static_cast(k)); + } } - fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), - camera_info_map_.at(roi_i), *output_msg); - (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); - is_fused_.at(roi_i) = true; + // remove outdated stamps + for (auto stamp : outdate_stamps) { + (cached_roi_msgs_.at(roi_i)).erase(stamp); + } - // add timestamp interval for debug - if (debug_publisher_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); + // fuseOnSingle + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); + } + + fuseOnSingleImage( + *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), + camera_info_map_.at(roi_i), *output_msg); + (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); + is_fused_.at(roi_i) = true; + + // add timestamp interval for debug + if (debug_publisher_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", + timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - input_offset_ms_.at(roi_i)); + } } } } @@ -279,6 +302,10 @@ void FusionNode::subCallback( // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current // Msg if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish message", *time_keeper_); + timer_->cancel(); postprocess(*output_msg); publish(*output_msg); @@ -306,6 +333,9 @@ template void FusionNode::roiCallback( const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + stop_watch_ptr_->toc("processing_time", true); int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + @@ -378,6 +408,9 @@ void FusionNode::postprocess(TargetMsg3D & output_msg template void FusionNode::timer_callback() { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + using std::chrono_literals::operator""ms; timer_->cancel(); if (mutex_cached_msgs_.try_lock()) { 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 4e0f88e2134ac..24d4fde75c772 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 @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,7 @@ namespace { +using autoware::universe_utils::ScopedTimeTrack; Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) { @@ -205,6 +207,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); @@ -263,6 +268,9 @@ void PointPaintingFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); @@ -283,6 +291,10 @@ void PointPaintingFusionNode::fuseOnSingleImage( // geometry_msgs::msg::TransformStamped transform_stamped; Eigen::Affine3f lidar2cam_affine; { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("calculate affine transform", *time_keeper_); + const auto transform_stamped_optional = getTransformStamped( tf_buffer_, /*target*/ input_roi_msg.header.frame_id, /*source*/ painted_pointcloud_msg.header.frame_id, input_roi_msg.header.stamp); @@ -322,51 +334,59 @@ dc | dc dc dc dc ||zc| auto objects = input_roi_msg.feature_objects; int iterations = painted_pointcloud_msg.data.size() / painted_pointcloud_msg.point_step; - // iterate points - // Requires 'OMP_NUM_THREADS=N' - omp_set_num_threads(omp_num_threads_); + + { // iterate points and calculate camera projections + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = + std::make_unique("calculate camera projection", *time_keeper_); + + // Requires 'OMP_NUM_THREADS=N' + omp_set_num_threads(omp_num_threads_); #pragma omp parallel for - for (int i = 0; i < iterations; i++) { - int stride = p_step * i; - unsigned char * data = &painted_pointcloud_msg.data[0]; - unsigned char * output = &painted_pointcloud_msg.data[0]; - // cppcheck-suppress-begin invalidPointerCast - float p_x = *reinterpret_cast(&data[stride + x_offset]); - float p_y = *reinterpret_cast(&data[stride + y_offset]); - float p_z = *reinterpret_cast(&data[stride + z_offset]); - // cppcheck-suppress-end invalidPointerCast - point_lidar << p_x, p_y, p_z; - point_camera = lidar2cam_affine * point_lidar; - p_x = point_camera.x(); - p_y = point_camera.y(); - p_z = point_camera.z(); - - if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { - continue; - } - // project - 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) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; - } + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &painted_pointcloud_msg.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress-begin invalidPointerCast + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + // cppcheck-suppress-end invalidPointerCast + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + + if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { + continue; } + // project + 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) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + } + } #if 0 - // Parallelizing loop don't support push_back - if (debugger_) { - debug_image_points.push_back(projected_point); - } + // Parallelizing loop don't support push_back + if (debugger_) { + debug_image_points.push_back(projected_point); + } #endif + } } } @@ -375,6 +395,10 @@ dc | dc dc dc dc ||zc| } if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); @@ -383,6 +407,9 @@ dc | dc dc dc dc ||zc| void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto objects_sub_count = obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { 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 9a94613a2b788..b427594a21751 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 @@ -16,12 +16,14 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -36,6 +38,7 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode( @@ -55,6 +58,9 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + // reset cluster semantic type if (!use_cluster_semantic_type_) { for (auto & feature_object : output_cluster_msg.feature_objects) { @@ -67,6 +73,9 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (!remove_unknown_) { return; } @@ -89,6 +98,9 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (!checkCameraInfo(camera_info)) return; image_geometry::PinholeCameraModel pinhole_camera_model; @@ -110,119 +122,137 @@ void RoiClusterFusionNode::fuseOnSingleImage( } std::map m_cluster_roi; - for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { - if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { - continue; - } - if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { - continue; - } + { // calculate camera projections and create ROI clusters + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = + std::make_unique("calculate camera projection", *time_keeper_); - // filter point out of scope - if (debugger_ && out_of_scope(input_cluster_msg.feature_objects.at(i))) { - continue; - } + for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { + if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { + continue; + } - sensor_msgs::msg::PointCloud2 transformed_cluster; - tf2::doTransform( - input_cluster_msg.feature_objects.at(i).feature.cluster, transformed_cluster, - transform_stamped); - - int min_x(camera_info.width), min_y(camera_info.height), max_x(0), max_y(0); - std::vector projected_points; - projected_points.reserve(transformed_cluster.data.size()); - for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cluster, "x"), - iter_y(transformed_cluster, "y"), iter_z(transformed_cluster, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (*iter_z <= 0.0) { + if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { continue; } - 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 && - 0 <= static_cast(projected_point.y()) && - static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { - min_x = std::min(static_cast(projected_point.x()), min_x); - min_y = std::min(static_cast(projected_point.y()), min_y); - max_x = std::max(static_cast(projected_point.x()), max_x); - max_y = std::max(static_cast(projected_point.y()), max_y); - projected_points.push_back(projected_point); - if (debugger_) debugger_->obstacle_points_.push_back(projected_point); + // filter point out of scope + if (debugger_ && out_of_scope(input_cluster_msg.feature_objects.at(i))) { + continue; } - } - if (projected_points.empty()) { - continue; - } - sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; - roi.x_offset = min_x; - roi.y_offset = min_y; - roi.width = max_x - min_x; - roi.height = max_y - min_y; - m_cluster_roi.insert(std::make_pair(i, roi)); - if (debugger_) debugger_->obstacle_rois_.push_back(roi); + sensor_msgs::msg::PointCloud2 transformed_cluster; + tf2::doTransform( + input_cluster_msg.feature_objects.at(i).feature.cluster, transformed_cluster, + transform_stamped); + + int min_x(camera_info.width), min_y(camera_info.height), max_x(0), max_y(0); + std::vector projected_points; + projected_points.reserve(transformed_cluster.data.size()); + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cluster, "x"), + iter_y(transformed_cluster, "y"), iter_z(transformed_cluster, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + if (*iter_z <= 0.0) { + continue; + } + + 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 && + 0 <= static_cast(projected_point.y()) && + static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { + min_x = std::min(static_cast(projected_point.x()), min_x); + min_y = std::min(static_cast(projected_point.y()), min_y); + max_x = std::max(static_cast(projected_point.x()), max_x); + max_y = std::max(static_cast(projected_point.y()), max_y); + projected_points.push_back(projected_point); + if (debugger_) debugger_->obstacle_points_.push_back(projected_point); + } + } + if (projected_points.empty()) { + continue; + } + + sensor_msgs::msg::RegionOfInterest roi; + // roi.do_rectify = m_camera_info_.at(id).do_rectify; + roi.x_offset = min_x; + roi.y_offset = min_y; + roi.width = max_x - min_x; + roi.height = max_y - min_y; + m_cluster_roi.insert(std::make_pair(i, roi)); + if (debugger_) debugger_->obstacle_rois_.push_back(roi); + } } - for (const auto & feature_obj : input_roi_msg.feature_objects) { - int index = -1; - bool associated = false; - double max_iou = 0.0; - const bool is_roi_label_known = - feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; - for (const auto & cluster_map : m_cluster_roi) { - double iou(0.0); - bool is_use_non_trust_object_iou_mode = is_far_enough( - input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); - auto image_roi = feature_obj.feature.roi; - auto cluster_roi = cluster_map.second; - sanitizeROI(image_roi, camera_info.width, camera_info.height); - sanitizeROI(cluster_roi, camera_info.width, camera_info.height); - if (is_use_non_trust_object_iou_mode || is_roi_label_known) { - iou = cal_iou_by_mode(cluster_roi, image_roi, non_trust_object_iou_mode_); - } else { - iou = cal_iou_by_mode(cluster_roi, image_roi, trust_object_iou_mode_); + { // search matching ROI and update label + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("compare ROI", *time_keeper_); + + for (const auto & feature_obj : input_roi_msg.feature_objects) { + int index = -1; + bool associated = false; + double max_iou = 0.0; + const bool is_roi_label_known = + feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; + for (const auto & cluster_map : m_cluster_roi) { + double iou(0.0); + bool is_use_non_trust_object_iou_mode = is_far_enough( + input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); + auto image_roi = feature_obj.feature.roi; + auto cluster_roi = cluster_map.second; + sanitizeROI(image_roi, camera_info.width, camera_info.height); + sanitizeROI(cluster_roi, camera_info.width, camera_info.height); + if (is_use_non_trust_object_iou_mode || is_roi_label_known) { + iou = cal_iou_by_mode(cluster_roi, image_roi, non_trust_object_iou_mode_); + } else { + iou = cal_iou_by_mode(cluster_roi, image_roi, trust_object_iou_mode_); + } + + const bool passed_inside_cluster_gate = + only_allow_inside_cluster_ ? is_inside(image_roi, cluster_roi, roi_scale_factor_) : true; + if (max_iou < iou && passed_inside_cluster_gate) { + index = cluster_map.first; + max_iou = iou; + associated = true; + } } - const bool passed_inside_cluster_gate = - only_allow_inside_cluster_ ? is_inside(image_roi, cluster_roi, roi_scale_factor_) : true; - if (max_iou < iou && passed_inside_cluster_gate) { - index = cluster_map.first; - max_iou = iou; - associated = true; + if (!associated) { + continue; } - } - if (!associated) { - continue; - } - - if (!output_cluster_msg.feature_objects.empty()) { - auto & fused_object = output_cluster_msg.feature_objects.at(index).object; - const bool is_roi_existence_prob_higher = - fused_object.existence_probability <= feature_obj.object.existence_probability; - const bool is_roi_iou_over_threshold = - (is_roi_label_known && iou_threshold_ < max_iou) || - (!is_roi_label_known && unknown_iou_threshold_ < max_iou); - - if (is_roi_iou_over_threshold && is_roi_existence_prob_higher) { - fused_object.classification = feature_obj.object.classification; - // Update existence_probability for fused objects - fused_object.existence_probability = - std::clamp(feature_obj.object.existence_probability, min_roi_existence_prob_, 1.0f); + if (!output_cluster_msg.feature_objects.empty()) { + auto & fused_object = output_cluster_msg.feature_objects.at(index).object; + const bool is_roi_existence_prob_higher = + fused_object.existence_probability <= feature_obj.object.existence_probability; + const bool is_roi_iou_over_threshold = + (is_roi_label_known && iou_threshold_ < max_iou) || + (!is_roi_label_known && unknown_iou_threshold_ < max_iou); + + if (is_roi_iou_over_threshold && is_roi_existence_prob_higher) { + fused_object.classification = feature_obj.object.classification; + // Update existence_probability for fused objects + fused_object.existence_probability = + std::clamp(feature_obj.object.existence_probability, min_roi_existence_prob_, 1.0f); + } } + if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); + if (debugger_) debugger_->max_iou_for_image_rois_.push_back(max_iou); } - if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); - if (debugger_) debugger_->max_iou_for_image_rois_.push_back(max_iou); } // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + debugger_->publishImage(image_id, input_roi_msg.header.stamp); } } 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 134dd7bad9129..39a85480c1f4c 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 @@ -18,14 +18,17 @@ #include #include +#include #include #include +#include #include #include namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode( @@ -49,6 +52,9 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; passthrough_object_flags.resize(output_msg.objects.size()); fused_object_flags.resize(output_msg.objects.size()); @@ -81,10 +87,17 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg __attribute__((unused))) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (!checkCameraInfo(camera_info)) return; Eigen::Affine3d object2camera_affine; - { + { // calculate affine transform + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("calculate affine transform", *time_keeper_); + const auto transform_stamped_optional = getTransformStamped( tf_buffer_, /*target*/ input_roi_msg.header.frame_id, /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp); @@ -103,6 +116,10 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); @@ -117,6 +134,9 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( const Eigen::Affine3d & object2camera_affine, const image_geometry::PinholeCameraModel & pinhole_camera_model) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::map object_roi_map; int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; @@ -205,6 +225,9 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( const std::vector & image_rois, const std::map & object_roi_map) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { 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 f9eb4ef909282..a96118e35e3fd 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 @@ -17,6 +17,9 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include + +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -31,6 +34,8 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode( "roi_pointcloud_fusion", options) @@ -47,12 +52,18 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt void RoiPointCloudFusionNode::preprocess( __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return; } void RoiPointCloudFusionNode::postprocess( __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + pub_objects_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { @@ -81,6 +92,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; } @@ -133,60 +147,86 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( std::vector clusters; std::vector clusters_data_size; clusters.resize(output_objs.size()); - for (auto & cluster : clusters) { - cluster.point_step = input_pointcloud_msg.point_step; - cluster.height = input_pointcloud_msg.height; - cluster.fields = input_pointcloud_msg.fields; - cluster.data.resize(max_cluster_size_ * input_pointcloud_msg.point_step); - clusters_data_size.push_back(0); - } - for (size_t offset = 0; offset < input_pointcloud_msg.data.size(); offset += point_step) { - const float transformed_x = - *reinterpret_cast(&transformed_cloud.data[offset + x_offset]); - const float transformed_y = - *reinterpret_cast(&transformed_cloud.data[offset + y_offset]); - const float transformed_z = - *reinterpret_cast(&transformed_cloud.data[offset + z_offset]); - if (transformed_z <= 0.0) { - continue; + { // copy cluster + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("copy cluster", *time_keeper_); + + for (auto & cluster : clusters) { + cluster.point_step = input_pointcloud_msg.point_step; + cluster.height = input_pointcloud_msg.height; + cluster.fields = input_pointcloud_msg.fields; + cluster.data.resize(max_cluster_size_ * input_pointcloud_msg.point_step); + clusters_data_size.push_back(0); } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - 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; - auto & cluster = clusters.at(i); - - if ( - clusters_data_size.at(i) >= - static_cast(max_cluster_size_) * static_cast(point_step)) { + } + + { // calculate camera projections + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = + std::make_unique("calculate camera projection", *time_keeper_); + + for (size_t offset = 0; offset < input_pointcloud_msg.data.size(); offset += point_step) { + const float transformed_x = + *reinterpret_cast(&transformed_cloud.data[offset + x_offset]); + const float transformed_y = + *reinterpret_cast(&transformed_cloud.data[offset + y_offset]); + const float transformed_z = + *reinterpret_cast(&transformed_cloud.data[offset + z_offset]); + if (transformed_z <= 0.0) { continue; } - if ( - check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && - check_roi.x_offset + check_roi.width >= projected_point.x() && - check_roi.y_offset + check_roi.height >= projected_point.y()) { - std::memcpy( - &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step); - clusters_data_size.at(i) += point_step; + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + 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; + auto & cluster = clusters.at(i); + + if ( + clusters_data_size.at(i) >= + static_cast(max_cluster_size_) * static_cast(point_step)) { + continue; + } + if ( + check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && + check_roi.x_offset + check_roi.width >= projected_point.x() && + check_roi.y_offset + check_roi.height >= projected_point.y()) { + std::memcpy( + &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], + point_step); + clusters_data_size.at(i) += point_step; + } } - } - if (debugger_) { - // add all points inside image to debug - if ( - projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height) { - debug_image_points.push_back(projected_point); + if (debugger_) { + // add all points inside image to debug + if ( + projected_point.x() > 0 && projected_point.x() < camera_info.width && + projected_point.y() > 0 && projected_point.y() < camera_info.height) { + debug_image_points.push_back(projected_point); + } } } } - // refine and update output_fused_objects_ - updateOutputFusedObjects( - output_objs, clusters, clusters_data_size, input_pointcloud_msg, input_roi_msg.header, - tf_buffer_, min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); + { // refine and update output_fused_objects_ + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("updateOutputFusedObjects", *time_keeper_); + + updateOutputFusedObjects( + output_objs, clusters, clusters_data_size, input_pointcloud_msg, input_roi_msg.header, + tf_buffer_, min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, + output_fused_objects_); + } + if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); 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 e678a956bc64e..d15852e7359c8 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 @@ -17,6 +17,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include #include #include @@ -32,6 +33,8 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode("segmentation_pointcloud_fusion", options) { @@ -49,11 +52,17 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return; } void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + auto original_cloud = std::make_shared(pointcloud_msg); int point_step = original_cloud->point_step; @@ -82,6 +91,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, __attribute__((unused)) PointCloud2 & output_cloud) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; } @@ -126,39 +138,46 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( int y_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; - for (size_t global_offset = 0; global_offset < transformed_cloud.data.size(); - global_offset += point_step) { - float transformed_x = - *reinterpret_cast(&transformed_cloud.data[global_offset + x_offset]); - float transformed_y = - *reinterpret_cast(&transformed_cloud.data[global_offset + y_offset]); - float transformed_z = - *reinterpret_cast(&transformed_cloud.data[global_offset + z_offset]); - // skip filtering pointcloud behind the camera or too far from camera - if (transformed_z <= 0.0 || transformed_z > filter_distance_threshold_) { - continue; + { // calculate camera projections + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = + std::make_unique("calculate camera projection", *time_keeper_); + + for (size_t global_offset = 0; global_offset < transformed_cloud.data.size(); + global_offset += point_step) { + float transformed_x = + *reinterpret_cast(&transformed_cloud.data[global_offset + x_offset]); + float transformed_y = + *reinterpret_cast(&transformed_cloud.data[global_offset + y_offset]); + float transformed_z = + *reinterpret_cast(&transformed_cloud.data[global_offset + z_offset]); + // skip filtering pointcloud behind the camera or too far from camera + if (transformed_z <= 0.0 || transformed_z > filter_distance_threshold_) { + continue; + } + + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + 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; + if (!is_inside_image) { + continue; + } + + // skip filtering pointcloud where semantic id out of the defined list + uint8_t semantic_id = mask.at( + static_cast(projected_point.y()), static_cast(projected_point.x())); + if ( + static_cast(semantic_id) >= filter_semantic_label_target_list_.size() || + !filter_semantic_label_target_list_.at(semantic_id).second) { + continue; + } + + filter_global_offset_set_.insert(global_offset); } - - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - 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; - if (!is_inside_image) { - continue; - } - - // skip filtering pointcloud where semantic id out of the defined list - uint8_t semantic_id = mask.at( - static_cast(projected_point.y()), static_cast(projected_point.x())); - if ( - static_cast(semantic_id) >= filter_semantic_label_target_list_.size() || - !filter_semantic_label_target_list_.at(semantic_id).second) { - continue; - } - - filter_global_offset_set_.insert(global_offset); } } From 2ea07924e45023c45f23deee3a5fbfab4cc06568 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 13 Dec 2024 11:13:54 +0900 Subject: [PATCH 2/4] chore: refactor time-keeper position Signed-off-by: Taekjin LEE --- .../src/fusion_node.cpp | 110 ++++++++---------- .../src/pointpainting_fusion/node.cpp | 98 +++++++--------- .../src/roi_cluster_fusion/node.cpp | 8 +- 3 files changed, 95 insertions(+), 121 deletions(-) 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 6943c8e2cc9f8..6e55c4928d0bf 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -232,68 +232,59 @@ void FusionNode::subCallback( int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; - { // if matching rois exist, fuseOnSingle - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("check matched rois", *time_keeper_); + // please ask maintainers before parallelize this loop because debugger is not thread safe + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); + continue; + } - // please ask maintainers before parallelize this loop because debugger is not thread safe - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - continue; + if ((cached_roi_msgs_.at(roi_i)).size() > 0) { + int64_t min_interval = 1e9; + int64_t matched_stamp = -1; + std::list outdate_stamps; + + for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { + int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); + int64_t interval = abs(static_cast(k) - new_stamp); + + if ( + interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { + min_interval = interval; + matched_stamp = k; + } else if ( + static_cast(k) < new_stamp && + interval > match_threshold_ms_ * static_cast(1e6)) { + outdate_stamps.push_back(static_cast(k)); + } } - if ((cached_roi_msgs_.at(roi_i)).size() > 0) { - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - - for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = - timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); - int64_t interval = abs(static_cast(k) - new_stamp); - - if ( - interval <= min_interval && - interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = k; - } else if ( - static_cast(k) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(k)); - } - } + // remove outdated stamps + for (auto stamp : outdate_stamps) { + (cached_roi_msgs_.at(roi_i)).erase(stamp); + } - // remove outdated stamps - for (auto stamp : outdate_stamps) { - (cached_roi_msgs_.at(roi_i)).erase(stamp); + // fuseOnSingle + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); } - // fuseOnSingle - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); - } - - fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), - camera_info_map_.at(roi_i), *output_msg); - (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); - is_fused_.at(roi_i) = true; - - // add timestamp interval for debug - if (debug_publisher_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", - timestamp_interval_ms); - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); - } + fuseOnSingleImage( + *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), + camera_info_map_.at(roi_i), *output_msg); + (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); + is_fused_.at(roi_i) = true; + + // add timestamp interval for debug + if (debug_publisher_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - input_offset_ms_.at(roi_i)); } } } @@ -302,10 +293,6 @@ void FusionNode::subCallback( // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current // Msg if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("publish message", *time_keeper_); - timer_->cancel(); postprocess(*output_msg); publish(*output_msg); @@ -470,6 +457,9 @@ void FusionNode::publish(const TargetMsg3D & output_msg if (pub_ptr_->get_subscription_count() < 1) { return; } + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + pub_ptr_->publish(output_msg); } 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 80154d95eeac4..8aba6a860eae6 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 @@ -265,9 +265,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); @@ -281,6 +278,9 @@ void PointPaintingFusionNode::fuseOnSingleImage( if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::vector debug_image_rois; std::vector debug_image_points; @@ -288,10 +288,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( // geometry_msgs::msg::TransformStamped transform_stamped; Eigen::Affine3f lidar2cam_affine; { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("calculate affine transform", *time_keeper_); - const auto transform_stamped_optional = getTransformStamped( tf_buffer_, /*target*/ input_roi_msg.header.frame_id, /*source*/ painted_pointcloud_msg.header.frame_id, input_roi_msg.header.stamp); @@ -332,58 +328,50 @@ dc | dc dc dc dc ||zc| auto objects = input_roi_msg.feature_objects; int iterations = painted_pointcloud_msg.data.size() / painted_pointcloud_msg.point_step; - { // iterate points and calculate camera projections - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = - std::make_unique("calculate camera projection", *time_keeper_); - - // Requires 'OMP_NUM_THREADS=N' - omp_set_num_threads(omp_num_threads_); + // Requires 'OMP_NUM_THREADS=N' + omp_set_num_threads(omp_num_threads_); #pragma omp parallel for - for (int i = 0; i < iterations; i++) { - int stride = p_step * i; - unsigned char * data = &painted_pointcloud_msg.data[0]; - unsigned char * output = &painted_pointcloud_msg.data[0]; - // cppcheck-suppress-begin invalidPointerCast - float p_x = *reinterpret_cast(&data[stride + x_offset]); - float p_y = *reinterpret_cast(&data[stride + y_offset]); - float p_z = *reinterpret_cast(&data[stride + z_offset]); - // cppcheck-suppress-end invalidPointerCast - point_lidar << p_x, p_y, p_z; - point_camera = lidar2cam_affine * point_lidar; - p_x = point_camera.x(); - p_y = point_camera.y(); - p_z = point_camera.z(); - - if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { - continue; - } - // project - 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) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if ( - !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; - } + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &painted_pointcloud_msg.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress-begin invalidPointerCast + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + // cppcheck-suppress-end invalidPointerCast + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + + if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { + continue; + } + // project + 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) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; } + } #if 0 - // Parallelizing loop don't support push_back - if (debugger_) { - debug_image_points.push_back(projected_point); - } -#endif + // Parallelizing loop don't support push_back + if (debugger_) { + debug_image_points.push_back(projected_point); } +#endif } } 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 b427594a21751..2a70613ce556b 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 @@ -98,11 +98,11 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { + if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - if (!checkCameraInfo(camera_info)) return; - image_geometry::PinholeCameraModel pinhole_camera_model; pinhole_camera_model.fromCameraInfo(camera_info); @@ -249,10 +249,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); - debugger_->publishImage(image_id, input_roi_msg.header.stamp); } } From 500986138bc91c5b7461959f73af0c40454cb43a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 13 Dec 2024 11:50:07 +0900 Subject: [PATCH 3/4] chore: bring back a missing comment Signed-off-by: Taekjin LEE --- .../src/pointpainting_fusion/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8aba6a860eae6..4d12de2685c95 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 @@ -327,7 +327,7 @@ dc | dc dc dc dc ||zc| auto objects = input_roi_msg.feature_objects; int iterations = painted_pointcloud_msg.data.size() / painted_pointcloud_msg.point_step; - + // iterate points // Requires 'OMP_NUM_THREADS=N' omp_set_num_threads(omp_num_threads_); #pragma omp parallel for From bd823589db03d4e6cdc0a30174dc043f22ea596d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 13 Dec 2024 12:47:30 +0900 Subject: [PATCH 4/4] chore: remove redundant timekeepers Signed-off-by: Taekjin LEE --- .../src/fusion_node.cpp | 4 +- .../src/roi_cluster_fusion/node.cpp | 201 ++++++++---------- .../src/roi_detected_object_fusion/node.cpp | 10 +- .../src/roi_pointcloud_fusion/node.cpp | 116 ++++------ .../segmentation_pointcloud_fusion/node.cpp | 71 +++---- 5 files changed, 173 insertions(+), 229 deletions(-) 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 6e55c4928d0bf..bd4d57e45c582 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -232,6 +232,7 @@ void FusionNode::subCallback( int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; + // if matching rois exist, fuseOnSingle // please ask maintainers before parallelize this loop because debugger is not thread safe for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { @@ -457,9 +458,6 @@ void FusionNode::publish(const TargetMsg3D & output_msg if (pub_ptr_->get_subscription_count() < 1) { return; } - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - pub_ptr_->publish(output_msg); } 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 2a70613ce556b..7520647544684 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 @@ -123,128 +123,115 @@ void RoiClusterFusionNode::fuseOnSingleImage( std::map m_cluster_roi; - { // calculate camera projections and create ROI clusters - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = - std::make_unique("calculate camera projection", *time_keeper_); - - for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { - if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { - continue; - } + for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { + if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { + continue; + } - if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { - continue; - } + if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { + continue; + } - // filter point out of scope - if (debugger_ && out_of_scope(input_cluster_msg.feature_objects.at(i))) { - continue; - } + // filter point out of scope + if (debugger_ && out_of_scope(input_cluster_msg.feature_objects.at(i))) { + continue; + } - sensor_msgs::msg::PointCloud2 transformed_cluster; - tf2::doTransform( - input_cluster_msg.feature_objects.at(i).feature.cluster, transformed_cluster, - transform_stamped); - - int min_x(camera_info.width), min_y(camera_info.height), max_x(0), max_y(0); - std::vector projected_points; - projected_points.reserve(transformed_cluster.data.size()); - for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cluster, "x"), - iter_y(transformed_cluster, "y"), iter_z(transformed_cluster, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (*iter_z <= 0.0) { - continue; - } - - 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 && - 0 <= static_cast(projected_point.y()) && - static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { - min_x = std::min(static_cast(projected_point.x()), min_x); - min_y = std::min(static_cast(projected_point.y()), min_y); - max_x = std::max(static_cast(projected_point.x()), max_x); - max_y = std::max(static_cast(projected_point.y()), max_y); - projected_points.push_back(projected_point); - if (debugger_) debugger_->obstacle_points_.push_back(projected_point); - } - } - if (projected_points.empty()) { + sensor_msgs::msg::PointCloud2 transformed_cluster; + tf2::doTransform( + input_cluster_msg.feature_objects.at(i).feature.cluster, transformed_cluster, + transform_stamped); + + int min_x(camera_info.width), min_y(camera_info.height), max_x(0), max_y(0); + std::vector projected_points; + projected_points.reserve(transformed_cluster.data.size()); + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cluster, "x"), + iter_y(transformed_cluster, "y"), iter_z(transformed_cluster, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + if (*iter_z <= 0.0) { continue; } - sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; - roi.x_offset = min_x; - roi.y_offset = min_y; - roi.width = max_x - min_x; - roi.height = max_y - min_y; - m_cluster_roi.insert(std::make_pair(i, roi)); - if (debugger_) debugger_->obstacle_rois_.push_back(roi); + 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 && + 0 <= static_cast(projected_point.y()) && + static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { + min_x = std::min(static_cast(projected_point.x()), min_x); + min_y = std::min(static_cast(projected_point.y()), min_y); + max_x = std::max(static_cast(projected_point.x()), max_x); + max_y = std::max(static_cast(projected_point.y()), max_y); + projected_points.push_back(projected_point); + if (debugger_) debugger_->obstacle_points_.push_back(projected_point); + } } + if (projected_points.empty()) { + continue; + } + + sensor_msgs::msg::RegionOfInterest roi; + // roi.do_rectify = m_camera_info_.at(id).do_rectify; + roi.x_offset = min_x; + roi.y_offset = min_y; + roi.width = max_x - min_x; + roi.height = max_y - min_y; + m_cluster_roi.insert(std::make_pair(i, roi)); + if (debugger_) debugger_->obstacle_rois_.push_back(roi); } - { // search matching ROI and update label - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("compare ROI", *time_keeper_); - - for (const auto & feature_obj : input_roi_msg.feature_objects) { - int index = -1; - bool associated = false; - double max_iou = 0.0; - const bool is_roi_label_known = - feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; - for (const auto & cluster_map : m_cluster_roi) { - double iou(0.0); - bool is_use_non_trust_object_iou_mode = is_far_enough( - input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); - auto image_roi = feature_obj.feature.roi; - auto cluster_roi = cluster_map.second; - sanitizeROI(image_roi, camera_info.width, camera_info.height); - sanitizeROI(cluster_roi, camera_info.width, camera_info.height); - if (is_use_non_trust_object_iou_mode || is_roi_label_known) { - iou = cal_iou_by_mode(cluster_roi, image_roi, non_trust_object_iou_mode_); - } else { - iou = cal_iou_by_mode(cluster_roi, image_roi, trust_object_iou_mode_); - } - - const bool passed_inside_cluster_gate = - only_allow_inside_cluster_ ? is_inside(image_roi, cluster_roi, roi_scale_factor_) : true; - if (max_iou < iou && passed_inside_cluster_gate) { - index = cluster_map.first; - max_iou = iou; - associated = true; - } + for (const auto & feature_obj : input_roi_msg.feature_objects) { + int index = -1; + bool associated = false; + double max_iou = 0.0; + const bool is_roi_label_known = + feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; + for (const auto & cluster_map : m_cluster_roi) { + double iou(0.0); + bool is_use_non_trust_object_iou_mode = is_far_enough( + input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); + auto image_roi = feature_obj.feature.roi; + auto cluster_roi = cluster_map.second; + sanitizeROI(image_roi, camera_info.width, camera_info.height); + sanitizeROI(cluster_roi, camera_info.width, camera_info.height); + if (is_use_non_trust_object_iou_mode || is_roi_label_known) { + iou = cal_iou_by_mode(cluster_roi, image_roi, non_trust_object_iou_mode_); + } else { + iou = cal_iou_by_mode(cluster_roi, image_roi, trust_object_iou_mode_); } - if (!associated) { - continue; + const bool passed_inside_cluster_gate = + only_allow_inside_cluster_ ? is_inside(image_roi, cluster_roi, roi_scale_factor_) : true; + if (max_iou < iou && passed_inside_cluster_gate) { + index = cluster_map.first; + max_iou = iou; + associated = true; } + } + + if (!associated) { + continue; + } - if (!output_cluster_msg.feature_objects.empty()) { - auto & fused_object = output_cluster_msg.feature_objects.at(index).object; - const bool is_roi_existence_prob_higher = - fused_object.existence_probability <= feature_obj.object.existence_probability; - const bool is_roi_iou_over_threshold = - (is_roi_label_known && iou_threshold_ < max_iou) || - (!is_roi_label_known && unknown_iou_threshold_ < max_iou); - - if (is_roi_iou_over_threshold && is_roi_existence_prob_higher) { - fused_object.classification = feature_obj.object.classification; - // Update existence_probability for fused objects - fused_object.existence_probability = - std::clamp(feature_obj.object.existence_probability, min_roi_existence_prob_, 1.0f); - } + if (!output_cluster_msg.feature_objects.empty()) { + auto & fused_object = output_cluster_msg.feature_objects.at(index).object; + const bool is_roi_existence_prob_higher = + fused_object.existence_probability <= feature_obj.object.existence_probability; + const bool is_roi_iou_over_threshold = + (is_roi_label_known && iou_threshold_ < max_iou) || + (!is_roi_label_known && unknown_iou_threshold_ < max_iou); + + if (is_roi_iou_over_threshold && is_roi_existence_prob_higher) { + fused_object.classification = feature_obj.object.classification; + // Update existence_probability for fused objects + fused_object.existence_probability = + std::clamp(feature_obj.object.existence_probability, min_roi_existence_prob_, 1.0f); } - if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); - if (debugger_) debugger_->max_iou_for_image_rois_.push_back(max_iou); } + if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); + if (debugger_) debugger_->max_iou_for_image_rois_.push_back(max_iou); } // note: debug objects are safely cleared in fusion_node.cpp 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 9215de99be8bc..035bc259ab73c 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 @@ -93,11 +93,7 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( if (!checkCameraInfo(camera_info)) return; Eigen::Affine3d object2camera_affine; - { // calculate affine transform - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("calculate affine transform", *time_keeper_); - + { const auto transform_stamped_optional = getTransformStamped( tf_buffer_, /*target*/ input_roi_msg.header.frame_id, /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp); @@ -116,10 +112,6 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); - debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); 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 a96118e35e3fd..206a5f77a0235 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 @@ -147,86 +147,60 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( std::vector clusters; std::vector clusters_data_size; clusters.resize(output_objs.size()); - { // copy cluster - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("copy cluster", *time_keeper_); - - for (auto & cluster : clusters) { - cluster.point_step = input_pointcloud_msg.point_step; - cluster.height = input_pointcloud_msg.height; - cluster.fields = input_pointcloud_msg.fields; - cluster.data.resize(max_cluster_size_ * input_pointcloud_msg.point_step); - clusters_data_size.push_back(0); - } + for (auto & cluster : clusters) { + cluster.point_step = input_pointcloud_msg.point_step; + cluster.height = input_pointcloud_msg.height; + cluster.fields = input_pointcloud_msg.fields; + cluster.data.resize(max_cluster_size_ * input_pointcloud_msg.point_step); + clusters_data_size.push_back(0); } - - { // calculate camera projections - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = - std::make_unique("calculate camera projection", *time_keeper_); - - for (size_t offset = 0; offset < input_pointcloud_msg.data.size(); offset += point_step) { - const float transformed_x = - *reinterpret_cast(&transformed_cloud.data[offset + x_offset]); - const float transformed_y = - *reinterpret_cast(&transformed_cloud.data[offset + y_offset]); - const float transformed_z = - *reinterpret_cast(&transformed_cloud.data[offset + z_offset]); - if (transformed_z <= 0.0) { + for (size_t offset = 0; offset < input_pointcloud_msg.data.size(); offset += point_step) { + const float transformed_x = + *reinterpret_cast(&transformed_cloud.data[offset + x_offset]); + const float transformed_y = + *reinterpret_cast(&transformed_cloud.data[offset + y_offset]); + const float transformed_z = + *reinterpret_cast(&transformed_cloud.data[offset + z_offset]); + if (transformed_z <= 0.0) { + continue; + } + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + 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; + auto & cluster = clusters.at(i); + + if ( + clusters_data_size.at(i) >= + static_cast(max_cluster_size_) * static_cast(point_step)) { continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - 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; - auto & cluster = clusters.at(i); - - if ( - clusters_data_size.at(i) >= - static_cast(max_cluster_size_) * static_cast(point_step)) { - continue; - } - if ( - check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && - check_roi.x_offset + check_roi.width >= projected_point.x() && - check_roi.y_offset + check_roi.height >= projected_point.y()) { - std::memcpy( - &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], - point_step); - clusters_data_size.at(i) += point_step; - } + if ( + check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && + check_roi.x_offset + check_roi.width >= projected_point.x() && + check_roi.y_offset + check_roi.height >= projected_point.y()) { + std::memcpy( + &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step); + clusters_data_size.at(i) += point_step; } - if (debugger_) { - // add all points inside image to debug - if ( - projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height) { - debug_image_points.push_back(projected_point); - } + } + if (debugger_) { + // add all points inside image to debug + if ( + projected_point.x() > 0 && projected_point.x() < camera_info.width && + projected_point.y() > 0 && projected_point.y() < camera_info.height) { + debug_image_points.push_back(projected_point); } } } - { // refine and update output_fused_objects_ - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("updateOutputFusedObjects", *time_keeper_); - - updateOutputFusedObjects( - output_objs, clusters, clusters_data_size, input_pointcloud_msg, input_roi_msg.header, - tf_buffer_, min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, - output_fused_objects_); - } - + // refine and update output_fused_objects_ + updateOutputFusedObjects( + output_objs, clusters, clusters_data_size, input_pointcloud_msg, input_roi_msg.header, + tf_buffer_, min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); if (debugger_) { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); - debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); 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 d15852e7359c8..afeff213c0afe 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 @@ -138,46 +138,39 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( int y_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; - { // calculate camera projections - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = - std::make_unique("calculate camera projection", *time_keeper_); - - for (size_t global_offset = 0; global_offset < transformed_cloud.data.size(); - global_offset += point_step) { - float transformed_x = - *reinterpret_cast(&transformed_cloud.data[global_offset + x_offset]); - float transformed_y = - *reinterpret_cast(&transformed_cloud.data[global_offset + y_offset]); - float transformed_z = - *reinterpret_cast(&transformed_cloud.data[global_offset + z_offset]); - // skip filtering pointcloud behind the camera or too far from camera - if (transformed_z <= 0.0 || transformed_z > filter_distance_threshold_) { - continue; - } - - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - 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; - if (!is_inside_image) { - continue; - } - - // skip filtering pointcloud where semantic id out of the defined list - uint8_t semantic_id = mask.at( - static_cast(projected_point.y()), static_cast(projected_point.x())); - if ( - static_cast(semantic_id) >= filter_semantic_label_target_list_.size() || - !filter_semantic_label_target_list_.at(semantic_id).second) { - continue; - } - - filter_global_offset_set_.insert(global_offset); + for (size_t global_offset = 0; global_offset < transformed_cloud.data.size(); + global_offset += point_step) { + float transformed_x = + *reinterpret_cast(&transformed_cloud.data[global_offset + x_offset]); + float transformed_y = + *reinterpret_cast(&transformed_cloud.data[global_offset + y_offset]); + float transformed_z = + *reinterpret_cast(&transformed_cloud.data[global_offset + z_offset]); + // skip filtering pointcloud behind the camera or too far from camera + if (transformed_z <= 0.0 || transformed_z > filter_distance_threshold_) { + continue; + } + + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + 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; + if (!is_inside_image) { + continue; } + + // skip filtering pointcloud where semantic id out of the defined list + uint8_t semantic_id = mask.at( + static_cast(projected_point.y()), static_cast(projected_point.x())); + if ( + static_cast(semantic_id) >= filter_semantic_label_target_list_.size() || + !filter_semantic_label_target_list_.at(semantic_id).second) { + continue; + } + + filter_global_offset_set_.insert(global_offset); } }