From ce6ccec0872481c136dc8df8fe4af08992ffee48 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Thu, 30 Jun 2022 15:01:07 +0900 Subject: [PATCH] feat: update data association in object merger (#1001) * backup Signed-off-by: Yukihiro Saito * backup Signed-off-by: Yukihiro Saito * bug fix Signed-off-by: Yukihiro Saito * backup Signed-off-by: Yukihiro Saito * backup Signed-off-by: Yukihiro Saito * ci(pre-commit): autofix * cosmetic change Signed-off-by: Yukihiro Saito * add parameters Signed-off-by: Yukihiro Saito Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/detection_by_tracker/src/utils.cpp | 16 +- .../multi_object_tracker/src/utils/utils.cpp | 16 +- perception/object_merger/CMakeLists.txt | 25 +- perception/object_merger/README.md | 9 +- .../config/data_association_matrix.param.yaml | 44 ++ .../data_association.hpp | 35 +- .../data_association/solver/gnn_solver.hpp | 22 + .../solver/gnn_solver_interface.hpp | 35 ++ .../solver/mu_successive_shortest_path.hpp | 37 ++ .../solver/successive_shortest_path.hpp | 37 ++ .../object_association_merger/node.hpp | 24 +- .../object_association_merger/utils/utils.hpp | 66 +++ .../object_association_merger.launch.xml | 2 + perception/object_merger/package.xml | 10 +- .../data_association/data_association.cpp | 415 +++++------------- .../mu_successive_shortest_path_wrapper.cpp} | 30 +- .../successive_shortest_path.cpp | 74 +--- .../src/object_association_merger/node.cpp | 183 ++++++-- .../object_association_merger/utils/utils.cpp | 241 +++++++++- 19 files changed, 891 insertions(+), 430 deletions(-) create mode 100644 perception/object_merger/config/data_association_matrix.param.yaml rename perception/object_merger/include/object_association_merger/{ => data_association}/data_association.hpp (63%) create mode 100644 perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp create mode 100644 perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp create mode 100644 perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp create mode 100644 perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp rename perception/object_merger/{include/object_association_merger/successive_shortest_path.hpp => src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp} (55%) diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp index 8ee4f76edfbd6..ffd75b0085117 100644 --- a/perception/detection_by_tracker/src/utils.cpp +++ b/perception/detection_by_tracker/src/utils.cpp @@ -81,14 +81,16 @@ double get2dIoU( 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); - } + 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; } @@ -122,6 +124,8 @@ double get2dPrecision( 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; @@ -149,13 +153,15 @@ double get2dRecall( toPolygon2d(std::get<0>(target_object), std::get<1>(target_object), target_polygon); std::vector intersection_polygons; - boost::geometry::union_(source_polygon, target_polygon, 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; diff --git a/perception/multi_object_tracker/src/utils/utils.cpp b/perception/multi_object_tracker/src/utils/utils.cpp index 677762b8e33e9..8d567b4434c70 100644 --- a/perception/multi_object_tracker/src/utils/utils.cpp +++ b/perception/multi_object_tracker/src/utils/utils.cpp @@ -81,14 +81,16 @@ double get2dIoU( 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); - } + 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; } @@ -122,6 +124,8 @@ double get2dPrecision( 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; @@ -149,13 +153,15 @@ double get2dRecall( toPolygon2d(std::get<0>(target_object), std::get<1>(target_object), target_polygon); std::vector intersection_polygons; - boost::geometry::union_(source_polygon, target_polygon, 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; diff --git a/perception/object_merger/CMakeLists.txt b/perception/object_merger/CMakeLists.txt index dd435b0ab28bc..fa00465902a60 100644 --- a/perception/object_merger/CMakeLists.txt +++ b/perception/object_merger/CMakeLists.txt @@ -4,16 +4,34 @@ project(object_merger) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(PCL REQUIRED COMPONENTS common filters) +# Ignore -Wnonportable-include-path in Clang for mussp +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-nonportable-include-path) +endif() + +### Find Eigen Dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(mu_successive_shortest_path SHARED + src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +) ament_auto_add_library(object_association_merger SHARED src/object_association_merger/utils/utils.cpp src/object_association_merger/data_association/data_association.cpp - src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp src/object_association_merger/node.cpp ) -target_link_libraries(object_association_merger ${PCL_LIBRARIES}) +target_link_libraries(object_association_merger + mu_successive_shortest_path + Eigen3::Eigen +) rclcpp_components_register_node(object_association_merger PLUGIN "object_association::ObjectAssociationMergerNode" @@ -22,4 +40,5 @@ rclcpp_components_register_node(object_association_merger ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_merger/README.md b/perception/object_merger/README.md index 6f52416df75e2..6993bb57b4f80 100644 --- a/perception/object_merger/README.md +++ b/perception/object_merger/README.md @@ -25,7 +25,14 @@ The successive shortest path algorithm is used to solve the data association pro ## Parameters -No Parameters. +| Name | Type | Description | +| -------------------- | ------ | ------------------------------------------- | +| `can_assign_matrix` | double | Assignment table for data association | +| `max_dist_matrix` | double | Maximum distance table for data association | +| `max_area_matrix` | double | Maximum area table for data association | +| `min_area_matrix` | double | Minimum area table for data association | +| `max_rad_matrix` | double | Maximum angle table for data association | +| `base_link_frame_id` | double | association frame | ## Assumptions / Known limits diff --git a/perception/object_merger/config/data_association_matrix.param.yaml b/perception/object_merger/config/data_association_matrix.param.yaml new file mode 100644 index 0000000000000..e1bf2c67b3056 --- /dev/null +++ b/perception/object_merger/config/data_association_matrix.param.yaml @@ -0,0 +1,44 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN diff --git a/perception/object_merger/include/object_association_merger/data_association.hpp b/perception/object_merger/include/object_association_merger/data_association/data_association.hpp similarity index 63% rename from perception/object_merger/include/object_association_merger/data_association.hpp rename to perception/object_merger/include/object_association_merger/data_association/data_association.hpp index 869d52949e4af..c03dd40b0d3b7 100644 --- a/perception/object_merger/include/object_association_merger/data_association.hpp +++ b/perception/object_merger/include/object_association_merger/data_association/data_association.hpp @@ -11,42 +11,49 @@ // 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. +// +// +// Author: v1.0 Yukihiro Saito +// -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_ +#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ #include +#include #include #include #define EIGEN_MPL2_ONLY +#include "object_association_merger/data_association/solver/gnn_solver.hpp" + #include #include #include -#include -#include + class DataAssociation { private: - double getDistance( - const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1); - geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); Eigen::MatrixXi can_assign_matrix_; Eigen::MatrixXd max_dist_matrix_; - Eigen::MatrixXd max_area_matrix_; - Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; const double score_threshold_; + std::unique_ptr gnn_solver_ptr_; public: - DataAssociation(); - bool assign( + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector); + void assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); Eigen::MatrixXd calcScoreMatrix( - const autoware_auto_perception_msgs::msg::DetectedObjects & object0, - const autoware_auto_perception_msgs::msg::DetectedObjects & object1); + const autoware_auto_perception_msgs::msg::DetectedObjects & objects0, + const autoware_auto_perception_msgs::msg::DetectedObjects & objects1); virtual ~DataAssociation() {} }; -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_ +#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp b/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..8432a4e870e20 --- /dev/null +++ b/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" +#include "object_association_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "object_association_merger/data_association/solver/successive_shortest_path.hpp" + +#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp b/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp new file mode 100644 index 0000000000000..ec4bc907d85f6 --- /dev/null +++ b/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp @@ -0,0 +1,35 @@ +// 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ + +#include +#include + +namespace gnn_solver +{ +class GnnSolverInterface +{ +public: + GnnSolverInterface() = default; + virtual ~GnnSolverInterface() = default; + + virtual void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) = 0; +}; +} // namespace gnn_solver + +#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp new file mode 100644 index 0000000000000..f9d88bd793db1 --- /dev/null +++ b/perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class MuSSP : public GnnSolverInterface +{ +public: + MuSSP() = default; + ~MuSSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp b/perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp new file mode 100644 index 0000000000000..2487faaa07c0e --- /dev/null +++ b/perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class SSP : public GnnSolverInterface +{ +public: + SSP() = default; + ~SSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 0558659c2d28b..db4f0511abba8 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -15,7 +15,8 @@ #ifndef OBJECT_ASSOCIATION_MERGER__NODE_HPP_ #define OBJECT_ASSOCIATION_MERGER__NODE_HPP_ -#include +#include "object_association_merger/data_association/data_association.hpp" + #include #include @@ -23,11 +24,21 @@ #include #include #include -#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + #include #include #include +#include namespace object_association { @@ -53,7 +64,14 @@ class ObjectAssociationMergerNode : public rclcpp::Node SyncPolicy; typedef message_filters::Synchronizer Sync; Sync sync_; - DataAssociation data_association_; + std::unique_ptr data_association_; + std::string base_link_frame_id_; // associated with the base_link frame + bool remove_overlapped_unknown_objects_; + struct + { + double precision_threshold; + double recall_threshold; + } overlapped_judge_param_; }; } // namespace object_association diff --git a/perception/object_merger/include/object_association_merger/utils/utils.hpp b/perception/object_merger/include/object_association_merger/utils/utils.hpp index c686244d1dd4b..9f0eebc7b849e 100644 --- a/perception/object_merger/include/object_association_merger/utils/utils.hpp +++ b/perception/object_merger/include/object_association_merger/utils/utils.hpp @@ -11,15 +11,24 @@ // 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. +// +// +// Author: v1.0 Yukihiro Saito +// #ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ #define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ +#include + +#include #include #include #include #include +#include +#include namespace utils { @@ -27,6 +36,63 @@ 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::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); +std::uint8_t getHighestProbLabel( + const std::vector & classification); +geometry_msgs::msg::Polygon rotatePolygon( + const geometry_msgs::msg::Polygon & polygon, const double & angle); + +enum MSG_COV_IDX { + X_X = 0, + X_Y = 1, + X_Z = 2, + X_ROLL = 3, + X_PITCH = 4, + X_YAW = 5, + Y_X = 6, + Y_Y = 7, + Y_Z = 8, + Y_ROLL = 9, + Y_PITCH = 10, + Y_YAW = 11, + Z_X = 12, + Z_Y = 13, + Z_Z = 14, + Z_ROLL = 15, + Z_PITCH = 16, + Z_YAW = 17, + ROLL_X = 18, + ROLL_Y = 19, + ROLL_Z = 20, + ROLL_ROLL = 21, + ROLL_PITCH = 22, + ROLL_YAW = 23, + PITCH_X = 24, + PITCH_Y = 25, + PITCH_Z = 26, + PITCH_ROLL = 27, + PITCH_PITCH = 28, + PITCH_YAW = 29, + YAW_X = 30, + YAW_Y = 31, + YAW_Z = 32, + YAW_ROLL = 33, + YAW_PITCH = 34, + YAW_YAW = 35 +}; } // namespace utils #endif // OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 33be0f4c37292..85b9eff90d25e 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -3,10 +3,12 @@ + + diff --git a/perception/object_merger/package.xml b/perception/object_merger/package.xml index 7f86280369a98..d1298a724e979 100644 --- a/perception/object_merger/package.xml +++ b/perception/object_merger/package.xml @@ -8,19 +8,19 @@ Apache License 2.0 ament_cmake_auto + eigen3_cmake_module autoware_cmake autoware_auto_perception_msgs - libpcl-all-dev - message_filters - pcl_conversions + eigen + mussp rclcpp rclcpp_components - sensor_msgs tf2 tf2_ros - tf2_sensor_msgs + tier4_autoware_utils + tier4_perception_msgs ament_lint_auto autoware_lint_common diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp index 7b17db0b4650b..0c366d1a20616 100644 --- a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -12,253 +12,82 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "object_association_merger/data_association/data_association.hpp" -#include +#include "object_association_merger/data_association/solver/gnn_solver.hpp" +#include "object_association_merger/utils/utils.hpp" #include +#include +#include #include #include -DataAssociation::DataAssociation() : score_threshold_(0.1) +namespace { - can_assign_matrix_ = Eigen::MatrixXi::Identity(20, 20); - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 1; - can_assign_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; - max_dist_matrix_ = Eigen::MatrixXd::Constant(20, 20, 1.0); - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 4.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 3.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 2.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 2.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; - max_dist_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) = 2.0; - max_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* large number */ 10000.0); - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 5.0 * 5.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.2 * 5.5; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 2.2 * 5.5; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 2.5 * 7.9; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.7 * 12.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.5 * 7.9; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 2.2 * 5.5; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 2.5 * 7.9; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.7 * 12.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.7 * 12.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 2.2 * 5.5; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 2.5 * 7.9; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.7 * 12.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.5; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.5; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 3.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 3.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.5; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 3.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; - max_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) = 2.0; - min_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* small number */ 0.0); - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 1.2 * 3.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1.2 * 3.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1.5 * 4.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.0 * 5.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 1.5 * 4.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1.2 * 3.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1.5 * 4.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.0 * 5.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0 * 5.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1.2 * 3.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1.5 * 4.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS, - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.0 * 5.0; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0.001; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 0.001; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 0.001; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0.001; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 0.001; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 0.001; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0.001; - min_area_matrix_( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) = 0.001; +double getDistance( + const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1) +{ + const double diff_x = point1.x - point0.x; + const double diff_y = point1.y - point0.y; + // const double diff_z = point1.z - point0.z; + return std::sqrt(diff_x * diff_x + diff_y * diff_y); +} + +double getFormedYawAngle( + const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, + const bool distinguish_front_or_back = true) +{ + const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; + const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; + // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 + double fixed_yaw0 = yaw0; + while (angle_range <= yaw1 - fixed_yaw0) { + fixed_yaw0 = fixed_yaw0 + angle_step; + } + while (angle_range <= fixed_yaw0 - yaw1) { + fixed_yaw0 = fixed_yaw0 - angle_step; + } + return std::fabs(fixed_yaw0 - yaw1); +} +} // namespace + +DataAssociation::DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector) +: score_threshold_(0.01) +{ + { + const int assign_label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), assign_label_num, assign_label_num); + can_assign_matrix_ = can_assign_matrix_tmp.transpose(); + } + { + const int max_dist_label_num = static_cast(std::sqrt(max_dist_vector.size())); + Eigen::Map max_dist_matrix_tmp( + max_dist_vector.data(), max_dist_label_num, max_dist_label_num); + max_dist_matrix_ = max_dist_matrix_tmp.transpose(); + } + { + const int max_rad_label_num = static_cast(std::sqrt(max_rad_vector.size())); + Eigen::Map max_rad_matrix_tmp( + max_rad_vector.data(), max_rad_label_num, max_rad_label_num); + max_rad_matrix_ = max_rad_matrix_tmp.transpose(); + } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } + + gnn_solver_ptr_ = std::make_unique(); } -bool DataAssociation::assign( +void DataAssociation::assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment) { @@ -270,7 +99,7 @@ bool DataAssociation::assign( } } // Solve - assignment_problem::MaximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); + gnn_solver_ptr_->maximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); for (auto itr = direct_assignment.begin(); itr != direct_assignment.end();) { if (src(itr->first, itr->second) < score_threshold_) { @@ -288,81 +117,63 @@ bool DataAssociation::assign( ++itr; } } - return true; } Eigen::MatrixXd DataAssociation::calcScoreMatrix( - const autoware_auto_perception_msgs::msg::DetectedObjects & object0, - const autoware_auto_perception_msgs::msg::DetectedObjects & object1) + const autoware_auto_perception_msgs::msg::DetectedObjects & objects0, + const autoware_auto_perception_msgs::msg::DetectedObjects & objects1) { Eigen::MatrixXd score_matrix = - Eigen::MatrixXd::Zero(object1.objects.size(), object0.objects.size()); - for (size_t object1_idx = 0; object1_idx < object1.objects.size(); ++object1_idx) { - for (size_t object0_idx = 0; object0_idx < object0.objects.size(); ++object0_idx) { + Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size()); + for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) { + const autoware_auto_perception_msgs::msg::DetectedObject & object1 = + objects1.objects.at(objects1_idx); + const std::uint8_t object1_label = utils::getHighestProbLabel(object1.classification); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const autoware_auto_perception_msgs::msg::DetectedObject & object0 = + objects0.objects.at(objects0_idx); + const std::uint8_t object0_label = utils::getHighestProbLabel(object0.classification); + double score = 0.0; - if (can_assign_matrix_( - object1.objects.at(object1_idx).classification.front().label, - object0.objects.at(object0_idx).classification.front().label)) { - const double max_dist = max_dist_matrix_( - object1.objects.at(object1_idx).classification.front().label, - object0.objects.at(object0_idx).classification.front().label); - const double max_area = max_area_matrix_( - object1.objects.at(object1_idx).classification.front().label, - object0.objects.at(object0_idx).classification.front().label); - const double min_area = min_area_matrix_( - object1.objects.at(object1_idx).classification.front().label, - object0.objects.at(object0_idx).classification.front().label); + if (can_assign_matrix_(object1_label, object0_label)) { + const double max_dist = max_dist_matrix_(object1_label, object0_label); const double dist = getDistance( - object0.objects.at(object0_idx).kinematics.pose_with_covariance.pose.position, - object1.objects.at(object1_idx).kinematics.pose_with_covariance.pose.position); - const double area0 = utils::getArea(object0.objects.at(object0_idx).shape); - const double area1 = utils::getArea(object1.objects.at(object1_idx).shape); - // the score (=cost) is reversed in ssp solver - score = (max_dist - std::min(dist, max_dist)) / max_dist; - if (max_dist < dist) { - score = 0.0; + object0.kinematics.pose_with_covariance.pose.position, + object1.kinematics.pose_with_covariance.pose.position); + + bool passed_gate = true; + // dist gate + if (passed_gate) { + if (max_dist < dist) passed_gate = false; } - if (area0 < min_area || max_area < area0) { - score = 0.0; + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(object1_label, object0_label); + const double angle = getFormedYawAngle( + object0.kinematics.pose_with_covariance.pose.orientation, + object1.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) + passed_gate = false; } - if (area1 < min_area || max_area < area1) { - score = 0.0; + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(object1_label, object0_label); + const double iou = utils::get2dIoU( + {object0.kinematics.pose_with_covariance.pose, object0.shape}, + {object1.kinematics.pose_with_covariance.pose, object1.shape}); + if (iou < min_iou) passed_gate = false; + } + + // all gate is passed + if (passed_gate) { + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; } } - score_matrix(object1_idx, object0_idx) = score; + score_matrix(objects1_idx, objects0_idx) = score; } } - return score_matrix; -} - -double DataAssociation::getDistance( - const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1) -{ - const double diff_x = point1.x - point0.x; - const double diff_y = point1.y - point0.y; - // const double diff_z = point1.z - point0.z; - return std::sqrt(diff_x * diff_x + diff_y * diff_y); -} -geometry_msgs::msg::Point DataAssociation::getCentroid( - const sensor_msgs::msg::PointCloud2 & pointcloud) -{ - geometry_msgs::msg::Point centroid; - centroid.x = 0; - centroid.y = 0; - centroid.z = 0; - for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), - iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - centroid.x += *iter_x; - centroid.y += *iter_y; - centroid.z += *iter_z; - } - centroid.x = - centroid.x / (static_cast(pointcloud.height) * static_cast(pointcloud.width)); - centroid.y = - centroid.y / (static_cast(pointcloud.height) * static_cast(pointcloud.width)); - centroid.z = - centroid.z / (static_cast(pointcloud.height) * static_cast(pointcloud.width)); - return centroid; + return score_matrix; } diff --git a/perception/object_merger/include/object_association_merger/successive_shortest_path.hpp b/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp similarity index 55% rename from perception/object_merger/include/object_association_merger/successive_shortest_path.hpp rename to perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp index 7fdeb2f0fe729..bb3d1ed8fdaa4 100644 --- a/perception/object_merger/include/object_association_merger/successive_shortest_path.hpp +++ b/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -12,18 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#include "object_association_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include + +#include +#include +#include +#include +#include #include #include -namespace assignment_problem +namespace gnn_solver { -// See IMPORTANT NOTE at the top of the file. -void MaximizeLinearAssignment( +void MuSSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment); -} // namespace assignment_problem + std::unordered_map * reverse_assignment) +{ + // Terminate if the graph is empty + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } -#endif // OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_ + // Solve DA by muSSP + solve_muSSP(cost, direct_assignment, reverse_assignment); +} +} // namespace gnn_solver diff --git a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp index ef8a582604bc8..904d57b71f072 100644 --- a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "object_association_merger/data_association/solver/successive_shortest_path.hpp" #include #include @@ -22,13 +22,7 @@ #include #include -// #include -// #include -// #include -// #include -// #include - -namespace assignment_problem +namespace gnn_solver { struct ResidualEdge { @@ -49,7 +43,7 @@ struct ResidualEdge } }; -void MaximizeLinearAssignment( +void SSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) { @@ -102,8 +96,8 @@ void MaximizeLinearAssignment( // - {1, ..., n_agents}: agent nodes // - {n_agents+1, ..., n_agents+n_tasks}: task nodes // - n_agents+n_tasks+1: sink node - // - {n_agents+n_tasks+2, ..., - // n_agents+n_tasks+1+n_agents}: dummy node (when sparse_cost is true) + // - {n_agents+n_tasks+2, ..., n_agents+n_tasks+1+n_agents}: + // dummy node (when sparse_cost is true) std::vector> adjacency_list(n_nodes); // Reserve memory @@ -187,27 +181,6 @@ void MaximizeLinearAssignment( } } - // // Print adjacency list - // std::cout << std::endl; - // for (int v = 0; v < n_nodes; v++) - // { - // std::cout << v << ": "; - // for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - // it_incident_edge != adjacency_list.at(v).cend(); - // it_incident_edge++) - // { - // std::cout << "(" << it_incident_edge->first << ", " << - // it_incident_edge->second.cost << ")"; - // } - // std::cout << std::endl; - // } - - // end_time = std::chrono::system_clock::now(); - // double time = static_cast( - // std::chrono::duration_cast( - // end_time - start_time).count() / 1000.0); - // std::cout << " " << time << " "; - // Maximum flow value const int max_flow = std::min(n_agents, n_tasks); @@ -215,36 +188,36 @@ void MaximizeLinearAssignment( std::vector potentials(n_nodes, 0); // Shortest path lengths - std::vector dists(n_nodes, INF_DIST); + std::vector distances(n_nodes, INF_DIST); // Whether previously visited the node or not std::vector is_visited(n_nodes, false); // Parent node () - std::vector> prevs(n_nodes); + std::vector> prev_values(n_nodes); for (int i = 0; i < max_flow; ++i) { // Initialize priority queue () std::priority_queue< std::pair, std::vector>, std::greater>> - pqueue; + p_queue; // Reset all trajectory states if (i > 0) { - std::fill(dists.begin(), dists.end(), INF_DIST); + std::fill(distances.begin(), distances.end(), INF_DIST); std::fill(is_visited.begin(), is_visited.end(), false); } // Start trajectory from the source node - pqueue.push(std::make_pair(0, source)); - dists.at(source) = 0; + p_queue.push(std::make_pair(0, source)); + distances.at(source) = 0; - while (!pqueue.empty()) { + while (!p_queue.empty()) { // Get the next element - std::pair cur_elem = pqueue.top(); + std::pair cur_elem = p_queue.top(); // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl; - pqueue.pop(); + p_queue.pop(); double cur_node_dist = cur_elem.first; int cur_node = cur_elem.second; @@ -253,7 +226,7 @@ void MaximizeLinearAssignment( if (is_visited.at(cur_node)) { continue; } - assert(cur_node_dist == dists.at(cur_node)); + assert(cur_node_dist == distances.at(cur_node)); // Mark as visited is_visited.at(cur_node) = true; @@ -274,19 +247,19 @@ void MaximizeLinearAssignment( double reduced_cost = it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); - if (dists.at(it_incident_edge->dst) > reduced_cost) { - dists.at(it_incident_edge->dst) = reduced_cost; - prevs.at(it_incident_edge->dst) = + if (distances.at(it_incident_edge->dst) > reduced_cost) { + distances.at(it_incident_edge->dst) = reduced_cost; + prev_values.at(it_incident_edge->dst) = std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin()); // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl; - pqueue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); + p_queue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); } } } } // Shortest path length to sink is greater than MAX_COST, - // which means no non-dummy routes left ,terminate + // which means no non-dummy routes left, terminate if (potentials.at(sink) >= MAX_COST) { break; } @@ -294,7 +267,7 @@ void MaximizeLinearAssignment( // Update potentials of unvisited nodes for (int v = 0; v < n_nodes; ++v) { if (!is_visited.at(v)) { - potentials.at(v) += dists.at(sink); + potentials.at(v) += distances.at(sink); } } // //Print potentials @@ -308,7 +281,8 @@ void MaximizeLinearAssignment( int v = sink; int prev_v; while (v != source) { - ResidualEdge & e_forward = adjacency_list.at(prevs.at(v).first).at(prevs.at(v).second); + ResidualEdge & e_forward = + adjacency_list.at(prev_values.at(v).first).at(prev_values.at(v).second); assert(e_forward.dst == v); ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse); prev_v = e_backward.dst; @@ -393,4 +367,4 @@ void MaximizeLinearAssignment( } #endif } -} // namespace assignment_problem +} // namespace gnn_solver diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index e456aa1220c05..94cb13f8333c7 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -12,77 +12,200 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include "object_association_merger/node.hpp" -#include -#include -#include +#include "object_association_merger/utils/utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include #include #include -// #include -#include + #define EIGEN_MPL2_ONLY #include #include +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +namespace +{ +boost::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 self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + /*target*/ target_frame_id, /*src*/ source_frame_id, time, + rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("object_association_merger"), ex.what()); + return boost::none; + } +} + +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; +} + +bool isUnknownObjectOverlapped( + const autoware_auto_perception_msgs::msg::DetectedObject & unknown_object, + const autoware_auto_perception_msgs::msg::DetectedObject & known_object, + const double precision_threshold, const double recall_threshold) +{ + constexpr double sq_distance_threshold = std::pow(5.0, 2.0); + const double sq_distance = tier4_autoware_utils::calcSquaredDistance2d( + unknown_object.kinematics.pose_with_covariance.pose, + known_object.kinematics.pose_with_covariance.pose); + if (sq_distance_threshold < sq_distance) return false; + const auto precision = utils::get2dPrecision(unknown_object, known_object); + const auto recall = utils::get2dRecall(unknown_object, known_object); + return precision > precision_threshold || recall > recall_threshold; +} +} // namespace + namespace object_association { ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptions & node_options) -: rclcpp::Node("cluster_data_association_node", node_options), +: rclcpp::Node("object_association_merger_node", node_options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), object0_sub_(this, "input/object0", rclcpp::QoS{1}.get_rmw_qos_profile()), object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()), sync_(SyncPolicy(10), object0_sub_, object1_sub_) { + // Create publishers and subscribers using std::placeholders::_1; using std::placeholders::_2; sync_.registerCallback(std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); - merged_object_pub_ = create_publisher( "output/object", rclcpp::QoS{1}); + + // Parameters + base_link_frame_id_ = declare_parameter("base_link_frame_id", "base_link"); + remove_overlapped_unknown_objects_ = + declare_parameter("remove_overlapped_unknown_objects", true); + overlapped_judge_param_.precision_threshold = + declare_parameter("precision_threshold_to_judge_overlapped", 0.4); + overlapped_judge_param_.recall_threshold = + declare_parameter("recall_threshold_to_judge_overlapped", 0.5); + + const auto tmp = this->declare_parameter>("can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); + const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); + const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); + data_association_ = std::make_unique( + can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix); } void ObjectAssociationMergerNode::objectsCallback( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object0_msg, - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object1_msg) + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects0_msg, + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects1_msg) { // Guard if (merged_object_pub_->get_subscription_count() < 1) { return; } + /* transform to base_link coordinate */ + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1; + if ( + !transformDetectedObjects( + *input_objects0_msg, base_link_frame_id_, tf_buffer_, transformed_objects0) || + !transformDetectedObjects( + *input_objects1_msg, base_link_frame_id_, tf_buffer_, transformed_objects1)) { + return; + } + // build output msg autoware_auto_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = input_object0_msg->header; + output_msg.header = input_objects0_msg->header; /* global nearest neighbor */ - std::unordered_map direct_assignment; - std::unordered_map reverse_assignment; + std::unordered_map direct_assignment, reverse_assignment; + const auto & objects0 = transformed_objects0.objects; + const auto & objects1 = transformed_objects1.objects; Eigen::MatrixXd score_matrix = - data_association_.calcScoreMatrix(*input_object1_msg, *input_object0_msg); - data_association_.assign(score_matrix, direct_assignment, reverse_assignment); - for (size_t object0_idx = 0; object0_idx < input_object0_msg->objects.size(); ++object0_idx) { - if (direct_assignment.find(object0_idx) != direct_assignment.end()) { // found - // The one with the higher score will be hired. - if ( - input_object1_msg->objects.at(direct_assignment.at(object0_idx)).existence_probability < - input_object0_msg->objects.at(object0_idx).existence_probability) { - output_msg.objects.push_back(input_object0_msg->objects.at(object0_idx)); - } else { - output_msg.objects.push_back( - input_object1_msg->objects.at(direct_assignment.at(object0_idx))); - } + data_association_->calcScoreMatrix(transformed_objects1, transformed_objects0); + data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + + for (size_t object0_idx = 0; object0_idx < objects0.size(); ++object0_idx) { + const auto & object0 = objects0.at(object0_idx); + if (direct_assignment.find(object0_idx) != direct_assignment.end()) { // found and merge + const auto & object1 = objects1.at(direct_assignment.at(object0_idx)); + if (object1.existence_probability <= object0.existence_probability) + output_msg.objects.push_back(object0); + else + output_msg.objects.push_back(object1); } else { // not found - output_msg.objects.push_back(input_object0_msg->objects.at(object0_idx)); + output_msg.objects.push_back(object0); } } - for (size_t object1_idx = 0; object1_idx < input_object1_msg->objects.size(); ++object1_idx) { + for (size_t object1_idx = 0; object1_idx < objects1.size(); ++object1_idx) { + const auto & object1 = objects1.at(object1_idx); if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found } else { // not found - output_msg.objects.push_back(input_object1_msg->objects.at(object1_idx)); + output_msg.objects.push_back(object1); + } + } + + // Remove overlapped unknown object + if (remove_overlapped_unknown_objects_) { + std::vector unknown_objects, known_objects; + unknown_objects.reserve(output_msg.objects.size()); + known_objects.reserve(output_msg.objects.size()); + for (const auto & object : output_msg.objects) { + if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + unknown_objects.push_back(object); + } else { + known_objects.push_back(object); + } + } + output_msg.objects.clear(); + output_msg.objects = known_objects; + for (const auto & unknown_object : unknown_objects) { + bool is_overlapped = false; + for (const auto & known_object : known_objects) { + if (isUnknownObjectOverlapped( + unknown_object, known_object, overlapped_judge_param_.precision_threshold, + overlapped_judge_param_.recall_threshold)) { + is_overlapped = true; + break; + } + } + if (!is_overlapped) { + output_msg.objects.push_back(unknown_object); + } } } diff --git a/perception/object_merger/src/object_association_merger/utils/utils.cpp b/perception/object_merger/src/object_association_merger/utils/utils.cpp index 80e81d581fa9d..b2d20f3cd77ab 100644 --- a/perception/object_merger/src/object_association_merger/utils/utils.cpp +++ b/perception/object_merger/src/object_association_merger/utils/utils.cpp @@ -12,10 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "object_association_merger/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; @@ -33,8 +47,8 @@ double getPolygonArea(const geometry_msgs::msg::Polygon & footprint) { double area = 0.0; - for (int i = 0; i < static_cast(footprint.points.size()); ++i) { - int j = (i + 1) % static_cast(footprint.points.size()); + 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); } @@ -51,4 +65,225 @@ 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); +} + +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; +} + +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