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

feat: improve perception #831

Merged
merged 3 commits into from
Sep 13, 2023
Merged

feat: improve perception #831

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
4c2772c
fix(tensorrt_yolox): update yolox-s model (#4949)
badai-nguyen Sep 13, 2023
7c15e16
fix(tier4_percetion_launch): fix order of Camera-Lidar-Radar fusion p…
scepter914 Aug 29, 2023
d0af89b
feat(image_projection_based_fusion): add roi based clustering for sma…
badai-nguyen Sep 12, 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
87 changes: 75 additions & 12 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
@@ -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
Loading