diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt index b1e1a07035ac7..236268438f852 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -27,7 +27,6 @@ include_directories( # 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 diff --git a/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp deleted file mode 100644 index 98a4df7048c23..0000000000000 --- a/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// 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); -geometry_msgs::msg::Polygon rotatePolygon( - const geometry_msgs::msg::Polygon & polygon, const double angle); -} // namespace utils - -#endif // DETECTION_BY_TRACKER__UTILS_HPP_ diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml index d42d650b731d8..1f31dfd6d5fc8 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/detection_by_tracker/package.xml @@ -16,6 +16,7 @@ euclidean_cluster libpcl-all-dev pcl_conversions + perception_utils rclcpp rclcpp_components shape_estimation diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 57d2600bb5c55..8e9d08e6b89f9 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -14,7 +14,7 @@ #include "detection_by_tracker/detection_by_tracker_core.hpp" -#include "detection_by_tracker/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -29,52 +29,6 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; 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, tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_object) @@ -99,27 +53,6 @@ autoware_auto_perception_msgs::msg::Shape extendShape( 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; -} - boost::optional getReferenceYawInfo(const uint8_t label, const float yaw) { const bool is_vehicle = @@ -234,13 +167,13 @@ void DetectionByTracker::onObjects( tracker_handler_.estimateTrackedObjects(input_msg->header.stamp, objects); if ( !available_trackers || - !transformTrackedObjects( + !perception_utils::transformObjects( objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. - tracked_objects = toDetectedObjects(transformed_objects); + tracked_objects = perception_utils::toDetectedObjects(transformed_objects); } debugger_->publishInitialObjects(*input_msg); debugger_->publishTrackedObjects(tracked_objects); @@ -300,8 +233,9 @@ void DetectionByTracker::divideUnderSegmentedObjects( 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 float recall = perception_utils::get2dRecall(initial_object.object, tracked_object); + const float precision = + perception_utils::get2dPrecision(initial_object.object, tracked_object); const bool is_under_segmented = (recall_min_threshold < recall && precision < precision_max_threshold); if (!is_under_segmented) { @@ -375,7 +309,8 @@ float DetectionByTracker::optimizeUnderSegmentedObject( if (!is_shape_estimated) { continue; } - const float iou = utils::get2dIoU(highest_iou_object_in_current_iter.object, target_object); + const float iou = + perception_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( @@ -396,7 +331,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( // build output highest_iou_object.object.classification = target_object.classification; highest_iou_object.object.existence_probability = - utils::get2dIoU(target_object, highest_iou_object.object); + perception_utils::get2dIoU(target_object, highest_iou_object.object); output = highest_iou_object; return highest_iou; @@ -432,7 +367,8 @@ void DetectionByTracker::mergeOverSegmentedObjects( } // If there is an initial object in the tracker, it will be merged. - const float precision = utils::get2dPrecision(initial_object.object, extended_tracked_object); + const float precision = + perception_utils::get2dPrecision(initial_object.object, extended_tracked_object); if (precision < precision_threshold) { continue; } @@ -461,7 +397,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( } feature_object.object.existence_probability = - utils::get2dIoU(tracked_object, feature_object.object); + perception_utils::get2dIoU(tracked_object, feature_object.object); setClusterInObjectWithFeature(in_cluster_objects.header, pcl_merged_cluster, feature_object); out_objects.feature_objects.push_back(feature_object); } diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp deleted file mode 100644 index ffd75b0085117..0000000000000 --- a/perception/detection_by_tracker/src/utils.cpp +++ /dev/null @@ -1,271 +0,0 @@ -// 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 geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, - tier4_autoware_utils::Polygon2d & output); -bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon); -tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_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 std::tuple< - const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object1, - const std::tuple< - const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object2) -{ - tier4_autoware_utils::Polygon2d polygon1, polygon2; - toPolygon2d(std::get<0>(object1), std::get<1>(object1), polygon1); - toPolygon2d(std::get<0>(object2), std::get<1>(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 intersection_area = 0.0; - double union_area = 0.0; - for (const auto & intersection_polygon : intersection_polygons) { - intersection_area += boost::geometry::area(intersection_polygon); - } - if (intersection_area == 0.0) return 0.0; - - for (const auto & union_polygon : union_polygons) { - union_area += boost::geometry::area(union_polygon); - } - const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area); - return iou; -} - -double get2dIoU( - const autoware_auto_perception_msgs::msg::DetectedObject & object1, - const autoware_auto_perception_msgs::msg::DetectedObject & object2) -{ - return get2dIoU( - {object1.kinematics.pose_with_covariance.pose, object1.shape}, - {object2.kinematics.pose_with_covariance.pose, object2.shape}); -} - -double get2dPrecision( - const std::tuple< - const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & - source_object, - const std::tuple< - const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & - target_object) -{ - tier4_autoware_utils::Polygon2d source_polygon, target_polygon; - toPolygon2d(std::get<0>(source_object), std::get<1>(source_object), source_polygon); - toPolygon2d(std::get<0>(target_object), std::get<1>(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); - } - if (intersection_area == 0.0) return 0.0; - - source_area = boost::geometry::area(source_polygon); - const double precision = std::min(1.0, intersection_area / source_area); - return precision; -} - -double get2dPrecision( - const autoware_auto_perception_msgs::msg::DetectedObject & source_object, - const autoware_auto_perception_msgs::msg::DetectedObject & target_object) -{ - return get2dPrecision( - {source_object.kinematics.pose_with_covariance.pose, source_object.shape}, - {target_object.kinematics.pose_with_covariance.pose, target_object.shape}); -} - -double get2dRecall( - const std::tuple< - const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & - source_object, - const std::tuple< - const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & - target_object) -{ - tier4_autoware_utils::Polygon2d source_polygon, target_polygon; - toPolygon2d(std::get<0>(source_object), std::get<1>(source_object), source_polygon); - toPolygon2d(std::get<0>(target_object), std::get<1>(target_object), target_polygon); - - std::vector intersection_polygons; - boost::geometry::intersection(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); - } - if (intersection_area == 0.0) return 0.0; - - target_area += boost::geometry::area(target_polygon); - const double recall = std::min(1.0, intersection_area / target_area); - return recall; -} - -double get2dRecall( - const autoware_auto_perception_msgs::msg::DetectedObject & source_object, - const autoware_auto_perception_msgs::msg::DetectedObject & target_object) -{ - return get2dRecall( - {source_object.kinematics.pose_with_covariance.pose, source_object.shape}, - {target_object.kinematics.pose_with_covariance.pose, target_object.shape}); -} - -tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon) -{ - tier4_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 tier4_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 geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, - tier4_autoware_utils::Polygon2d & output) -{ - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - const double yaw = tier4_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(shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); - offset1 = rotation * Eigen::Vector2d(shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); - offset2 = rotation * Eigen::Vector2d(-shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); - offset3 = rotation * Eigen::Vector2d(-shape.dimensions.x * 0.5f, 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 (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - const auto & center = pose.position; - const auto & radius = 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 (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - const double yaw = tf2::getYaw(pose.orientation); - const auto rotated_footprint = rotatePolygon(shape.footprint, yaw); - for (const auto & point : rotated_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); -} - -geometry_msgs::msg::Polygon rotatePolygon( - const geometry_msgs::msg::Polygon & polygon, const double angle) -{ - const double cos = std::cos(angle); - const double sin = std::sin(angle); - geometry_msgs::msg::Polygon rotated_polygon; - for (const auto & point : polygon.points) { - auto rotated_point = point; - rotated_point.x = cos * point.x - sin * point.y; - rotated_point.y = sin * point.x + cos * point.y; - rotated_polygon.points.push_back(rotated_point); - } - return rotated_polygon; -} -} // namespace utils