From a30cfea4b83119ccaf49967f941dc7cad90b6fcd Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 12 Mar 2024 13:48:23 +0900 Subject: [PATCH 1/3] fix: set object minimum size limits Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 10 ++++++++++ .../src/tracker/model/big_vehicle_tracker.cpp | 10 ++++++++++ .../src/tracker/model/normal_vehicle_tracker.cpp | 10 ++++++++++ .../src/tracker/model/pedestrian_tracker.cpp | 14 ++++++++++++++ 4 files changed, 44 insertions(+) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 8a3a5629ec277..ed2fc1344c09f 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -140,6 +140,11 @@ BicycleTracker::BicycleTracker( } else { bounding_box_ = {1.0, 0.5, 1.7}; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); + ekf_.init(X, P); // Set lf, lr @@ -415,6 +420,11 @@ bool BicycleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); + // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 80c0df7e5ffb1..2dc0ef57526ed 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -157,6 +157,11 @@ BigVehicleTracker::BigVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 2.5); + bounding_box_.width = std::max(bounding_box_.width, 1.0); + bounding_box_.height = std::max(bounding_box_.height, 1.0); + // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m @@ -455,6 +460,11 @@ bool BigVehicleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 2.5); + bounding_box_.width = std::max(bounding_box_.width, 1.0); + bounding_box_.height = std::max(bounding_box_.height, 1.0); + // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 7714c381894f0..9ba0036e930f8 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -157,6 +157,11 @@ NormalVehicleTracker::NormalVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 1.5); + bounding_box_.width = std::max(bounding_box_.width, 0.5); + bounding_box_.height = std::max(bounding_box_.height, 1.0); + // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m @@ -455,6 +460,11 @@ bool NormalVehicleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 1.5); + bounding_box_.width = std::max(bounding_box_.width, 0.5); + bounding_box_.height = std::max(bounding_box_.height, 1.0); + // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 7562684b84220..fea22cfd9911b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -143,6 +143,13 @@ PedestrianTracker::PedestrianTracker( cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); + cylinder_.width = std::max(cylinder_.width, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.8); + ekf_.init(X, P); } @@ -317,6 +324,13 @@ bool PedestrianTracker::measureWithShape( return false; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.8); + cylinder_.width = std::max(cylinder_.width, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.8); + return true; } From 87dbd8e560ff3d89ddab1ee89e225d488410a379 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 12 Mar 2024 13:49:02 +0900 Subject: [PATCH 2/3] fix: object shape conversion, from cylinder to bbox Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index ed2fc1344c09f..53012b454c82b 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -405,10 +405,15 @@ bool BicycleTracker::measureWithShape( { // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bbox_object = object; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // convert cylinder to bbox + bbox_object.shape.dimensions.x = object.shape.dimensions.x; + bbox_object.shape.dimensions.y = object.shape.dimensions.x; + bbox_object.shape.dimensions.z = object.shape.dimensions.z; + } else { + utils::convertConvexHullToBoundingBox(object, bbox_object); } constexpr float gain = 0.9; From 79bddcebb3cc61b9f8022e27e5b3b8a6d13d500c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 12 Mar 2024 15:30:13 +0900 Subject: [PATCH 3/3] fix: adjust minimum sizes to 0.3 m The minimum size is unified to a small number (0.3 m) to avoid side-effect of tracking. Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 8 ++++---- .../src/tracker/model/big_vehicle_tracker.cpp | 12 ++++++------ .../src/tracker/model/normal_vehicle_tracker.cpp | 12 ++++++------ .../src/tracker/model/pedestrian_tracker.cpp | 8 ++++---- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 53012b454c82b..2d732db6bb709 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -141,9 +141,9 @@ BicycleTracker::BicycleTracker( bounding_box_ = {1.0, 0.5, 1.7}; } // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); ekf_.init(X, P); @@ -426,9 +426,9 @@ bool BicycleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.5); + bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 2dc0ef57526ed..8516e9a8d02c9 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -158,9 +158,9 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 2.5); - bounding_box_.width = std::max(bounding_box_.width, 1.0); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m @@ -461,9 +461,9 @@ bool BigVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 2.5); - bounding_box_.width = std::max(bounding_box_.width, 1.0); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 9ba0036e930f8..18f73c8610f04 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -158,9 +158,9 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 1.5); - bounding_box_.width = std::max(bounding_box_.width, 0.5); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m @@ -461,9 +461,9 @@ bool NormalVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 1.5); - bounding_box_.width = std::max(bounding_box_.width, 0.5); - bounding_box_.height = std::max(bounding_box_.height, 1.0); + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index fea22cfd9911b..b5b3bd5826e69 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -146,9 +146,9 @@ PedestrianTracker::PedestrianTracker( // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.8); + cylinder_.height = std::max(cylinder_.height, 0.3); ekf_.init(X, P); } @@ -327,9 +327,9 @@ bool PedestrianTracker::measureWithShape( // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.8); + bounding_box_.height = std::max(bounding_box_.height, 0.3); cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.8); + cylinder_.height = std::max(cylinder_.height, 0.3); return true; }