diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 4446c87427e88..31892853a855f 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -15,15 +15,22 @@ #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#include + #include +#include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC +#include #include #else +#include + #include #endif @@ -45,6 +52,23 @@ namespace detail return boost::none; } } + +[[maybe_unused]] inline boost::optional getTransformMatrix( + const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header, + const tf2_ros::Buffer & tf_buffer) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp, + rclcpp::Duration::from_seconds(1.0)); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + return mat; + } catch (tf2::TransformException & e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what()); + return boost::none; + } +} } // namespace detail namespace object_recognition_utils @@ -79,6 +103,46 @@ bool transformObjects( } return true; } +template +bool transformObjectsWithFeature( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer); + if (!tf_matrix) { + RCLCPP_WARN( + rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix"); + return false; + } + for (auto & feature_object : output_msg.feature_objects) { + // transform object + tf2::fromMsg( + feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose); + + // transform cluster + sensor_msgs::msg::PointCloud2 transformed_cluster; + pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster); + transformed_cluster.header.frame_id = target_frame_id; + feature_object.feature.cluster = transformed_cluster; + } + output_msg.header.frame_id = target_frame_id; + return true; + } + return true; +} } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 95925e846a55c..2f2472515ebad 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -17,7 +17,13 @@ geometry_msgs interpolation libboost-dev + pcl_conversions + pcl_ros rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen tier4_autoware_utils ament_cmake_ros diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index c74c0fdcc63b7..7f2fc45396f7f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -11,13 +11,12 @@ - - + @@ -59,15 +58,16 @@ --> - + - + + @@ -75,13 +75,54 @@ - - + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -96,6 +137,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 99f09cfe6abbd..2bd007b658978 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -11,8 +11,6 @@ - - @@ -78,15 +76,16 @@ - + - + + @@ -94,13 +93,12 @@ - - + + - - - - + + + @@ -143,12 +141,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ca5d1d0f7e8bb..ab55047132482 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -5,7 +5,6 @@ - @@ -65,6 +64,8 @@ + + @@ -94,6 +95,8 @@ + + @@ -107,6 +110,7 @@ + @@ -119,6 +123,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 5b6134dc45584..ccc236d9cdc94 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -4,8 +4,6 @@ - - @@ -19,29 +17,27 @@ - + - + + - - - + - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index a47e6a86f1c7b..4756fc9338e46 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -3,7 +3,6 @@ - @@ -19,6 +18,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index a5d8d2113d2db..93d395ca3e466 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -35,7 +35,6 @@ def __init__(self, context): ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] @@ -46,47 +45,40 @@ def __init__(self, context): ] self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] + self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): - if self.use_down_sample_filter: - return self.create_down_sample_pipeline() + if self.use_pointcloud_map == "true": + return self.create_compare_map_pipeline() else: - return self.create_normal_pipeline() + return self.create_no_compare_map_pipeline() - def create_normal_pipeline(self): + def create_no_compare_map_pipeline(self): components = [] components.append( ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), - ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, - "input_frame": "map", + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, } ], extra_arguments=[ - {"use_intra_process_comms": False}, + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], - ) + ), ) return components - def create_down_sample_pipeline(self): + def create_compare_map_pipeline(self): components = [] down_sample_topic = ( "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" @@ -94,7 +86,7 @@ def create_down_sample_pipeline(self): components.append( ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index c41177e51116b..a30b87a1068b5 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,10 +1,8 @@ - - @@ -59,8 +57,9 @@ - + + - - - + + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index c1723c1fa07e8..336588891d9b2 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt new file mode 100644 index 0000000000000..49506f4b439fb --- /dev/null +++ b/perception/cluster_merger/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(cluster_merger) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(cluster_merger_node_component SHARED + src/cluster_merger/node.cpp +) + +rclcpp_components_register_node(cluster_merger_node_component + PLUGIN "cluster_merger::ClusterMergerNode" + EXECUTABLE cluster_merger_node) + + +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/cluster_merger/README.md b/perception/cluster_merger/README.md new file mode 100644 index 0000000000000..b46f7401b70ec --- /dev/null +++ b/perception/cluster_merger/README.md @@ -0,0 +1,72 @@ +# cluster merger + +## Purpose + +cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. + +## Inner-working / Algorithms + +The clusters of merged topics are simply concatenated from clusters of input topics. + +## Input / Output + +### Input + +| Name | Type | Description | +| ---------------- | -------------------------------------------------------- | ------------------- | +| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | +| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | + +### Output + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | --------------- | +| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters | + +## Parameters + +| Name | Type | Description | Default value | +| :---------------- | :----- | :----------------------------------- | :------------ | +| `output_frame_id` | string | The header frame_id of output topic. | base_link | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp new file mode 100644 index 0000000000000..8da5999f00384 --- /dev/null +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -0,0 +1,73 @@ +// Copyright 2023 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 CLUSTER_MERGER__NODE_HPP_ +#define CLUSTER_MERGER__NODE_HPP_ + +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace cluster_merger +{ +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +class ClusterMergerNode : public rclcpp::Node +{ +public: + explicit ClusterMergerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr sub_objects_{}; + message_filters::Subscriber objects0_sub_; + message_filters::Subscriber objects1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, DetectedObjectsWithFeature> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + + std::string output_frame_id_; + + std::vector::SharedPtr> sub_objects_array{}; + std::shared_ptr transform_listener_; + + void objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg); + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_; +}; + +} // namespace cluster_merger + +#endif // CLUSTER_MERGER__NODE_HPP_ diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml new file mode 100644 index 0000000000000..1bbd0ebd91e12 --- /dev/null +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml new file mode 100644 index 0000000000000..14826ad07e098 --- /dev/null +++ b/perception/cluster_merger/package.xml @@ -0,0 +1,28 @@ + + + + cluster_merger + 0.1.0 + The ROS 2 cluster merger package + Yukihiro Saito + Shunsuke Miura + autoware + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + message_filters + object_recognition_utils + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/cluster_merger/src/cluster_merger/node.cpp b/perception/cluster_merger/src/cluster_merger/node.cpp new file mode 100644 index 0000000000000..48bf953027510 --- /dev/null +++ b/perception/cluster_merger/src/cluster_merger/node.cpp @@ -0,0 +1,73 @@ +// Copyright 2023 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 "cluster_merger/node.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include +#include +#include +namespace cluster_merger +{ + +ClusterMergerNode::ClusterMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("cluster_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + objects0_sub_(this, "input/cluster0", rclcpp::QoS{1}.get_rmw_qos_profile()), + objects1_sub_(this, "input/cluster1", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) +{ + output_frame_id_ = declare_parameter("output_frame_id"); + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&ClusterMergerNode::objectsCallback, this, _1, _2)); + + // Publisher + pub_objects_ = create_publisher("~/output/clusters", rclcpp::QoS{1}); +} + +void ClusterMergerNode::objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg) +{ + if (pub_objects_->get_subscription_count() < 1) { + return; + } + // TODO(badai-nguyen): transform input topics to desired frame before concatenating + /* transform to the same with cluster0 frame id*/ + DetectedObjectsWithFeature transformed_objects0; + DetectedObjectsWithFeature transformed_objects1; + if ( + !object_recognition_utils::transformObjectsWithFeature( + *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || + !object_recognition_utils::transformObjectsWithFeature( + *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { + return; + } + + DetectedObjectsWithFeature output_objects; + output_objects.header = input_objects0_msg->header; + // add check frame id and transform if they are different + output_objects.feature_objects = transformed_objects0.feature_objects; + output_objects.feature_objects.insert( + output_objects.feature_objects.end(), transformed_objects1.feature_objects.begin(), + transformed_objects1.feature_objects.end()); + pub_objects_->publish(output_objects); +} +} // namespace cluster_merger + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(cluster_merger::ClusterMergerNode) diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml deleted file mode 100644 index 3dd303464a2c1..0000000000000 --- a/perception/euclidean_cluster/config/compare_map.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - - # distance threshold for compare compare - distance_threshold: 0.5 - - # publish voxelized map pointcloud for debug - publish_debug_pcd: False - - # use dynamic map loading - use_dynamic_map_loading: True - - # time interval to check dynamic map loading - timer_interval_ms: 100 - - # distance threshold for dynamic map update - map_update_distance_threshold: 10.0 - - # radius map for dynamic map loading - map_loader_radius: 150.0 diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml deleted file mode 100644 index 1962fba1f332a..0000000000000 --- a/perception/euclidean_cluster/config/outlier.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.3 - voxel_size_y: 0.3 - voxel_size_z: 100.0 - voxel_points_threshold: 3 diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml deleted file mode 100644 index 3ff32bfbb7146..0000000000000 --- a/perception/euclidean_cluster/config/voxel_grid.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.15 - voxel_size_y: 0.15 - voxel_size_z: 0.15 diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 2f3de2b789eb3..26b027f0077aa 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -7,9 +7,11 @@ max_cluster_size: 3000 use_height: false input_frame: "base_link" - max_x: 70.0 - min_x: -70.0 - max_y: 70.0 - min_y: -70.0 - max_z: 4.5 - min_z: -4.5 + + # low height crop box filter param + max_x: 200.0 + min_x: -200.0 + max_y: 200.0 + min_y: -200.0 + max_z: 2.0 + min_z: -10.0 diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index db86bbf80d250..66c25396a656e 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -35,61 +35,35 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "voxel_grid_filtered/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), - ("map", LaunchConfiguration("input_map")), - ("output", "compare_map_filtered/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[ - { - "distance_threshold": 0.5, - "timer_interval_ms": 100, - "use_dynamic_map_loading": True, - "downsize_ratio_z_axis": 0.5, - "map_update_distance_threshold": 10.0, - "map_loader_radius": 150.0, - "publish_debug_pcd": True, - "input_frame": "map", - } + ("output", "low_height/pointcloud"), ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_euclidean_cluster_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "compare_map_filtered/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], ) - disuse_map_euclidean_cluster_component = ComposableNode( + disuse_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), + ("input", LaunchConfiguration("input_pointcloud")), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], @@ -100,26 +74,26 @@ def load_composable_node_param(param_path): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[voxel_grid_filter_component], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_euclidean_cluster_component], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) - return [container, use_map_loader, disuse_map_loader] + return [container, use_low_height_pointcloud_loader, disuse_low_height_pointcloud_loader] def generate_launch_description(): @@ -131,14 +105,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index 7159fb4c42793..fd1ea82befae0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -3,16 +3,14 @@ - - + - - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 82ce5605b67cd..00bcd4422bd3c 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,10 +16,8 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution -from launch.substitutions import AnonName +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -36,159 +34,31 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - namespace=ns, - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("map", LaunchConfiguration("input_map")), - ("output", "map_filter/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[load_composable_node_param("compare_map_param_path")], - ) - - # separate range of pointcloud when map_filter used - use_map_short_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", - remappings=[ - ("input", "map_filter/pointcloud"), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - use_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # disuse_map_voxel_grid_filter - disuse_map_short_range_crop_box_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - disuse_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[ - ("input", "short_range/pointcloud"), - ("output", "downsampled/short_range/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set outlier filter as a component - outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", - name="outlier_filter", - remappings=[ - ("input", "downsampled/short_range/pointcloud"), - ("output", "outlier_filter/pointcloud"), - ], - parameters=[load_composable_node_param("outlier_param_path")], - ) - - # concat with-outlier pointcloud and without-outlier pcl - downsample_concat_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concat_downsampled_pcl", - remappings=[("output", "downsampled/concatenated/pointcloud")], - parameters=[ - { - "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], - "output_frame": "base_link", - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", "downsampled/concatenated/pointcloud"), - ("output", LaunchConfiguration("output_clusters")), + ("output", "low_height/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_disuse_downsample_euclidean_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "map_filter/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - disuse_map_disuse_downsample_euclidean_component = ComposableNode( + + disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -209,75 +79,24 @@ def load_composable_node_param(param_path): output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_short_range_crop_box_filter_component, - use_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_short_range_crop_box_filter_component, - disuse_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, - ], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), - ) - - use_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - compare_map_filter_component, - use_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - (LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - - disuse_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ container, - use_map_use_downsample_loader, - disuse_map_use_downsample_loader, - use_map_disuse_downsample_loader, - disuse_map_disuse_downsample_loader, + use_low_height_pointcloud_loader, + disuse_low_height_pointcloud_loader, ] @@ -290,29 +109,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg("use_downsample_pointcloud", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), - add_launch_arg( - "outlier_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/outlier.param.yaml", - ], - ), - add_launch_arg( - "compare_map_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/compare_map.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 2833fed81c28e..b6a426dabfd12 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -3,22 +3,14 @@ - - - - - + - - - - - + diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 906ad71d21732..ce7c3b5ea12a9 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/roi_pointcloud_fusion/node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -39,6 +40,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" + EXECUTABLE roi_pointcloud_fusion_node +) + set(CUDA_VERBOSE OFF) # set flags for CUDA availability diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index 1bbee35ab44f8..207989d8a6f25 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -58,3 +58,4 @@ The rclcpp::TimerBase timer could not break a for loop, therefore even if time i | roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | | roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | | pointpainting_fusion | Paint the point cloud with the ROIs from a 2D object detector and feed to a 3D object detector. | [link](./docs/pointpainting-fusion.md) | +| roi_pointcloud_fusion | Matching pointcloud with ROIs from a 2D object detector to detect unknown-labeled objects | [link](./docs/roi-pointcloud-fusion.md) | diff --git a/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png new file mode 100644 index 0000000000000..c529de7c728fb Binary files /dev/null and b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png differ diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md new file mode 100644 index 0000000000000..5d0b241a578d6 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md @@ -0,0 +1,93 @@ +# roi_pointcloud_fusion + +## Purpose + +The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of Interests (ROIs) detected by a 2D object detector, specific for unknown labeled ROI. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto image planes and extracted as cluster if they are inside the unknown labeled ROIs. +- Since the cluster might contain unrelated points from background, then a refinement step is added to filter the background pointcloud by distance to camera. + +![roi_pointcloud_fusion_image](./images/roi_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| ----------------- | -------------------------------------------------------- | -------------------------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output pointcloud as default of interface | +| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | +| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `cluster_2d_tolerance` | double | cluster tolerance measured in radial direction | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + +- Currently, the refinement is only based on distance to camera, the roi based clustering is supposed to work well with small object ROIs. Others criteria for refinement might needed in the future. + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 016633fa4ef92..5174aebe069a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include #include #include @@ -49,6 +52,8 @@ using autoware_auto_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::ObjectClassification; template class FusionNode : public rclcpp::Node diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..2b4eb822a9edb --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 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 IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +namespace image_projection_based_fusion +{ +class RoiPointCloudFusionNode +: public FusionNode +{ +private: + int min_cluster_size_{1}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; + + rclcpp::Publisher::SharedPtr pub_objects_ptr_; + std::vector output_fused_objects_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; + + /* data */ +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + bool out_of_scope(const DetectedObjectWithFeature & obj); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 541cf997412c2..d7fd3c3882046 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -32,18 +32,54 @@ #include #endif +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/shape.hpp" +#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" + +#include +#include +#include + #include #include +#include namespace image_projection_based_fusion { +using PointCloud = pcl::PointCloud; std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj); +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center); + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects); + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); + } // namespace image_projection_based_fusion #endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..181f4ccb88320 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 32d7a5633b811..5ff99af2ebb21 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_point_types cv_bridge + euclidean_cluster image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 1bc351b08c01b..a540688f7e751 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -422,4 +422,5 @@ void FusionNode::publish(const Msg & output_msg) template class FusionNode; template class FusionNode; template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9180b18adeed8..978d026a3c859 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -171,12 +171,16 @@ void RoiClusterFusionNode::fuseOnSingleImage( for (const auto & feature_obj : input_roi_msg.feature_objects) { int index = 0; double max_iou = 0.0; + 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), iou_x(0.0), iou_y(0.0); if (use_iou_) { iou = calcIoU(cluster_map.second, feature_obj.feature.roi); } - if (use_iou_x_) { + // use for unknown roi to improve small objects like traffic cone detect + // TODO(badai-nguyen): add option to disable roi_cluster mode + if (use_iou_x_ || !is_roi_label_known) { iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); } if (use_iou_y_) { @@ -192,8 +196,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_label_known = feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; bool is_roi_existence_prob_higher = output_cluster_msg.feature_objects.at(index).object.existence_probability <= feature_obj.object.existence_probability; diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..233e568ebee0b --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -0,0 +1,165 @@ +// Copyright 2023 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 "image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +#include "euclidean_cluster/utils.hpp" + +namespace image_projection_based_fusion +{ +RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_pointcloud_fusion", options) +{ + fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); + min_cluster_size_ = declare_parameter("min_cluster_size"); + cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); + pub_objects_ptr_ = + this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + return; +} + +void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + + pub_objects_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + DetectedObjectsWithFeature output_msg; + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + if (objects_sub_count > 0) { + pub_objects_ptr_->publish(output_msg); + } + output_fused_objects_.clear(); + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + sensor_msgs::msg::PointCloud2 debug_cluster_msg; + euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } +} +void RoiPointCloudFusionNode::fuseOnSingleImage( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + __attribute__((unused)) const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + std::vector output_objs; + // select ROIs for fusion + for (const auto & feature_obj : input_roi_msg.feature_objects) { + if (fuse_unknown_only_) { + bool is_roi_label_unknown = feature_obj.object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (is_roi_label_unknown) { + output_objs.push_back(feature_obj); + } + } else { + // TODO(badai-nguyen): selected class from a list + output_objs.push_back(feature_obj); + } + } + if (output_objs.empty()) { + return; + } + + // transform pointcloud to camera optical frame id + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + geometry_msgs::msg::TransformStamped transform_stamped; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_roi_msg.header.frame_id, input_pointcloud_msg.header.frame_id, + input_roi_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + sensor_msgs::msg::PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + std::vector clusters; + clusters.resize(output_objs.size()); + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_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()); + + 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 ( + check_roi.x_offset <= normalized_projected_point.x() && + check_roi.y_offset <= normalized_projected_point.y() && + check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && + check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + cluster.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + } + + // refine and update output_fused_objects_ + updateOutputFusedObjects( + output_objs, clusters, input_pointcloud_msg.header, input_roi_msg.header, tf_buffer_, + min_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); +} + +bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const DetectedObjectWithFeature & obj) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiPointCloudFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 670c5596001fb..76cd1e3c61e82 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" - namespace image_projection_based_fusion { @@ -39,4 +38,197 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj) +{ + PointCloud2 ros_cluster; + pcl::toROSMsg(cluster, ros_cluster); + ros_cluster.header = header; + feature_obj.feature.cluster = ros_cluster; + feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); + feature_obj.object.existence_probability = 1.0f; +} + +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center) +{ + // sort point by distance to camera origin + + auto func = [center](const pcl::PointXYZ & p1, const pcl::PointXYZ & p2) { + return tier4_autoware_utils::calcDistance2d(center, p1) < + tier4_autoware_utils::calcDistance2d(center, p2); + }; + std::sort(cluster.begin(), cluster.end(), func); + PointCloud out_cluster; + for (auto & point : cluster) { + if (out_cluster.empty()) { + out_cluster.push_back(point); + continue; + } + if (tier4_autoware_utils::calcDistance2d(out_cluster.back(), point) < cluster_2d_tolerance) { + out_cluster.push_back(point); + continue; + } + if (out_cluster.size() >= static_cast(min_cluster_size)) { + return out_cluster; + } + out_cluster.clear(); + out_cluster.push_back(point); + } + return out_cluster; +} + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects) +{ + if (output_objs.size() != clusters.size()) { + return; + } + Eigen::Vector3d orig_camera_frame, orig_point_frame; + Eigen::Affine3d camera2lidar_affine; + orig_camera_frame << 0.0, 0.0, 0.0; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer, in_cloud_header.frame_id, in_roi_header.frame_id, in_roi_header.stamp); + if (!transform_stamped_optional) { + return; + } + camera2lidar_affine = transformToEigen(transform_stamped_optional.value().transform); + } + orig_point_frame = camera2lidar_affine * orig_camera_frame; + pcl::PointXYZ camera_orig_point_frame = + pcl::PointXYZ(orig_point_frame.x(), orig_point_frame.y(), orig_point_frame.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + PointCloud cluster = clusters.at(i); + auto & feature_obj = output_objs.at(i); + if (cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest + // to output refine cluster and centroid + auto refine_cluster = + closest_cluster(cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame); + if (refine_cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + refine_cluster.width = refine_cluster.points.size(); + refine_cluster.height = 1; + refine_cluster.is_dense = false; + // convert cluster to object + convertCluster2FeatureObject(in_cloud_header, refine_cluster, feature_obj); + output_fused_objects.push_back(feature_obj); + } +} + +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) +{ + if (cluster.empty()) { + return; + } + pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + float max_z = -1e6; + float min_z = 1e6; + for (const auto & point : cluster) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; + max_z = max_z < point.z ? point.z : max_z; + min_z = min_z > point.z ? point.z : min_z; + } + centroid.x = centroid.x / static_cast(cluster.size()); + centroid.y = centroid.y / static_cast(cluster.size()); + centroid.z = centroid.z / static_cast(cluster.size()); + + std::vector cluster2d; + std::vector cluster2d_convex; + + for (size_t i = 0; i < cluster.size(); ++i) { + cluster2d.push_back( + cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); + } + cv::convexHull(cluster2d, cluster2d_convex); + if (cluster2d_convex.empty()) { + return; + } + pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; + polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; + } + polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); + polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); + + autoware_auto_perception_msgs::msg::Shape shape; + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + geometry_msgs::msg::Point32 point; + point.x = cluster2d_convex.at(i).x / 1000.0; + point.y = cluster2d_convex.at(i).y / 1000.0; + point.z = 0.0; + shape.footprint.points.push_back(point); + } + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + constexpr float eps = 0.01; + shape.dimensions.x = 0; + shape.dimensions.y = 0; + shape.dimensions.z = std::max((max_z - min_z), eps); + feature_obj.object.shape = shape; + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = + centroid.x + polygon_centroid.x; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = + centroid.y + polygon_centroid.y; + feature_obj.object.kinematics.pose_with_covariance.pose.position.z = + min_z + shape.dimensions.z * 0.5; + feature_obj.object.existence_probability = 1.0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; +} + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0.0f; + centroid.y = 0.0f; + centroid.z = 0.0f; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + const size_t size = pointcloud.width * pointcloud.height; + centroid.x = centroid.x / static_cast(size); + centroid.y = centroid.y / static_cast(size); + centroid.z = centroid.z / static_cast(size); + return centroid; +} + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) +{ + pcl::PointXYZ closest_point; + double min_dist = 1e6; + pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); + for (std::size_t i = 0; i < cluster.points.size(); ++i) { + pcl::PointXYZ point = cluster.points.at(i); + double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + if (min_dist > dist_closest_point) { + min_dist = dist_closest_point; + closest_point = pcl::PointXYZ(point.x, point.y, point.z); + } + } + return closest_point; +} + } // namespace image_projection_based_fusion