From 1ebf6c810c45cf0282023c1e5e87c42b66467add Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Jul 2022 15:22:04 +0900 Subject: [PATCH] refactor(multi_object_tracker): use common utils (#1408) Signed-off-by: Takayuki Murooka --- .../multi_object_tracker/CMakeLists.txt | 1 - .../tracker/model/tracker_base.hpp | 6 +- .../multi_object_tracker/utils/utils.hpp | 19 -- perception/multi_object_tracker/package.xml | 1 + .../src/data_association/data_association.cpp | 20 +- .../src/multi_object_tracker_core.cpp | 47 +-- .../src/tracker/model/bicycle_tracker.cpp | 8 +- .../src/tracker/model/big_vehicle_tracker.cpp | 10 +- .../model/multiple_vehicle_tracker.cpp | 2 +- .../tracker/model/normal_vehicle_tracker.cpp | 10 +- .../tracker/model/pass_through_tracker.cpp | 3 +- .../model/pedestrian_and_bicycle_tracker.cpp | 2 +- .../src/tracker/model/pedestrian_tracker.cpp | 7 +- .../src/tracker/model/unknown_tracker.cpp | 3 +- .../multi_object_tracker/src/utils/utils.cpp | 302 ------------------ 15 files changed, 47 insertions(+), 394 deletions(-) delete mode 100644 perception/multi_object_tracker/src/utils/utils.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 0bfcff6f6b0b2..7dc7ee9d75cf3 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -21,7 +21,6 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp - src/utils/utils.cpp src/tracker/model/tracker_base.cpp src/tracker/model/big_vehicle_tracker.cpp src/tracker/model/normal_vehicle_tracker.cpp diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index ed18f5b5ff777..a094e32d1ef6b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -21,6 +21,7 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -63,7 +64,10 @@ class Tracker { return classification_; } - std::uint8_t getHighestProbLabel() const { return utils::getHighestProbLabel(classification_); } + std::uint8_t getHighestProbLabel() const + { + return perception_utils::getHighestProbLabel(classification_); + } int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } int getTotalMeasurementCount() const { return total_measurement_count_; } diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index f754b7e57d708..59ea0c747f4d1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -33,25 +33,6 @@ 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 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); -double get2dIoU( - const autoware_auto_perception_msgs::msg::TrackedObject & object1, - const autoware_auto_perception_msgs::msg::TrackedObject & object2); -std::uint8_t getHighestProbLabel( - const std::vector & classification); -autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject( - const autoware_auto_perception_msgs::msg::DetectedObject & detected_object); -geometry_msgs::msg::Polygon rotatePolygon( - const geometry_msgs::msg::Polygon & polygon, const double angle); - enum MSG_COV_IDX { X_X = 0, X_Y = 1, diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 6bc48b5e0979d..27c9d2e836358 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -17,6 +17,7 @@ eigen kalman_filter mussp + perception_utils rclcpp rclcpp_components tf2 diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 4d3d83e20f3c0..4955a104c7adc 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -16,6 +16,7 @@ #include "multi_object_tracker/data_association/solver/gnn_solver.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -25,15 +26,6 @@ namespace { -double getDistance( - const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker) -{ - const double diff_x = tracker.x - measurement.x; - const double diff_y = tracker.y - measurement.y; - // const double diff_z = tracker.z - measurement.z; - return std::sqrt(diff_x * diff_x + diff_y * diff_y); -} - double getMahalanobisDistance( const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker, const Eigen::Matrix2d & covariance) @@ -170,7 +162,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_auto_perception_msgs::msg::DetectedObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = - utils::getHighestProbLabel(measurement_object.classification); + perception_utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { @@ -178,7 +170,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = getDistance( + const double dist = tier4_autoware_utils::calcDistance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -191,7 +183,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = utils::getArea(measurement_object.shape); + const double area = tier4_autoware_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate @@ -214,9 +206,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( // 2d iou gate if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); - const double iou = utils::get2dIoU( - {measurement_object.kinematics.pose_with_covariance.pose, measurement_object.shape}, - {tracked_object.kinematics.pose_with_covariance.pose, tracked_object.shape}); + const double iou = perception_utils::get2dIoU(measurement_object, tracked_object); if (iou < min_iou) passed_gate = false; } diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d8a90e14a5e77..3873d9a7b7d6b 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -30,6 +30,7 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/multi_object_tracker_core.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -39,7 +40,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; namespace { -boost::optional getTransform( +boost::optional getTransformAnonymous( const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, const std::string & target_frame_id, const rclcpp::Time & time) { @@ -55,37 +56,6 @@ boost::optional getTransform( } } -bool transformDetectedObjects( - const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg, - const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, - autoware_auto_perception_msgs::msg::DetectedObjects & output_msg) -{ - output_msg = input_msg; - - /* transform to world coordinate */ - 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 = - 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); - // TODO(yukkysaito) transform covariance - } - } - return true; -} - inline float getVelocity(const autoware_auto_perception_msgs::msg::TrackedObject & object) { return std::hypot( @@ -217,15 +187,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) void MultiObjectTracker::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { - const auto self_transform = - getTransform(tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); + const auto self_transform = getTransformAnonymous( + tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); if (!self_transform) { return; } /* transform to world coordinate */ autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; - if (!transformDetectedObjects( + if (!perception_utils::transformObjects( *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } @@ -279,7 +249,7 @@ std::shared_ptr MultiObjectTracker::createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) const { - const std::uint8_t label = utils::getHighestProbLabel(object.classification); + const std::uint8_t label = perception_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); @@ -305,7 +275,8 @@ std::shared_ptr MultiObjectTracker::createNewTracker( void MultiObjectTracker::onTimer() { rclcpp::Time current_time = this->now(); - const auto self_transform = getTransform(tf_buffer_, world_frame_id_, "base_link", current_time); + const auto self_transform = + getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); if (!self_transform) { return; } @@ -360,7 +331,7 @@ void MultiObjectTracker::sanitizeTracker( continue; } - const auto iou = utils::get2dIoU(object1, object2); + const auto iou = perception_utils::get2dIoU(object1, object2); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index fa43caf7b5daf..05e1e2e0b1677 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -32,6 +32,7 @@ #else #include #endif +#include "perception_utils/perception_utils.hpp" #define EIGEN_MPL2_ONLY #include @@ -318,7 +319,7 @@ bool BicycleTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } @@ -337,7 +338,7 @@ bool BicycleTracker::measure( bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - object = utils::toTrackedObject(object_); + object = perception_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); @@ -410,7 +411,8 @@ bool BicycleTracker::getTrackedObject( object.shape.dimensions.z = bounding_box_.height; const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - object.shape.footprint = utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + object.shape.footprint = + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 8c15ec8d94adf..695cc779c4dda 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -30,6 +30,7 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -225,7 +226,7 @@ bool BigVehicleTracker::measureWithPose( float r_cov_x; float r_cov_y; - const uint8_t label = utils::getHighestProbLabel(object.classification); + const uint8_t label = perception_utils::getHighestProbLabel(object.classification); if (label == Label::CAR) { constexpr float r_stddev_x = 8.0; // [m] @@ -367,7 +368,7 @@ bool BigVehicleTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } @@ -386,7 +387,7 @@ bool BigVehicleTracker::measure( bool BigVehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - object = utils::toTrackedObject(object_); + object = perception_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); @@ -459,7 +460,8 @@ bool BigVehicleTracker::getTrackedObject( object.shape.dimensions.z = bounding_box_.height; const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - object.shape.footprint = utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + object.shape.footprint = + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index c80b7aa0e165d..9d66e713b5fe1 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -42,7 +42,7 @@ bool MultipleVehicleTracker::measure( { big_vehicle_tracker_.measure(object, time); normal_vehicle_tracker_.measure(object, time); - if (utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) + if (perception_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) setClassification(object.classification); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 474bb6f232cbf..66e414f8861ee 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -30,6 +30,7 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -225,7 +226,7 @@ bool NormalVehicleTracker::measureWithPose( float r_cov_x; float r_cov_y; - const uint8_t label = utils::getHighestProbLabel(object.classification); + const uint8_t label = perception_utils::getHighestProbLabel(object.classification); if (label == Label::CAR) { r_cov_x = ekf_params_.r_cov_x; @@ -369,7 +370,7 @@ bool NormalVehicleTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } @@ -388,7 +389,7 @@ bool NormalVehicleTracker::measure( bool NormalVehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - object = utils::toTrackedObject(object_); + object = perception_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); @@ -461,6 +462,7 @@ bool NormalVehicleTracker::getTrackedObject( object.shape.dimensions.z = bounding_box_.height; const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - object.shape.footprint = utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + object.shape.footprint = + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index ce7c16cddfb10..427e5263ee5a2 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -30,6 +30,7 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -79,7 +80,7 @@ bool PassThroughTracker::measure( bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - object = utils::toTrackedObject(object_); + object = perception_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 2810468bef7a1..6ddc2d9aee4f2 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -42,7 +42,7 @@ bool PedestrianAndBicycleTracker::measure( { pedestrian_tracker_.measure(object, time); bicycle_tracker_.measure(object, time); - if (utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) + if (perception_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) setClassification(object.classification); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index ebc624c54217e..72ccd20c8c7ef 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -19,6 +19,7 @@ #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include @@ -324,7 +325,7 @@ bool PedestrianTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } @@ -343,7 +344,7 @@ bool PedestrianTracker::measure( bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - object = utils::toTrackedObject(object_); + object = perception_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); @@ -423,7 +424,7 @@ bool PedestrianTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 5d2443c0aa0c5..af31e798ac0af 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -26,6 +26,7 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/unknown_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "perception_utils/perception_utils.hpp" #include #include @@ -252,7 +253,7 @@ bool UnknownTracker::measure( bool UnknownTracker::getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - object = utils::toTrackedObject(object_); + object = perception_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/multi_object_tracker/src/utils/utils.cpp b/perception/multi_object_tracker/src/utils/utils.cpp deleted file mode 100644 index 8d567b4434c70..0000000000000 --- a/perception/multi_object_tracker/src/utils/utils.cpp +++ /dev/null @@ -1,302 +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 "multi_object_tracker/utils/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::TrackedObject & object1, - const autoware_auto_perception_msgs::msg::TrackedObject & 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::TrackedObject & source_object, - const autoware_auto_perception_msgs::msg::TrackedObject & 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::TrackedObject & source_object, - const autoware_auto_perception_msgs::msg::TrackedObject & 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); -} - -std::uint8_t getHighestProbLabel( - const std::vector & classification) -{ - std::uint8_t label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - float highest_prob = 0.0; - for (const auto & _class : classification) { - if (highest_prob < _class.probability) { - highest_prob = _class.probability; - label = _class.label; - } - } - return label; -} - -autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject( - const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) -{ - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; - tracked_object.existence_probability = detected_object.existence_probability; - - tracked_object.classification = detected_object.classification; - - tracked_object.kinematics.pose_with_covariance = detected_object.kinematics.pose_with_covariance; - tracked_object.kinematics.twist_with_covariance = - detected_object.kinematics.twist_with_covariance; - - tracked_object.shape = detected_object.shape; - return tracked_object; -} - -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