Skip to content

Commit

Permalink
Merge pull request #849 from tier4/feat/rearrange-clustering-v0.11.0
Browse files Browse the repository at this point in the history 
refactor(perception): rearrange clustering pipeline v0.11.0
  • Loading branch information
@badai-nguyen
badai-nguyen authored Sep 22, 2023
2 parents 4f35db8 + 8b60391 commit c3ed832
Show file tree
Hide file tree
Showing 37 changed files with 1,180 additions and 394 deletions.
64 changes: 64 additions & 0 deletions common/object_recognition_utils/include/object_recognition_utils/transform.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,22 @@
#ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
#define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_

#include <pcl_ros/transforms.hpp>

#include <geometry_msgs/msg/transform.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/point_field.hpp>

#include <boost/optional.hpp>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#ifdef ROS_DISTRO_GALACTIC
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
#include <tf2_eigen/tf2_eigen.hpp>

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

Expand All @@ -45,6 +52,23 @@ namespace detail
return boost::none;
}
}

[[maybe_unused]] inline boost::optional<Eigen::Matrix4f> 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<float>();
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
Expand Down Expand Up @@ -79,6 +103,46 @@ bool transformObjects(
}
return true;
}
template <class T>
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_
6 changes: 6 additions & 0 deletions common/object_recognition_utils/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,13 @@
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
<depend>libboost-dev</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_eigen</depend>
<depend>tier4_autoware_utils</depend>

<test_depend>ament_cmake_ros</test_depend>
Expand Down
63 changes: 53 additions & 10 deletions ...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,12 @@
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
<arg name="score_threshold" default="0.35"/>

<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
<arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
<arg name="remove_unknown" default="true"/>
<arg name="trust_distance" default="30.0"/>
<arg name="use_roi_based_cluster" default="false"/>

<!-- Camera param -->
<arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
Expand Down Expand Up @@ -59,29 +58,71 @@
<arg name="image_number" value="$(var image_number)"/>
</include> -->

<!-- Pointcloud map filter -->
<!-- Pointcloud filter -->
<group>
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
<arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
<arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
</include>
</group>

<!-- Clustering -->
<group>
<push-ros-namespace namespace="clustering"/>
<group>
<let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
<let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
<let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
<let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
<include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
<arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
<arg name="output_clusters" value="clusters"/>
<arg name="use_pointcloud_map" value="false"/>
<arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
<arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
<arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

<!-- roi based clustering -->
<group>
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/camera_info0" value="$(var camera_info0)"/>
<arg name="input/rois0" value="$(var detection_rois0)"/>
<arg name="input/camera_info1" value="$(var camera_info1)"/>
<arg name="input/rois1" value="$(var detection_rois1)"/>
<arg name="input/camera_info2" value="$(var camera_info2)"/>
<arg name="input/rois2" value="$(var detection_rois2)"/>
<arg name="input/camera_info3" value="$(var camera_info3)"/>
<arg name="input/rois3" value="$(var detection_rois3)"/>
<arg name="input/camera_info4" value="$(var camera_info4)"/>
<arg name="input/rois4" value="$(var detection_rois4)"/>
<arg name="input/camera_info5" value="$(var camera_info5)"/>
<arg name="input/rois5" value="$(var detection_rois5)"/>
<arg name="input/camera_info6" value="$(var camera_info6)"/>
<arg name="input/rois6" value="$(var detection_rois6)"/>
<arg name="input/camera_info7" value="$(var camera_info7)"/>
<arg name="input/rois7" value="$(var detection_rois7)"/>
<arg name="input/rois_number" value="$(var image_number)"/>
<arg name="input/image0" value="$(var image_raw0)"/>
<arg name="input/image1" value="$(var image_raw1)"/>
<arg name="input/image2" value="$(var image_raw2)"/>
<arg name="input/image3" value="$(var image_raw3)"/>
<arg name="input/image4" value="$(var image_raw4)"/>
<arg name="input/image5" value="$(var image_raw5)"/>
<arg name="input/image6" value="$(var image_raw6)"/>
<arg name="input/image7" value="$(var image_raw7)"/>
<arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
<arg name="output_clusters" value="roi_cluster/clusters"/>
</include>
</group>

<!-- simple_cluster_merger -->
<group>
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/cluster0" value="euclidean_cluster/clusters"/>
<arg name="input/cluster1" value="roi_cluster/clusters"/>
<arg name="output/clusters" value="clusters"/>
</include>
</group>

Expand All @@ -96,6 +137,8 @@
<let name="input/clustering" value="/perception/object_recognition/detection/clustering/clusters"/>
<push-ros-namespace namespace="camera_lidar_fusion"/>
<!-- Fusion camera-lidar to classify -->

<!-- euclidean clustering -->
<group>
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_cluster_fusion.launch.xml">
<arg name="input/camera_info0" value="$(var camera_info0)"/>
Expand Down
62 changes: 51 additions & 11 deletions .../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,6 @@
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
<arg name="score_threshold" default="0.35"/>

<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
<arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
Expand Down Expand Up @@ -78,29 +76,29 @@
</include>
</group>

<!-- Pointcloud map filter -->
<!-- Pointcloud filter -->
<group>
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
<arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
<arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
</include>
</group>

<!-- Clustering -->
<group>
<push-ros-namespace namespace="clustering"/>
<group>
<let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
<let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
<let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
<let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
<include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
<arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
<arg name="output_clusters" value="clusters"/>
<arg name="use_pointcloud_map" value="false"/>
<arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
<arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
<arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

Expand Down Expand Up @@ -143,12 +141,54 @@
<arg name="input/image5" value="$(var image_raw5)"/>
<arg name="input/image6" value="$(var image_raw6)"/>
<arg name="input/image7" value="$(var image_raw7)"/>
<arg name="output/clusters" value="clusters"/>
<arg name="output/clusters" value="euclidean_cluster/clusters"/>
<arg name="remove_unknown" value="$(var remove_unknown)"/>
<arg name="trust_distance" value="$(var trust_distance)"/>
</include>
</group>

<!-- roi based clustering -->
<group>
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/camera_info0" value="$(var camera_info0)"/>
<arg name="input/rois0" value="$(var detection_rois0)"/>
<arg name="input/camera_info1" value="$(var camera_info1)"/>
<arg name="input/rois1" value="$(var detection_rois1)"/>
<arg name="input/camera_info2" value="$(var camera_info2)"/>
<arg name="input/rois2" value="$(var detection_rois2)"/>
<arg name="input/camera_info3" value="$(var camera_info3)"/>
<arg name="input/rois3" value="$(var detection_rois3)"/>
<arg name="input/camera_info4" value="$(var camera_info4)"/>
<arg name="input/rois4" value="$(var detection_rois4)"/>
<arg name="input/camera_info5" value="$(var camera_info5)"/>
<arg name="input/rois5" value="$(var detection_rois5)"/>
<arg name="input/camera_info6" value="$(var camera_info6)"/>
<arg name="input/rois6" value="$(var detection_rois6)"/>
<arg name="input/camera_info7" value="$(var camera_info7)"/>
<arg name="input/rois7" value="$(var detection_rois7)"/>
<arg name="input/rois_number" value="$(var image_number)"/>
<arg name="input/image0" value="$(var image_raw0)"/>
<arg name="input/image1" value="$(var image_raw1)"/>
<arg name="input/image2" value="$(var image_raw2)"/>
<arg name="input/image3" value="$(var image_raw3)"/>
<arg name="input/image4" value="$(var image_raw4)"/>
<arg name="input/image5" value="$(var image_raw5)"/>
<arg name="input/image6" value="$(var image_raw6)"/>
<arg name="input/image7" value="$(var image_raw7)"/>
<arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
<arg name="output_clusters" value="roi_cluster/clusters"/>
</include>
</group>

<!-- simple_cluster_merger -->
<group>
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/cluster0" value="euclidean_cluster/clusters"/>
<arg name="input/cluster1" value="roi_cluster/clusters"/>
<arg name="output/clusters" value="clusters"/>
</include>
</group>

<group>
<include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
<arg name="input/objects" value="clusters"/>
Expand Down
7 changes: 6 additions & 1 deletion launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
<!-- lidar param -->
<arg name="input/pointcloud"/>
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="container_name" default="pointcloud_container"/>
Expand Down Expand Up @@ -65,6 +64,8 @@
<arg name="remove_unknown" value="$(var remove_unknown)"/>
<arg name="trust_distance" value="$(var trust_distance)"/>
<arg name="input/radar" value="$(var input/radar)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
<arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
</include>
</group>

Expand Down Expand Up @@ -94,6 +95,8 @@
<arg name="use_object_filter" value="$(var use_object_filter)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
<arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
</include>
</group>

Expand All @@ -107,6 +110,7 @@
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="input/radar" value="$(var input/radar)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

Expand All @@ -119,6 +123,7 @@
<arg name="use_object_filter" value="$(var use_object_filter)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

Expand Down
Loading

0 comments on commit c3ed832

Please sign in to comment.