diff --git a/perception/roi_cluster_fusion/CMakeLists.txt b/perception/roi_cluster_fusion/CMakeLists.txt new file mode 100644 index 0000000000000..122b7ee4208a0 --- /dev/null +++ b/perception/roi_cluster_fusion/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.5) +project(roi_cluster_fusion) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_lint_auto REQUIRED) +find_package(ament_cmake_auto REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_find_build_dependencies() + + +include_directories( + include + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(roi_cluster_fusion_nodelet SHARED src/roi_cluster_fusion_nodelet.cpp) +target_link_libraries(roi_cluster_fusion_nodelet + ${OpenCV_LIBRARIES} + ${EIGEN3_LIBRARIES} +) + +rclcpp_components_register_node(roi_cluster_fusion_nodelet + PLUGIN "roi_cluster_fusion::RoiClusterFusionNodelet" + EXECUTABLE roi_cluster_fusion_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + endif() diff --git a/perception/roi_cluster_fusion/README.md b/perception/roi_cluster_fusion/README.md new file mode 100644 index 0000000000000..26d8ed2190c41 --- /dev/null +++ b/perception/roi_cluster_fusion/README.md @@ -0,0 +1,90 @@ +# roi_cluster_fusion + +## Purpose + +roi_cluster_fusion is a package for filtering clusters that are less likely to be objects and overwriting labels of clusters with that of Region Of Interests (ROIs) by a 2D object detector. + +## Inner-workings / Algorithms + +The clusters are projected onto image planes, and then if the ROIs of clusters and ROIs by a detector are overlapped, the labels of clusters are overwritten with that of ROIs by detector. Intersection over Union (IoU) is used to determine if there are overlaps between them. + +![roi_cluster_fusion_image](./images/roi_cluster_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------- | ----------------------------------------------------------- | ---------------------------------------------------------------------------------- | +| `clusters` | `autoware_perception_msgs::msg::DetectedObjectsWithFeature` | clustered pointcloud | +| `input/camera_infoID` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes, `ID` is between 0 and 7 | +| `input/roisID` | `autoware_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image, `ID` is between 0 and 7 | +| `input/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | + +### Output + +| Name | Type | Description | +| -------------------- | ------------------------- | ------------------------------------------------- | +| `output/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| --------------------------- | ----- | ----------------------------------------------------------------------------- | +| `use_iou_x` | bool | calculate IoU only along x-axis | +| `use_iou_y` | bool | calculate IoU only along y-axis | +| `use_iou` | bool | calculate IoU both along x-axis and y-axis | +| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | +| `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/roi_cluster_fusion/images/roi_cluster_fusion.png b/perception/roi_cluster_fusion/images/roi_cluster_fusion.png new file mode 100644 index 0000000000000..9e98f8a682863 Binary files /dev/null and b/perception/roi_cluster_fusion/images/roi_cluster_fusion.png differ diff --git a/perception/roi_cluster_fusion/include/roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp b/perception/roi_cluster_fusion/include/roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp new file mode 100644 index 0000000000000..63860c2e1fd03 --- /dev/null +++ b/perception/roi_cluster_fusion/include/roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp @@ -0,0 +1,135 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROI_CLUSTER_FUSION__ROI_CLUSTER_FUSION_NODELET_HPP_ +#define ROI_CLUSTER_FUSION__ROI_CLUSTER_FUSION_NODELET_HPP_ + +#define EIGEN_MPL2_ONLY + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace roi_cluster_fusion +{ +class Debugger +{ +public: + explicit Debugger(rclcpp::Node * node, const int camera_num); + ~Debugger() = default; + rclcpp::Node * node_; + void showImage( + const int id, const rclcpp::Time & time, + const std::vector & image_rois, + const std::vector & pointcloud_rois, + const std::vector & points); + +private: + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, const int id); + std::shared_ptr image_transport_; + std::vector image_subs_; + std::vector image_pubs_; + std::vector> image_buffers_; +}; + +class RoiClusterFusionNodelet : public rclcpp::Node +{ +public: + explicit RoiClusterFusionNodelet(const rclcpp::NodeOptions & options); + +private: + void fusionCallback( + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_cluster_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg); + void cameraInfoCallback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const int id); + double calcIoU( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2); + double calcIoUX( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2); + double calcIoUY( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2); + + rclcpp::Publisher::SharedPtr + labeled_cluster_pub_; + std::vector::SharedPtr> v_camera_info_sub_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_ptr_; + message_filters::Subscriber + cluster_sub_; + std::vector>> + v_roi_sub_; + message_filters::PassThrough + passthrough_; + typedef message_filters::sync_policies::ApproximateTime< + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature, + autoware_perception_msgs::msg::DetectedObjectsWithFeature> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + std::shared_ptr sync_ptr_; + inline void dummyCallback( + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input) + { + auto dummy = input; + passthrough_.add(dummy); + } + // ROS Parameters + bool use_iou_x_; + bool use_iou_y_; + bool use_iou_; + bool use_cluster_semantic_type_; + double iou_threshold_; + int rois_number_; + std::map m_camera_info_; + std::shared_ptr debugger_; +}; + +} // namespace roi_cluster_fusion + +#endif // ROI_CLUSTER_FUSION__ROI_CLUSTER_FUSION_NODELET_HPP_ diff --git a/perception/roi_cluster_fusion/launch/roi_cluster_fusion.launch.xml b/perception/roi_cluster_fusion/launch/roi_cluster_fusion.launch.xml new file mode 100644 index 0000000000000..58e9951ce55ec --- /dev/null +++ b/perception/roi_cluster_fusion/launch/roi_cluster_fusion.launch.xml @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/roi_cluster_fusion/package.xml b/perception/roi_cluster_fusion/package.xml new file mode 100644 index 0000000000000..d339162a2fed8 --- /dev/null +++ b/perception/roi_cluster_fusion/package.xml @@ -0,0 +1,32 @@ + + + roi_cluster_fusion + 0.1.0 + The roi_cluster_fusion package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + autoware_perception_msgs + cv_bridge + image_transport + message_filters + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_ros + tf2_sensor_msgs + + + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp b/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp new file mode 100644 index 0000000000000..0a48fa83725d4 --- /dev/null +++ b/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp @@ -0,0 +1,511 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace roi_cluster_fusion +{ +Debugger::Debugger(rclcpp::Node * node, const int camera_num) : node_(node) +{ + image_buffers_.resize(camera_num); + for (int id = 0; id < camera_num; ++id) { + auto sub = image_transport::create_subscription( + node, "input/image_raw" + std::to_string(id), + boost::bind(&Debugger::imageCallback, this, _1, id), "raw"); + image_subs_.push_back(sub); + if (node->has_parameter("format")) { + node->undeclare_parameter("format"); + } + if (node->has_parameter("jpeg_quality")) { + node->undeclare_parameter("jpeg_quality"); + } + if (node->has_parameter("png_level")) { + node->undeclare_parameter("png_level"); + } + auto pub = image_transport::create_publisher(node, "output/image_raw" + std::to_string(id)); + image_pubs_.push_back(pub); + image_buffers_.at(id).set_capacity(5); + } +} + +void Debugger::imageCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, const int id) +{ + image_buffers_.at(id).push_front(input_image_msg); +} + +void Debugger::showImage( + const int id, const rclcpp::Time & time, + const std::vector & image_rois, + const std::vector & pointcloud_rois, + const std::vector & points) +{ + const boost::circular_buffer & image_buffer = + image_buffers_.at(id); + const image_transport::Publisher & image_pub = image_pubs_.at(id); + for (size_t i = 0; i < image_buffer.size(); ++i) { + if (image_buffer.at(i)->header.stamp == time) { + cv_bridge::CvImagePtr cv_ptr; + cv_ptr = cv_bridge::toCvCopy(image_buffer.at(i), image_buffer.at(i)->encoding); + for (const auto & point : points) { + cv::circle( + cv_ptr->image, cv::Point(static_cast(point.x()), static_cast(point.y())), 2, + cv::Scalar(255, 255, 255), 3, 4); + } + for (const auto & image_roi : image_rois) { + cv::line( + cv_ptr->image, cv::Point(image_roi.x_offset, image_roi.y_offset), + cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset), + cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + cv::line( + cv_ptr->image, cv::Point(image_roi.x_offset, image_roi.y_offset), + cv::Point(image_roi.x_offset, image_roi.y_offset + image_roi.height), + cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + cv::line( + cv_ptr->image, cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset), + cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset + image_roi.height), + cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + cv::line( + cv_ptr->image, cv::Point(image_roi.x_offset, image_roi.y_offset + image_roi.height), + cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset + image_roi.height), + cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + } + for (const auto & pointcloud_roi : pointcloud_rois) { + cv::line( + cv_ptr->image, cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset), + cv::Point(pointcloud_roi.x_offset + pointcloud_roi.width, pointcloud_roi.y_offset), + cv::Scalar(255, 0, 0), 1, cv::LINE_AA); + cv::line( + cv_ptr->image, cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset), + cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset + pointcloud_roi.height), + cv::Scalar(255, 0, 0), 1, cv::LINE_AA); + cv::line( + cv_ptr->image, + cv::Point(pointcloud_roi.x_offset + pointcloud_roi.width, pointcloud_roi.y_offset), + cv::Point( + pointcloud_roi.x_offset + pointcloud_roi.width, + pointcloud_roi.y_offset + pointcloud_roi.height), + cv::Scalar(255, 0, 0), 1, cv::LINE_AA); + cv::line( + cv_ptr->image, + cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset + pointcloud_roi.height), + cv::Point( + pointcloud_roi.x_offset + pointcloud_roi.width, + pointcloud_roi.y_offset + pointcloud_roi.height), + cv::Scalar(255, 0, 0), 1, cv::LINE_AA); + } + image_pub.publish(cv_ptr->toImageMsg()); + // cv::imshow("ROI" + std::to_string(id), cv_ptr->image); + // cv::waitKey(2); + + break; + } + } +} + +RoiClusterFusionNodelet::RoiClusterFusionNodelet(const rclcpp::NodeOptions & options) +: Node("roi_cluster_fusion_node", options), + tf_buffer_(this->get_clock()), + tf_listener_ptr_(tf_buffer_) +{ + use_iou_x_ = declare_parameter("use_iou_x", true); + use_iou_y_ = declare_parameter("use_iou_y", false); + use_iou_ = declare_parameter("use_iou", false); + use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); + iou_threshold_ = declare_parameter("iou_threshold", 0.1); + int rois_number = declare_parameter("rois_number", 1); + if (rois_number < 1) { + RCLCPP_WARN(this->get_logger(), "minimum roi_num is 1. current roi_num is %d", rois_number); + rois_number = 1; + } + if (8 < rois_number) { + RCLCPP_WARN(this->get_logger(), "maximum roi_num is 8. current roi_num is %d", rois_number); + rois_number = 8; + } + + cluster_sub_.subscribe(this, "clusters", rclcpp::QoS{1}.get_rmw_qos_profile()); + for (int id = 0; id < rois_number; ++id) { + std::function fcn = + std::bind(&RoiClusterFusionNodelet::cameraInfoCallback, this, std::placeholders::_1, id); + v_camera_info_sub_.push_back(this->create_subscription( + "input/camera_info" + std::to_string(id), rclcpp::QoS{1}.best_effort(), fcn)); + } + v_roi_sub_.resize(rois_number); + for (int id = 0; id < static_cast(v_roi_sub_.size()); ++id) { + v_roi_sub_.at(id) = std::make_shared< + message_filters::Subscriber>( + this, "input/rois" + std::to_string(id), rclcpp::QoS{1}.get_rmw_qos_profile()); + } + // add dummy callback to enable passthrough filter + v_roi_sub_.at(0)->registerCallback(bind(&RoiClusterFusionNodelet::dummyCallback, this, _1)); + switch (rois_number) { + case 1: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), passthrough_, passthrough_, passthrough_, + passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 2: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), passthrough_, + passthrough_, passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 3: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), + passthrough_, passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 4: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), + *v_roi_sub_.at(3), passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 5: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), + *v_roi_sub_.at(3), *v_roi_sub_.at(4), passthrough_, passthrough_, passthrough_); + break; + case 6: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), + *v_roi_sub_.at(3), *v_roi_sub_.at(4), *v_roi_sub_.at(5), passthrough_, passthrough_); + break; + case 7: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), + *v_roi_sub_.at(3), *v_roi_sub_.at(4), *v_roi_sub_.at(5), *v_roi_sub_.at(6), passthrough_); + break; + case 8: + sync_ptr_ = std::make_shared( + SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), + *v_roi_sub_.at(3), *v_roi_sub_.at(4), *v_roi_sub_.at(5), *v_roi_sub_.at(6), + *v_roi_sub_.at(7)); + break; + default: + return; + } + // sync_ptr_->registerCallback( + // boost::bind(&RoiClusterFusionNodelet::fusionCallback, this, _1, _2, _3, _4, _5, _6, _7, + // _8, _9)); + sync_ptr_->registerCallback(std::bind( + &RoiClusterFusionNodelet::fusionCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, + std::placeholders::_7, std::placeholders::_8, std::placeholders::_9)); + labeled_cluster_pub_ = + this->create_publisher( + "output/labeled_clusters", rclcpp::QoS{1}); + + const bool debug_mode = declare_parameter("debug_mode", false); + if (debug_mode) { + debugger_ = std::make_shared(this, rois_number); + } +} + +void RoiClusterFusionNodelet::cameraInfoCallback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const int id) +{ + m_camera_info_[id] = *input_camera_info_msg; +} + +void RoiClusterFusionNodelet::fusionCallback( + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_cluster_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg) +{ + // Guard + if (labeled_cluster_pub_->get_subscription_count() < 1) { + return; + } + + // build output msg + autoware_perception_msgs::msg::DetectedObjectsWithFeature output_msg; + output_msg = *input_cluster_msg; + + // reset cluster semantic type + if (!use_cluster_semantic_type_) { + for (auto & feature_object : output_msg.feature_objects) { + feature_object.object.classification.front().label = + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + feature_object.object.existence_probability = 0.0; + } + } + + // check camera info + for (int id = 0; id < static_cast(v_roi_sub_.size()); ++id) { + // debug variable + std::vector debug_image_rois; + std::vector debug_pointcloud_rois; + std::vector debug_image_points; + // cannot find camera info + if (m_camera_info_.find(id) == m_camera_info_.end()) { + RCLCPP_WARN(this->get_logger(), "no camera info. id is %d", id); + continue; + } + + // projection matrix + Eigen::Matrix4d projection; + projection << m_camera_info_.at(id).p.at(0), m_camera_info_.at(id).p.at(1), + m_camera_info_.at(id).p.at(2), m_camera_info_.at(id).p.at(3), m_camera_info_.at(id).p.at(4), + m_camera_info_.at(id).p.at(5), m_camera_info_.at(id).p.at(6), m_camera_info_.at(id).p.at(7), + m_camera_info_.at(id).p.at(8), m_camera_info_.at(id).p.at(9), m_camera_info_.at(id).p.at(10), + m_camera_info_.at(id).p.at(11); + + // get transform from cluster frame id to camera optical frame id + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_.lookupTransform( + /*target*/ m_camera_info_.at(id).header.frame_id, + /*src*/ input_cluster_msg->header.frame_id, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + return; + } + + // build cluster roi + std::map m_cluster_roi; + for (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); + + // for reduce calc cost + // Eigen::Vector3d centroid_point; + // centroid_point << 0, 0, 0; + // 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) + // { + // centroid_point += Eigen::Vector3d(*iter_x, *iter_y, *iter_z); + // } + // centroid_point *= 1.0 / static_cast( + // input_cluster_msg->feature_objects.at(i).feature.cluster.data.size()); + // if (centroid_point.z() <= 0.0) + // continue; + + std::vector projected_points; + projected_points.reserve(transformed_cluster.data.size()); + int min_x(m_camera_info_.at(id).width), min_y(m_camera_info_.at(id).height), max_x(0), + max_y(0); + 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::Vector4d projected_point = + projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + if ( + 0 <= static_cast(normalized_projected_point.x()) && + static_cast(normalized_projected_point.x()) <= + static_cast(m_camera_info_.at(id).width) - 1 && + 0 <= static_cast(normalized_projected_point.y()) && + static_cast(normalized_projected_point.y()) <= + static_cast(m_camera_info_.at(id).height) - 1) { + min_x = std::min(static_cast(normalized_projected_point.x()), min_x); + min_y = std::min(static_cast(normalized_projected_point.y()), min_y); + max_x = std::max(static_cast(normalized_projected_point.x()), max_x); + max_y = std::max(static_cast(normalized_projected_point.y()), max_y); + projected_points.push_back(normalized_projected_point); + debug_image_points.push_back(normalized_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)); + debug_pointcloud_rois.push_back(roi); + } + + // calc iou + autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg; + if (id == 0) { + input_roi_msg = input_roi0_msg; + } else if (id == 1) { + input_roi_msg = input_roi1_msg; + } else if (id == 2) { + input_roi_msg = input_roi2_msg; + } else if (id == 3) { + input_roi_msg = input_roi3_msg; + } else if (id == 4) { + input_roi_msg = input_roi4_msg; + } else if (id == 5) { + input_roi_msg = input_roi5_msg; + } else if (id == 6) { + input_roi_msg = input_roi6_msg; + } else if (id == 7) { + input_roi_msg = input_roi7_msg; + } + for (size_t i = 0; i < input_roi_msg->feature_objects.size(); ++i) { + int index = 0; + double max_iou = 0.0; + for (auto m_cluster_roi_itr = m_cluster_roi.begin(); m_cluster_roi_itr != m_cluster_roi.end(); + ++m_cluster_roi_itr) { + double iou(0.0), iou_x(0.0), iou_y(0.0); + if (use_iou_) { + iou = + calcIoU(m_cluster_roi_itr->second, input_roi_msg->feature_objects.at(i).feature.roi); + } + if (use_iou_x_) { + iou_x = + calcIoUX(m_cluster_roi_itr->second, input_roi_msg->feature_objects.at(i).feature.roi); + } + if (use_iou_y_) { + iou_y = + calcIoUY(m_cluster_roi_itr->second, input_roi_msg->feature_objects.at(i).feature.roi); + } + if (max_iou < iou + iou_x + iou_y) { + index = m_cluster_roi_itr->first; + max_iou = iou + iou_x + iou_y; + } + } + if ( + iou_threshold_ < max_iou && + output_msg.feature_objects.at(index).object.existence_probability <= + input_roi_msg->feature_objects.at(i).object.existence_probability && + input_roi_msg->feature_objects.at(i).object.classification.front().label != + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { + output_msg.feature_objects.at(index).object.classification = + input_roi_msg->feature_objects.at(i).object.classification; + } + debug_image_rois.push_back(input_roi_msg->feature_objects.at(i).feature.roi); + } + if (debugger_) { + debugger_->showImage( + id, input_roi_msg->header.stamp, debug_image_rois, debug_pointcloud_rois, + debug_image_points); + } + } + // publish output msg + labeled_cluster_pub_->publish(output_msg); +} + +double RoiClusterFusionNodelet::calcIoU( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2) +{ + double s_1, s_2; + s_1 = static_cast(roi_1.width * static_cast(roi_1.height)); + s_2 = static_cast(roi_2.width * static_cast(roi_2.height)); + + double overlap_s; + double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; + overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; + overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; + overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width + ? roi_1.x_offset + roi_1.width + : roi_2.x_offset + roi_2.width; + overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height + ? roi_1.y_offset + roi_1.height + : roi_2.y_offset + roi_2.height; + overlap_s = (overlap_max_x - overlap_min_x) * (overlap_max_y - overlap_min_y); + if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { + return 0.0; + } + return overlap_s / (s_1 + s_2 - overlap_s); +} +double RoiClusterFusionNodelet::calcIoUX( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2) +{ + double s_1, s_2; + s_1 = static_cast(roi_1.width); + s_2 = static_cast(roi_2.width); + double overlap_s; + double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; + overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; + overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; + overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width + ? roi_1.x_offset + roi_1.width + : roi_2.x_offset + roi_2.width; + overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height + ? roi_1.y_offset + roi_1.height + : roi_2.y_offset + roi_2.height; + overlap_s = (overlap_max_x - overlap_min_x); + if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { + return 0.0; + } + return overlap_s / (s_1 + s_2 - overlap_s); +} +double RoiClusterFusionNodelet::calcIoUY( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2) +{ + double s_1, s_2; + s_1 = static_cast(roi_1.height); + s_2 = static_cast(roi_2.height); + double overlap_s; + double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; + overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; + overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; + overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width + ? roi_1.x_offset + roi_1.width + : roi_2.x_offset + roi_2.width; + overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height + ? roi_1.y_offset + roi_1.height + : roi_2.y_offset + roi_2.height; + overlap_s = (overlap_max_y - overlap_min_y); + if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { + return 0.0; + } + return overlap_s / (s_1 + s_2 - overlap_s); +} +} // namespace roi_cluster_fusion + +RCLCPP_COMPONENTS_REGISTER_NODE(roi_cluster_fusion::RoiClusterFusionNodelet)