Skip to content

Commit

Permalink
Merge pull request #831 from tier4/feat/improve_perception
Browse files Browse the repository at this point in the history 
feat: improve perception
  • Loading branch information
@Naophis
Naophis authored Sep 13, 2023
2 parents 0351eb8 + d0af89b commit ac9786f
Show file tree
Hide file tree
Showing 26 changed files with 1,434 additions and 67 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
87 changes: 75 additions & 12 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
@@ -1,8 +1,25 @@
<?xml version="1.0"?>
<launch>
<!-- Interface param -->
<arg name="output/objects" default="objects"/>

<!-- LiDAR param -->
<arg name="input/pointcloud"/>
<arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
<arg name="output/objects" default="objects"/>
<arg name="container_name" default="pointcloud_container"/>

<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"/>

<!-- Camera param -->
<arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
<arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
<arg name="detection_rois0" default="/perception/object_recognition/detection/rois0" description="detection rois output topic name"/>
Expand All @@ -28,15 +45,6 @@
<arg name="camera_info7" default="/camera_info7"/>
<arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
<arg name="image_number" default="1" description="choose image raw number(1-8)"/>
<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"/>
<arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
<arg name="score_threshold" default="0.35"/>
<arg name="remove_unknown" default="true"/>
<arg name="trust_distance" default="30.0"/>

<!-- Jetson AGX -->
<!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
Expand Down Expand Up @@ -71,11 +79,53 @@
<let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
<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="output_clusters" value="euclidean_cluster/clusters"/>
<arg name="use_pointcloud_map" value="false"/>
</include>
</group>

<!-- roi based clustering -->
<group>
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
<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">
<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 All @@ -87,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 Expand Up @@ -279,11 +331,22 @@
</include>
</group>

<group>
<include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
<arg name="input/object0" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
<arg name="input/object1" value="roi_pointcloud_fusion/objects"/>
<arg name="output/object" value="$(var lidar_detection_model)_roi_fusion/objects"/>
<arg name="priority_mode" value="0"/>
<arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
<arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>
</include>
</group>

<group>
<let name="merger/output/objects" value="objects_before_filter" if="$(var use_object_filter)"/>
<let name="merger/output/objects" value="$(var output/objects)" unless="$(var use_object_filter)"/>
<include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
<arg name="input/object0" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
<arg name="input/object0" value="$(var lidar_detection_model)_roi_fusion/objects"/>
<arg name="input/object1" value="detection_by_tracker/objects"/>
<arg name="priority_mode" value="0"/>
<arg name="output/object" value="$(var merger/output/objects)"/>
Expand Down
Loading

0 comments on commit ac9786f

Please sign in to comment.