diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt new file mode 100644 index 0000000000000..72c74947372f9 --- /dev/null +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.5) +project(detection_by_tracker) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# Ignore -Wnonportable-include-path in Clang for mussp +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-nonportable-include-path) +endif() + +### Find Packages +find_package(ament_cmake_auto REQUIRED) + +### Find PCL Dependencies +find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation) + +### Find Eigen Dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +### Find dependencies listed in the package.xml +ament_auto_find_build_dependencies() + +include_directories( + include + ${PCL_COMMON_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +# Generate exe file +set(DETECTION_BY_TRACKER_SRC + src/detection_by_tracker_core.cpp + src/utils.cpp +) + +ament_auto_add_library(detection_by_tracker_node SHARED + ${DETECTION_BY_TRACKER_SRC} +) + +target_link_libraries(detection_by_tracker_node + Eigen3::Eigen + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(detection_by_tracker_node + PLUGIN "DetectionByTracker" + EXECUTABLE detection_by_tracker +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/perception/detection_by_tracker/README.md b/perception/detection_by_tracker/README.md new file mode 100644 index 0000000000000..fd5281587fbb0 --- /dev/null +++ b/perception/detection_by_tracker/README.md @@ -0,0 +1,63 @@ +# detection_by_tracker + +## Purpose + +This package feeds back the tracked objects to the detection module to keep it stable and keep detecting objects. +![purpose](image/purpose.svg) + +The detection by tracker takes as input an unknown object containing a cluster of points and a tracker. +The unknown object is optimized to fit the size of the tracker so that it can continue to be detected. + +## Inner-workings / Algorithms + +The detection by tracker receives an unknown object containing a point cloud and a tracker, where the unknown object is mainly shape-fitted using euclidean clustering. +Shape fitting using euclidean clustering and other methods has a problem called under segmentation and over segmentation. + +[![segmentation_fail](image/segmentation_fail.png)](https://www.researchgate.net/figure/Examples-of-an-undersegmentation-error-top-and-an-oversegmentation-error-bottom-Each_fig1_304533062) +_Adapted from [3]_ + +Simply looking at the overlap between the unknown object and the tracker does not work. We need to take measures for under segmentation and over segmentation. + +### Policy for dealing with over segmentation + +1. Merge the unknown objects in the tracker as a single object. +2. Shape fitting using the tracker information such as angle and size as reference information. + +### Policy for dealing with under segmentation + +1. Compare the tracker and unknown objects, and determine that those with large recall and small precision are under segmented objects. +2. In order to divide the cluster of under segmented objects, it iterate the parameters to make small clusters. +3. Adjust the parameters several times and adopt the one with the highest IoU. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------- | -------------------------------------------------------------- | --------------- | +| `~/input/initial_objects` | `autoware_perception_msgs::msg::DynamicObjectWithFeatureArray` | unknown objects | +| `~/input/tracked_objects` | `autoware_perception_msgs::msg::DynamicObjectArray` | trackers | + +### Output + +| Name | Type | Description | +| ---------- | -------------------------------------------------------------- | ----------- | +| `~/output` | `autoware_perception_msgs::msg::DynamicObjectWithFeatureArray` | objects | + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] M. Himmelsbach, et al. "Tracking and classification of arbitrary objects with bottom-up/top-down detection." (2012). + +[2] Arya Senna Abdul Rachman, Arya. "3D-LIDAR Multi Object Tracking for Autonomous Driving: Multi-target Detection and Tracking under Urban Road Uncertainties." (2017). + +[3] David Held, et al. "A Probabilistic Framework for Real-time 3D Segmentation using Spatial, Temporal, and Semantic Cues." (2016). + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/detection_by_tracker/image/purpose.svg b/perception/detection_by_tracker/image/purpose.svg new file mode 100644 index 0000000000000..f92e7e511df94 --- /dev/null +++ b/perception/detection_by_tracker/image/purpose.svg @@ -0,0 +1,3 @@ + + +
detection by tracker
detection by tracker
detector
detector
multi object tracker
multi object tracker
merger
merger
Detection Module
Detection Module
Tracking Module
Tracking Module
unknown
objects
unknown...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/perception/detection_by_tracker/image/segmentation_fail.png b/perception/detection_by_tracker/image/segmentation_fail.png new file mode 100644 index 0000000000000..922578eaf5255 Binary files /dev/null and b/perception/detection_by_tracker/image/segmentation_fail.png differ diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp new file mode 100644 index 0000000000000..ab53afb5f6f17 --- /dev/null +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -0,0 +1,96 @@ +// Copyright 2021 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 DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ +#define DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class TrackerHandler +{ +private: + std::deque objects_buffer_; + +public: + TrackerHandler() = default; + void onTrackedObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg); + bool estimateTrackedObjects( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObjects & output); +}; + +class DetectionByTracker : public rclcpp::Node +{ +public: + explicit DetectionByTracker(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Subscription::SharedPtr trackers_sub_; + rclcpp::Subscription::SharedPtr + initial_objects_sub_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + TrackerHandler tracker_handler_; + std::shared_ptr shape_estimator_; + std::shared_ptr cluster_; + + void onObjects( + const autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg); + + void divideUnderSegmentedObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects, + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & in_objects, + autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); + + float optimizeUnderSegmentedObject( + const autoware_auto_perception_msgs::msg::DetectedObject & target_object, + const sensor_msgs::msg::PointCloud2 & under_segmented_cluster, + autoware_perception_msgs::msg::DetectedObjectWithFeature & output); + + void mergeOverSegmentedObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects, + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & in_objects, + autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); +}; + +#endif // DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ diff --git a/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp new file mode 100644 index 0000000000000..1205c050f8921 --- /dev/null +++ b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp @@ -0,0 +1,44 @@ +// Copyright 2020 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 DETECTION_BY_TRACKER__UTILS_HPP_ +#define DETECTION_BY_TRACKER__UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace utils +{ +double getPolygonArea(const geometry_msgs::msg::Polygon & footprint); +double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions); +double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions); +double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); +double get2dIoU( + const autoware_auto_perception_msgs::msg::DetectedObject & object1, + const autoware_auto_perception_msgs::msg::DetectedObject & object2); +double get2dPrecision( + const autoware_auto_perception_msgs::msg::DetectedObject & source_object, + const autoware_auto_perception_msgs::msg::DetectedObject & target_object); +double get2dRecall( + const autoware_auto_perception_msgs::msg::DetectedObject & source_object, + const autoware_auto_perception_msgs::msg::DetectedObject & target_object); +} // namespace utils + +#endif // DETECTION_BY_TRACKER__UTILS_HPP_ diff --git a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml b/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml new file mode 100644 index 0000000000000..c618b088d97fe --- /dev/null +++ b/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml new file mode 100644 index 0000000000000..1321fbad5a880 --- /dev/null +++ b/perception/detection_by_tracker/package.xml @@ -0,0 +1,30 @@ + + + detection_by_tracker + 1.0.0 + The ROS2 detection_by_tracker package + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + eigen3_cmake_module + + autoware_perception_msgs + autoware_utils + eigen + euclidean_cluster + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + shape_estimation + tf2 + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp new file mode 100644 index 0000000000000..f123acb3555ce --- /dev/null +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -0,0 +1,443 @@ +// Copyright 2021 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 "detection_by_tracker/detection_by_tracker_core.hpp" + +#include "detection_by_tracker/utils.hpp" + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace +{ +std::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + /*target*/ target_frame_id, /*src*/ source_frame_id, time, + rclcpp::Duration::from_seconds(0.5)); + return transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detection_by_tracker"), ex.what()); + return std::nullopt; + } +} + +bool transformTrackedObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_auto_perception_msgs::msg::TrackedObjects & output_msg) +{ + output_msg = input_msg; + + // Transform to world coordinate + if (output_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 = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); + } + } + return true; +} + +void setClusterInObjectWithFeature( + const std_msgs::msg::Header & header, const pcl::PointCloud & cluster, + autoware_perception_msgs::msg::DetectedObjectWithFeature & feature_object) +{ + sensor_msgs::msg::PointCloud2 ros_pointcloud; + pcl::toROSMsg(cluster, ros_pointcloud); + ros_pointcloud.header = header; + feature_object.feature.cluster = ros_pointcloud; +} +autoware_auto_perception_msgs::msg::Shape extendShape( + const autoware_auto_perception_msgs::msg::Shape & shape, const float scale) +{ + autoware_auto_perception_msgs::msg::Shape output = shape; + output.dimensions.x *= scale; + output.dimensions.y *= scale; + output.dimensions.z *= scale; + for (auto & point : output.footprint.points) { + point.x *= scale; + point.y *= scale; + point.z *= scale; + } + return output; +} + +autoware_auto_perception_msgs::msg::DetectedObjects toDetectedObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) +{ + autoware_auto_perception_msgs::msg::DetectedObjects out_objects; + out_objects.header = tracked_objects.header; + for (auto & tracked_object : tracked_objects.objects) { + autoware_auto_perception_msgs::msg::DetectedObject object; + object.existence_probability = tracked_object.existence_probability; + object.classification = tracked_object.classification; + object.kinematics.pose_with_covariance = tracked_object.kinematics.pose_with_covariance; + object.kinematics.has_position_covariance = true; + object.kinematics.orientation_availability = tracked_object.kinematics.orientation_availability; + object.kinematics.twist_with_covariance = tracked_object.kinematics.twist_with_covariance; + object.kinematics.has_twist = true; + object.kinematics.has_twist_covariance = true; + object.shape = tracked_object.shape; + out_objects.objects.push_back(object); + } + return out_objects; +} + +} // namespace + +void TrackerHandler::onTrackedObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) +{ + constexpr size_t max_buffer_size = 10; + + // Add tracked objects to buffer + objects_buffer_.push_front(*msg); + + // Remove old data + while (max_buffer_size < objects_buffer_.size()) { + objects_buffer_.pop_back(); + } +} + +bool TrackerHandler::estimateTrackedObjects( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObjects & output) +{ + if (objects_buffer_.empty()) { + return false; + } + + // Get the objects closest to the target time. + const auto target_objects_iter = std::min_element( + objects_buffer_.cbegin(), objects_buffer_.cend(), + [&time]( + autoware_auto_perception_msgs::msg::TrackedObjects first, + autoware_auto_perception_msgs::msg::TrackedObjects second) { + return std::fabs((time - first.header.stamp).seconds()) < + std::fabs((time - second.header.stamp).seconds()); + }); + + // Estimate the pose of the object at the target time + const auto dt = time - target_objects_iter->header.stamp; + output.header.frame_id = target_objects_iter->header.frame_id; + output.header.stamp = time; + for (const auto & object : target_objects_iter->objects) { + const auto & pose_with_covariance = object.kinematics.pose_with_covariance; + const auto & x = pose_with_covariance.pose.position.x; + const auto & y = pose_with_covariance.pose.position.y; + const auto & z = pose_with_covariance.pose.position.z; + const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation); + const auto & twist = object.kinematics.twist_with_covariance.twist; + const float & vx = twist.linear.x; + const float & wz = twist.angular.z; + + // build output + autoware_auto_perception_msgs::msg::TrackedObject estimated_object; + estimated_object.object_id = object.object_id; + estimated_object.existence_probability = object.existence_probability; + estimated_object.classification = object.classification; + estimated_object.shape = object.shape; + estimated_object.kinematics.pose_with_covariance.pose.position.x = + x + vx * std::cos(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.y = + y + vx * std::sin(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.z = z; + const float yaw_hat = autoware_utils::normalizeRadian(yaw + wz * dt.seconds()); + estimated_object.kinematics.pose_with_covariance.pose.orientation = + autoware_utils::createQuaternionFromYaw(yaw_hat); + output.objects.push_back(estimated_object); + } + return true; +} + +DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("detection_by_tracker", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + // Create publishers and subscribers + trackers_sub_ = create_subscription( + "~/input/tracked_objects", rclcpp::QoS{1}, + std::bind(&TrackerHandler::onTrackedObjects, &tracker_handler_, std::placeholders::_1)); + initial_objects_sub_ = + create_subscription( + "~/input/initial_objects", rclcpp::QoS{1}, + std::bind(&DetectionByTracker::onObjects, this, std::placeholders::_1)); + objects_pub_ = create_publisher( + "~/output", rclcpp::QoS{1}); + + shape_estimator_ = std::make_shared(true, true); + cluster_ = std::make_shared( + false, 10, 10000, 0.7, 0.3, 0); +} + +void DetectionByTracker::onObjects( + const autoware_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg) +{ + autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + detected_objects.header = input_msg->header; + + // get objects from tracking module + autoware_auto_perception_msgs::msg::TrackedObjects tracked_objects; + { + autoware_auto_perception_msgs::msg::TrackedObjects objects; + const bool available_trackers = + tracker_handler_.estimateTrackedObjects(input_msg->header.stamp, objects); + if ( + !available_trackers || + !transformTrackedObjects(objects, input_msg->header.frame_id, tf_buffer_, tracked_objects)) { + objects_pub_->publish(detected_objects); + return; + } + } + + // to simplify post processes, convert tracked_objects to DetectedObjects message. + autoware_auto_perception_msgs::msg::DetectedObjects tracked_objects_dt; + tracked_objects_dt = toDetectedObjects(tracked_objects); + + // merge over segmented objects + autoware_perception_msgs::msg::DetectedObjectsWithFeature merged_objects; + autoware_auto_perception_msgs::msg::DetectedObjects no_found_tracked_objects; + mergeOverSegmentedObjects( + tracked_objects_dt, *input_msg, no_found_tracked_objects, merged_objects); + + // divide under segmented objects + autoware_perception_msgs::msg::DetectedObjectsWithFeature divided_objects; + autoware_auto_perception_msgs::msg::DetectedObjects temp_no_found_tracked_objects; + divideUnderSegmentedObjects( + no_found_tracked_objects, *input_msg, temp_no_found_tracked_objects, divided_objects); + + // merge under/over segmented objects to build output objects + for (const auto & merged_object : merged_objects.feature_objects) { + detected_objects.objects.push_back(merged_object.object); + } + for (const auto & divided_object : divided_objects.feature_objects) { + detected_objects.objects.push_back(divided_object.object); + } + + objects_pub_->publish(detected_objects); +} + +void DetectionByTracker::divideUnderSegmentedObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects, + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & in_cluster_objects, + autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & out_objects) +{ + constexpr float recall_min_threshold = 0.4; + constexpr float precision_max_threshold = 0.5; + constexpr float max_search_range = 6.0; + constexpr float min_score_threshold = 0.4; + + out_objects.header = in_cluster_objects.header; + out_no_found_tracked_objects.header = tracked_objects.header; + + for (const auto & tracked_object : tracked_objects.objects) { + std::optional + highest_score_divided_object = std::nullopt; + float highest_score = 0.0; + + for (const auto & initial_object : in_cluster_objects.feature_objects) { + // search near object + const float distance = autoware_utils::calcDistance2d( + tracked_object.kinematics.pose_with_covariance.pose, + initial_object.object.kinematics.pose_with_covariance.pose); + if (max_search_range < distance) { + continue; + } + // detect under segmented cluster + const float recall = utils::get2dRecall(initial_object.object, tracked_object); + const float precision = utils::get2dPrecision(initial_object.object, tracked_object); + const bool is_under_segmented = + (recall_min_threshold < recall && precision < precision_max_threshold); + if (!is_under_segmented) { + continue; + } + // optimize clustering + autoware_perception_msgs::msg::DetectedObjectWithFeature divided_object; + float score = optimizeUnderSegmentedObject( + tracked_object, initial_object.feature.cluster, divided_object); + if (score < min_score_threshold) { + continue; + } + + if (highest_score < score) { + highest_score = score; + highest_score_divided_object = divided_object; + } + } + if (highest_score_divided_object) { // found + out_objects.feature_objects.push_back(highest_score_divided_object.value()); + } else { // not found + out_no_found_tracked_objects.objects.push_back(tracked_object); + } + } +} + +float DetectionByTracker::optimizeUnderSegmentedObject( + const autoware_auto_perception_msgs::msg::DetectedObject & target_object, + const sensor_msgs::msg::PointCloud2 & under_segmented_cluster, + autoware_perception_msgs::msg::DetectedObjectWithFeature & output) +{ + constexpr float iter_rate = 0.8; + constexpr int iter_max_count = 5; + constexpr float initial_cluster_range = 0.7; + float cluster_range = initial_cluster_range; + constexpr float initial_voxel_size = initial_cluster_range / 2.0f; + float voxel_size = initial_voxel_size; + + // initialize clustering parameters + euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( + false, 4, 10000, initial_cluster_range, initial_voxel_size, 0); + + // convert to pcl + pcl::PointCloud::Ptr pcl_cluster(new pcl::PointCloud); + pcl::fromROSMsg(under_segmented_cluster, *pcl_cluster); + + // iterate to find best fit divided object + float highest_iou = 0.0; + autoware_perception_msgs::msg::DetectedObjectWithFeature highest_iou_object; + for (int iter_count = 0; iter_count < iter_max_count; + ++iter_count, cluster_range *= iter_rate, voxel_size *= iter_rate) { + // divide under segmented cluster + std::vector> divided_clusters; + cluster.setTolerance(cluster_range); + cluster.setVoxelLeafSize(voxel_size); + cluster.cluster(pcl_cluster, divided_clusters); + + // find highest iou object in divided clusters + float highest_iou_in_current_iter = 0.0f; + autoware_perception_msgs::msg::DetectedObjectWithFeature highest_iou_object_in_current_iter; + highest_iou_object_in_current_iter.object.classification = target_object.classification; + for (const auto & divided_cluster : divided_clusters) { + bool is_shape_estimated = shape_estimator_->estimateShapeAndPose( + target_object.classification.front().label, divided_cluster, + tf2::getYaw(target_object.kinematics.pose_with_covariance.pose.orientation), + highest_iou_object_in_current_iter.object.shape, + highest_iou_object_in_current_iter.object.kinematics.pose_with_covariance.pose); + if (!is_shape_estimated) { + continue; + } + const float iou = utils::get2dIoU(highest_iou_object_in_current_iter.object, target_object); + if (highest_iou_in_current_iter < iou) { + highest_iou_in_current_iter = iou; + setClusterInObjectWithFeature( + under_segmented_cluster.header, divided_cluster, highest_iou_object_in_current_iter); + } + } + + // finish iteration when current score is under previous score + if (highest_iou_in_current_iter < highest_iou) { + break; + } + + // copy for next iteration + highest_iou = highest_iou_in_current_iter; + highest_iou_object = highest_iou_object_in_current_iter; + } + + // build output + highest_iou_object.object.classification = target_object.classification; + // TODO(yukkysaito): It is necessary to consider appropriate values in the future. + highest_iou_object.object.existence_probability = 0.1f; + output = highest_iou_object; + return highest_iou; +} + +void DetectionByTracker::mergeOverSegmentedObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects, + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & in_cluster_objects, + autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & out_objects) +{ + constexpr float precision_threshold = 0.5; + constexpr float max_search_range = 5.0; + out_objects.header = in_cluster_objects.header; + out_no_found_tracked_objects.header = tracked_objects.header; + + for (const auto & tracked_object : tracked_objects.objects) { + // extend shape + autoware_auto_perception_msgs::msg::DetectedObject extended_tracked_object = tracked_object; + extended_tracked_object.shape = extendShape(tracked_object.shape, /*scale*/ 1.1); + + pcl::PointCloud pcl_merged_cluster; + for (const auto & initial_object : in_cluster_objects.feature_objects) { + const float distance = autoware_utils::calcDistance2d( + tracked_object.kinematics.pose_with_covariance.pose, + initial_object.object.kinematics.pose_with_covariance.pose); + + if (max_search_range < distance) { + continue; + } + + // If there is an initial object in the tracker, it will be merged. + const float precision = utils::get2dPrecision(initial_object.object, extended_tracked_object); + if (precision < precision_threshold) { + continue; + } + pcl::PointCloud pcl_cluster; + pcl::fromROSMsg(initial_object.feature.cluster, pcl_cluster); + pcl_merged_cluster += pcl_cluster; + } + + if (pcl_merged_cluster.points.empty()) { // if clusters aren't found + out_no_found_tracked_objects.objects.push_back(tracked_object); + continue; + } + + // build output clusters + autoware_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.object.existence_probability = 0.1f; + feature_object.object.classification = tracked_object.classification; + + bool is_shape_estimated = shape_estimator_->estimateShapeAndPose( + tracked_object.classification.front().label, pcl_merged_cluster, + tf2::getYaw(tracked_object.kinematics.pose_with_covariance.pose.orientation), + feature_object.object.shape, feature_object.object.kinematics.pose_with_covariance.pose); + if (!is_shape_estimated) { + out_no_found_tracked_objects.objects.push_back(tracked_object); + continue; + } + + setClusterInObjectWithFeature(in_cluster_objects.header, pcl_merged_cluster, feature_object); + out_objects.feature_objects.push_back(feature_object); + } +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(DetectionByTracker) diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp new file mode 100644 index 0000000000000..a4a015ad846ac --- /dev/null +++ b/perception/detection_by_tracker/src/utils.cpp @@ -0,0 +1,218 @@ +// Copyright 2020 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 "detection_by_tracker/utils.hpp" + +#include + +#include +#include + +#include +#include + +namespace utils +{ +void toPolygon2d( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + autoware_utils::Polygon2d & output); +bool isClockWise(const autoware_utils::Polygon2d & polygon); +autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon); + +double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + double area = 0.0; + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + area = getRectangleArea(shape.dimensions); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + area = getCircleArea(shape.dimensions); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + area = getPolygonArea(shape.footprint); + } + return area; +} + +double getPolygonArea(const geometry_msgs::msg::Polygon & footprint) +{ + double area = 0.0; + + for (size_t i = 0; i < footprint.points.size(); ++i) { + size_t j = (i + 1) % footprint.points.size(); + area += 0.5 * (footprint.points.at(i).x * footprint.points.at(j).y - + footprint.points.at(j).x * footprint.points.at(i).y); + } + + return area; +} + +double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions) +{ + return static_cast(dimensions.x * dimensions.y); +} + +double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) +{ + return static_cast((dimensions.x / 2.0) * (dimensions.x / 2.0) * M_PI); +} + +double get2dIoU( + const autoware_auto_perception_msgs::msg::DetectedObject & object1, + const autoware_auto_perception_msgs::msg::DetectedObject & object2) +{ + autoware_utils::Polygon2d polygon1, polygon2; + toPolygon2d(object1, polygon1); + toPolygon2d(object2, polygon2); + + std::vector union_polygons; + std::vector intersection_polygons; + boost::geometry::union_(polygon1, polygon2, union_polygons); + boost::geometry::intersection(polygon1, polygon2, intersection_polygons); + + double union_area = 0.0; + double intersection_area = 0.0; + for (const auto & union_polygon : union_polygons) { + union_area += boost::geometry::area(union_polygon); + } + for (const auto & intersection_polygon : intersection_polygons) { + intersection_area += boost::geometry::area(intersection_polygon); + } + const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou; +} + +double get2dPrecision( + const autoware_auto_perception_msgs::msg::DetectedObject & source_object, + const autoware_auto_perception_msgs::msg::DetectedObject & target_object) +{ + autoware_utils::Polygon2d source_polygon, target_polygon; + toPolygon2d(source_object, source_polygon); + toPolygon2d(target_object, target_polygon); + + std::vector intersection_polygons; + boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); + + double intersection_area = 0.0; + double source_area = 0.0; + for (const auto & intersection_polygon : intersection_polygons) { + intersection_area += boost::geometry::area(intersection_polygon); + } + source_area = boost::geometry::area(source_polygon); + const double precision = std::min(1.0, intersection_area / source_area); + return precision; +} + +double get2dRecall( + const autoware_auto_perception_msgs::msg::DetectedObject & source_object, + const autoware_auto_perception_msgs::msg::DetectedObject & target_object) +{ + autoware_utils::Polygon2d source_polygon, target_polygon; + toPolygon2d(source_object, source_polygon); + toPolygon2d(target_object, target_polygon); + + std::vector intersection_polygons; + boost::geometry::union_(source_polygon, target_polygon, intersection_polygons); + + double intersection_area = 0.0; + double target_area = 0.0; + for (const auto & intersection_polygon : intersection_polygons) { + intersection_area += boost::geometry::area(intersection_polygon); + } + target_area += boost::geometry::area(target_polygon); + const double recall = std::min(1.0, intersection_area / target_area); + return recall; +} + +autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon) +{ + autoware_utils::Polygon2d inverted_polygon; + for (int i = polygon.outer().size() - 1; 0 <= i; --i) { + inverted_polygon.outer().push_back(polygon.outer().at(i)); + } + return inverted_polygon; +} + +bool isClockWise(const autoware_utils::Polygon2d & polygon) +{ + const int n = polygon.outer().size(); + const double x_offset = polygon.outer().at(0).x(); + const double y_offset = polygon.outer().at(0).y(); + double sum = 0.0; + for (std::size_t i = 0; i < polygon.outer().size(); ++i) { + sum += + (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) - + (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset); + } + + return sum < 0.0; +} + +void toPolygon2d( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + autoware_utils::Polygon2d & output) +{ + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const auto & pose = object.kinematics.pose_with_covariance.pose; + const double yaw = autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + Eigen::Matrix2d rotation; + rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + Eigen::Vector2d offset0, offset1, offset2, offset3; + offset0 = rotation * + Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + offset1 = rotation * + Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + offset2 = rotation * + Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + offset3 = rotation * + Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + output.outer().push_back(boost::geometry::make( + pose.position.x + offset0.x(), pose.position.y + offset0.y())); + output.outer().push_back(boost::geometry::make( + pose.position.x + offset1.x(), pose.position.y + offset1.y())); + output.outer().push_back(boost::geometry::make( + pose.position.x + offset2.x(), pose.position.y + offset2.y())); + output.outer().push_back(boost::geometry::make( + pose.position.x + offset3.x(), pose.position.y + offset3.y())); + output.outer().push_back(output.outer().front()); + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + const auto & center = object.kinematics.pose_with_covariance.pose.position; + const auto & radius = object.shape.dimensions.x * 0.5; + constexpr int n = 6; + for (int i = 0; i < n; ++i) { + Eigen::Vector2d point; + point.x() = std::cos( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.x; + point.y() = std::sin( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.y; + output.outer().push_back( + boost::geometry::make(point.x(), point.y())); + } + output.outer().push_back(output.outer().front()); + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + const auto & pose = object.kinematics.pose_with_covariance.pose; + for (const auto & point : object.shape.footprint.points) { + output.outer().push_back(boost::geometry::make( + pose.position.x + point.x, pose.position.y + point.y)); + } + output.outer().push_back(output.outer().front()); + } + output = isClockWise(output) ? output : inverseClockWise(output); +} + +} // namespace utils