From e079e343b830e3e3dc522ba6f967cb669577bf13 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Sun, 17 Jan 2021 14:04:32 +0900 Subject: [PATCH] Ros2 v0.8.0 multi object tracker (#259) * restore file name for v0.8.0 update Signed-off-by: wep21 * fix typos in perception (#862) * Feature/camera lidar perception (#937) * add object splitter Signed-off-by: Yukihiro Saito * add object merger Signed-off-by: Yukihiro Saito * change pkg name Signed-off-by: Yukihiro Saito * cosmetic change Signed-off-by: Yukihiro Saito * add comment Signed-off-by: Yukihiro Saito * remove litter Signed-off-by: Yukihiro Saito * bug fix : debug code Signed-off-by: Yukihiro Saito * enable vehicle to unknown track Signed-off-by: Yukihiro Saito * bug fix * add object position in clustering * :put_litter_in_its_place: * change param * fix name * bug fix * add install * add delay compensation param (#1035) * fix tracking bug and change tracking param (#1036) * fix bug * cut stop noise velocity * update param * support unknown labeled object tracking (#1017) * add data association matrix param * fix typo * apply clang-format-6.0 * set pedestrian model when label is unknown Co-authored-by: Yukihiro Saito * Add missing install of config directory (#1045) Signed-off-by: Kenji Miyake * Revert "restore file name for v0.8.0 update" This reverts commit 5fdf5b179f2395f01672c976920da1f8de8cc33a. * Fix typo Signed-off-by: wep21 * Fix parameter type Signed-off-by: wep21 * Fix missing arg Signed-off-by: wep21 Co-authored-by: Kazuki Miyahara Co-authored-by: Yukihiro Saito Co-authored-by: Taichi Higashide Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .../multi_object_tracker/CMakeLists.txt | 7 +- .../config/data_assosiaction_matrix.yaml | 42 +++ .../data_association/data_association.hpp | 6 +- .../multi_object_tracker_core.hpp | 3 +- .../launch/multi_object_tracker.launch.xml | 2 + .../src/data_association/data_association.cpp | 269 ++---------------- .../successive_shortest_path.cpp | 2 +- .../src/multi_object_tracker_core.cpp | 92 ++++-- .../src/tracker/model/bicycle_tracker.cpp | 3 +- .../src/tracker/model/pedestrian_tracker.cpp | 5 +- .../src/tracker/model/vehicle_tracker.cpp | 15 +- 11 files changed, 164 insertions(+), 282 deletions(-) create mode 100644 perception/object_recognition/tracking/multi_object_tracker/config/data_assosiaction_matrix.yaml diff --git a/perception/object_recognition/tracking/multi_object_tracker/CMakeLists.txt b/perception/object_recognition/tracking/multi_object_tracker/CMakeLists.txt index 97b9d4d0c8945..d2bb3f2cd655e 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/CMakeLists.txt +++ b/perception/object_recognition/tracking/multi_object_tracker/CMakeLists.txt @@ -45,8 +45,8 @@ set(MULTI_OBJECT_TRACKER_HEADERS include/multi_object_tracker/multi_object_tracker_core.hpp ) -ament_auto_add_library(multi_object_tracker_node SHARED - ${MULTI_OBJECT_TRACKER_SRC} +ament_auto_add_library(multi_object_tracker_node SHARED + ${MULTI_OBJECT_TRACKER_SRC} ${MULTI_OBJECT_TRACKER_HEADERS} ) @@ -64,4 +64,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch -) + config +) diff --git a/perception/object_recognition/tracking/multi_object_tracker/config/data_assosiaction_matrix.yaml b/perception/object_recognition/tracking/multi_object_tracker/config/data_assosiaction_matrix.yaml new file mode 100644 index 0000000000000..d738ae36a737c --- /dev/null +++ b/perception/object_recognition/tracking/multi_object_tracker/config/data_assosiaction_matrix.yaml @@ -0,0 +1,42 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 1, 1, 1, 1, 0, 0, 0, 0, #CAR + 1, 1, 1, 1, 0, 0, 0, 0, #TRUCK + 1, 1, 1, 1, 0, 0, 0, 0, #BUS + 0, 0, 0, 0, 1, 1, 0, 0, #BICYCLE + 0, 0, 0, 0, 1, 1, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 1, 0, #PEDESTRIAN + 0, 0, 0, 0, 0, 0, 0, 1] #ANIMAL + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + 4.0, 4.5, 4.5, 4.5, 1.0, 1.0, 1.0, 1.0, #CAR + 4.0, 4.0, 4.0, 4.0, 1.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 4.0, 4.0, 4.0, 1.0, 1.0, 1.0, 1.0, #BUS + 3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 1.0, 1.0, #BICYCLE + 3.0, 1.0, 1.0, 1.0, 3.0, 2.0, 1.0, 1.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 2.0, 1.0, #PEDESTRIAN + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #ANIMAL + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN + 12.10, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #CAR + 19.75, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #TRUCK + 32.40, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #BUS + 2.50, 10000.00, 10000.00, 10000.00, 2.50, 3.00, 10000.00, 10000.00, #BICYCLE + 3.00, 10000.00, 10000.00, 10000.00, 2.50, 3.00, 10000.00, 10000.00, #MOTORBIKE + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 2.00, 10000.00, #PEDESTRIAN + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00] #ANIMAL + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 3.600, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #CAR + 6.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #TRUCK + 10.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #BUS + 0.001, 0.000, 0.000, 0.000, 0.001, 0.001, 0.000, 0.000, #BICYCLE + 0.001, 0.000, 0.000, 0.000, 0.001, 0.001, 0.000, 0.000, #MOTORBIKE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.000, 0.001, 0.000, #PEDESTRIAN + 0.500, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #ANIMAL diff --git a/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index f725ecdfb9ce3..39401726876d2 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp @@ -35,14 +35,16 @@ class DataAssociation private: double getDistance( const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker); - Eigen::MatrixXi can_assgin_matrix_; + Eigen::MatrixXi can_assign_matrix_; Eigen::MatrixXd max_dist_matrix_; Eigen::MatrixXd max_area_matrix_; Eigen::MatrixXd min_area_matrix_; const double score_threshold_; public: - DataAssociation(); + DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_area_vector, std::vector min_area_vector); void assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); diff --git a/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 3aa69c8a0d3ae..d2adb39c25eb7 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/object_recognition/tracking/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -58,7 +58,8 @@ class MultiObjectTracker : public rclcpp::Node std::string world_frame_id_; // tracking frame std::list> list_tracker_; - DataAssociation data_association_; + std::unique_ptr data_association_; + bool enable_delay_compensation_; }; #endif diff --git a/perception/object_recognition/tracking/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/object_recognition/tracking/multi_object_tracker/launch/multi_object_tracker.launch.xml index bb8644c16eadd..495e06dbe3e37 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/object_recognition/tracking/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -5,11 +5,13 @@ + + diff --git a/perception/object_recognition/tracking/multi_object_tracker/src/data_association/data_association.cpp b/perception/object_recognition/tracking/multi_object_tracker/src/data_association/data_association.cpp index 3ff2be2941b26..88e2f0753230f 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/object_recognition/tracking/multi_object_tracker/src/data_association/data_association.cpp @@ -21,248 +21,35 @@ #include "successive_shortest_path/successive_shortest_path.hpp" #include "multi_object_tracker/utils/utils.hpp" -DataAssociation::DataAssociation() +DataAssociation::DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_area_vector, std::vector min_area_vector) : score_threshold_(0.1) { - can_assgin_matrix_ = Eigen::MatrixXi::Identity(20, 20); - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::UNKNOWN, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) = - 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) = 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) = - 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) = - 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) = 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) = - 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::BICYCLE) = 1; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::PEDESTRIAN, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - can_assgin_matrix_( - autoware_perception_msgs::msg::Semantic::ANIMAL, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; - max_dist_matrix_ = Eigen::MatrixXd::Constant(20, 20, 1.0); - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::CAR) = - 4.5; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) = - 4.5; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) = - 4.5; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) = - 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, - autoware_perception_msgs::msg::Semantic::TRUCK) = 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) = - 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) = - 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) = - 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::BUS) = - 4.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::BICYCLE) = 3.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::BICYCLE) = 3.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 2.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::PEDESTRIAN, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::PEDESTRIAN, - autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 2.0; - max_dist_matrix_( - autoware_perception_msgs::msg::Semantic::ANIMAL, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.0; - max_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* large number */ 10000.0); - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.2 * 5.5; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::CAR) = - 2.2 * 5.5; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) = - 2.5 * 7.9; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) = - 2.7 * 12.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.5 * 7.9; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) = - 2.2 * 5.5; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, - autoware_perception_msgs::msg::Semantic::TRUCK) = 2.5 * 7.9; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) = - 2.7 * 12.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.7 * 12.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) = - 2.2 * 5.5; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) = - 2.5 * 7.9; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::BUS) = - 2.7 * 12.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.5; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.5; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.5; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::PEDESTRIAN, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::PEDESTRIAN, - autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 2.0; - max_area_matrix_( - autoware_perception_msgs::msg::Semantic::ANIMAL, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; - min_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* small number */ 0.0); - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.2 * 3.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::CAR) = - 1.2 * 3.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) = - 1.5 * 4.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) = - 2.0 * 5.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.5 * 4.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) = - 1.2 * 3.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, - autoware_perception_msgs::msg::Semantic::TRUCK) = 1.5 * 4.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) = - 2.0 * 5.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0 * 5.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) = - 1.2 * 3.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) = - 1.5 * 4.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::BUS) = - 2.0 * 5.0; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::BICYCLE) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::BICYCLE, - autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::BICYCLE) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::MOTORBIKE, - autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::PEDESTRIAN, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::PEDESTRIAN, - autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 0.001; - min_area_matrix_( - autoware_perception_msgs::msg::Semantic::ANIMAL, - autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.5; + { + 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_area_label_num = static_cast(std::sqrt(max_area_vector.size())); + Eigen::Map max_area_matrix_tmp( + max_area_vector.data(), max_area_label_num, max_area_label_num); + max_area_matrix_ = max_area_matrix_tmp.transpose(); + } + { + const int min_area_label_num = static_cast(std::sqrt(min_area_vector.size())); + Eigen::Map min_area_matrix_tmp( + min_area_vector.data(), min_area_label_num, min_area_label_num); + min_area_matrix_ = min_area_matrix_tmp.transpose(); + } } void DataAssociation::assign( @@ -311,7 +98,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( ++measurement_idx) { double score = 0.0; - if (can_assgin_matrix_( + if (can_assign_matrix_( (*tracker_itr)->getType(), measurements.feature_objects.at(measurement_idx).object.semantic.type)) { diff --git a/perception/object_recognition/tracking/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/object_recognition/tracking/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp index 6511d8bbedc31..2c031941c3f1b 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/object_recognition/tracking/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -28,7 +28,7 @@ namespace assignment_problem { struct ResidualEdge { - // Desternation node + // Destination node const int dst; int capacity; const double cost; diff --git a/perception/object_recognition/tracking/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/object_recognition/tracking/multi_object_tracker/src/multi_object_tracker_core.cpp index 8b345e9883c2d..5e7645db31157 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/object_recognition/tracking/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -46,6 +46,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Parameters double publish_rate = declare_parameter("publish_rate", 30.0); world_frame_id_ = declare_parameter("world_frame_id", "world"); + enable_delay_compensation_ = declare_parameter("enable_delay_compensation", false); auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); @@ -60,6 +61,22 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) this->get_clock(), period, std::move(timer_callback), this->get_node_base_interface()->get_context()); this->get_node_timers_interface()->add_timer(publish_timer_, nullptr); + + this->declare_parameter("can_assign_matrix"); + this->declare_parameter("max_dist_matrix"); + this->declare_parameter("max_area_matrix"); + this->declare_parameter("min_area_matrix"); + auto can_assign_matrix_tmp = + this->get_parameter("can_assign_matrix").as_integer_array(); + std::vector can_assign_matrix(can_assign_matrix_tmp.begin(), can_assign_matrix_tmp.end()); + std::vector max_dist_matrix = + this->get_parameter("max_dist_matrix").as_double_array(); + std::vector max_area_matrix = + this->get_parameter("max_area_matrix").as_double_array(); + std::vector min_area_matrix = + this->get_parameter("max_area_matrix").as_double_array(); + data_association_ = std::make_unique( + can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix); } void MultiObjectTracker::measurementCallback( @@ -106,14 +123,20 @@ void MultiObjectTracker::measurementCallback( } /* life cycle check */ - // TODO + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + if (1.0 < (*itr)->getElapsedTimeFromLastUpdate(rclcpp::Node::now())) { + auto erase_itr = itr; + --itr; + list_tracker_.erase(erase_itr); + } + } - /* global nearest neighboor */ + /* global nearest neighbor */ std::unordered_map direct_assignment; std::unordered_map reverse_assignment; - Eigen::MatrixXd score_matrix = data_association_.calcScoreMatrix( + Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( input_transformed_objects, list_tracker_); // row : tracker, col : measurement - data_association_.assign(score_matrix, direct_assignment, reverse_assignment); + data_association_->assign(score_matrix, direct_assignment, reverse_assignment); /* tracker measurement update */ int tracker_idx = 0; @@ -165,8 +188,25 @@ void MultiObjectTracker::measurementCallback( std::make_shared( measurement_time, input_transformed_objects.feature_objects.at(i).object)); } else { - // list_tracker_.push_back(std::make_shared(input_transformed_objects.feature_objects.at(i).object)); + list_tracker_.push_back(std::make_shared( + measurement_time, input_transformed_objects.feature_objects.at(i).object)); + } + } + + if (!enable_delay_compensation_) { + // Create output msg + autoware_perception_msgs::msg::DynamicObjectArray output_msg; + output_msg.header.frame_id = world_frame_id_; + output_msg.header.stamp = measurement_time; + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + if ((*itr)->getTotalMeasurementCount() < 3) continue; + autoware_perception_msgs::msg::DynamicObject object; + (*itr)->getEstimatedDynamicObject(measurement_time, object); + output_msg.objects.push_back(object); } + + // Publish + dynamic_object_pub_->publish(output_msg); } } @@ -175,29 +215,31 @@ void MultiObjectTracker::publishTimerCallback() // Guard if (dynamic_object_pub_->get_subscription_count() < 1) {return;} - /* life cycle check */ - for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - if (1.0 < (*itr)->getElapsedTimeFromLastUpdate(rclcpp::Node::now())) { - auto erase_itr = itr; - --itr; - list_tracker_.erase(erase_itr); + if (enable_delay_compensation_) { + /* life cycle check */ + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + if (1.0 < (*itr)->getElapsedTimeFromLastUpdate(rclcpp::Node::now())) { + auto erase_itr = itr; + --itr; + list_tracker_.erase(erase_itr); + } } - } - // Create output msg - rclcpp::Time current_time = rclcpp::Node::now(); - autoware_perception_msgs::msg::DynamicObjectArray output_msg; - output_msg.header.frame_id = world_frame_id_; - output_msg.header.stamp = current_time; - for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - if ((*itr)->getTotalMeasurementCount() < 3) {continue;} - autoware_perception_msgs::msg::DynamicObject object; - (*itr)->getEstimatedDynamicObject(current_time, object); - output_msg.objects.push_back(object); - } + // Create output msg + rclcpp::Time current_time = rclcpp::Node::now(); + autoware_perception_msgs::msg::DynamicObjectArray output_msg; + output_msg.header.frame_id = world_frame_id_; + output_msg.header.stamp = current_time; + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + if ((*itr)->getTotalMeasurementCount() < 3) {continue;} + autoware_perception_msgs::msg::DynamicObject object; + (*itr)->getEstimatedDynamicObject(current_time, object); + output_msg.objects.push_back(object); + } - // Publish - dynamic_object_pub_->publish(output_msg); + // Publish + dynamic_object_pub_->publish(output_msg); + } } RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) diff --git a/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 91aa7936c0c33..7a4e2d946df56 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -84,8 +84,7 @@ bool BicycleTracker::measure( (object.state.pose_covariance.pose.position.y - last_measurement_posy_) * (object.state.pose_covariance.pose.position.y - last_measurement_posy_)); const double max_vel = 15.0; /* [m/s]*/ - const double vel_scale = - current_vel < 0.01 ? 1.0 : std::min(max_vel, current_vel) / current_vel; + const double vel_scale = max_vel < current_vel ? max_vel / current_vel : 1.0; if (is_changed_unknown_object) { filtered_vx_ = 0.9 * filtered_vx_ + diff --git a/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index a6fa4b6ae1df0..e5bf92c8b774e 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -83,9 +83,8 @@ bool PedestrianTracker::measure( (object.state.pose_covariance.pose.position.x - last_measurement_posx_) + (object.state.pose_covariance.pose.position.y - last_measurement_posy_) * (object.state.pose_covariance.pose.position.y - last_measurement_posy_)); - const double max_vel = 15.0; /* [m/s]*/ - const double vel_scale = - current_vel < 0.01 ? 1.0 : std::min(max_vel, current_vel) / current_vel; + const double max_vel = 4.0; /* [m/s]*/ + const double vel_scale = max_vel < current_vel ? max_vel / current_vel : 1.0; if (is_changed_unknown_object) { filtered_vx_ = 0.9 * filtered_vx_ + diff --git a/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/vehicle_tracker.cpp b/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/vehicle_tracker.cpp index 17ff9d2d1180d..c11f4d1be387b 100644 --- a/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/vehicle_tracker.cpp +++ b/perception/object_recognition/tracking/multi_object_tracker/src/tracker/model/vehicle_tracker.cpp @@ -41,7 +41,7 @@ VehicleTracker::VehicleTracker( pos_filter_gain_(0.2), filtered_vx_(0.0), filtered_vy_(0.0), - v_filter_gain_(0.7), + v_filter_gain_(0.4), area_filter_gain_(0.8), last_measurement_posx_(object.state.pose_covariance.pose.position.x), last_measurement_posy_(object.state.pose_covariance.pose.position.y), @@ -165,9 +165,7 @@ bool VehicleTracker::measure( (object.state.pose_covariance.pose.position.y - last_measurement_posy_) * (object.state.pose_covariance.pose.position.y - last_measurement_posy_)); const double max_vel = 20.0; /* [m/s]*/ - const double vel_scale = - current_vel < 0.01 ? 1.0 : std::min(max_vel, current_vel) / current_vel; - + const double vel_scale = max_vel < current_vel ? max_vel / current_vel : 1.0; if (is_changed_unknown_object) { filtered_vx_ = 0.95 * filtered_vx_ + @@ -266,6 +264,15 @@ bool VehicleTracker::getEstimatedDynamicObject( filtered_vx_ * std::sin(-yaw) + filtered_vy_ * std::cos(-yaw); } + // cut microspeed + constexpr float min_velocity = 0.56; // 2.0 kmph + if (std::fabs(object.state.twist_covariance.twist.linear.x) < min_velocity) { + object.state.twist_covariance.twist.linear.x = 0.0; + } + if (std::fabs(object.state.twist_covariance.twist.linear.y) < min_velocity) { + object.state.twist_covariance.twist.linear.y = 0.0; + } + object.state.twist_reliable = true; return true;