Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(perception): rearrange clustering pipeline v0.11.0 #849

Merged
merged 3 commits into from
Sep 22, 2023
Merged

refactor(perception): rearrange clustering pipeline v0.11.0 #849

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6479c94
feat(image_projection_based_fusion): add roi based clustering for sma…
badai-nguyen Sep 12, 2023
2f253b8
refactor(perception): rearrange clustering pipeline (#4999)
badai-nguyen Sep 19, 2023
8b60391
fix(tier4_perception_launch): camera lidar fusion launch (#4983)
miursh Sep 14, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
64 changes: 64 additions & 0 deletions common/object_recognition_utils/include/object_recognition_utils/transform.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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