From ca40f14a75a71f5f504acf3bdd84da2ef377d3c4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 6 Feb 2024 18:33:18 +0900 Subject: [PATCH 1/2] fix: bicycle motion model - set minimum wheel-to-center length for stability Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 8 ++++---- .../src/tracker/model/big_vehicle_tracker.cpp | 9 ++++----- .../src/tracker/model/normal_vehicle_tracker.cpp | 8 ++++---- 3 files changed, 12 insertions(+), 13 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 0b56b11451ad3..87dfacfa850ca 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -143,8 +143,8 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.3; // 30% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center } bool BicycleTracker::predict(const rclcpp::Time & time) @@ -425,8 +425,8 @@ bool BicycleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.3; // 30% rear from the center + 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 return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 34a72437d7721..8b89b01e7904e 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,8 +158,8 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -457,9 +457,8 @@ bool BigVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center - + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index fcf72360b84c2..8a9507663d631 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,8 +158,8 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -457,8 +457,8 @@ bool NormalVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = bounding_box_.length * 0.3; // 30% front from the center - lr_ = bounding_box_.length * 0.25; // 25% rear from the center + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center return true; } From ce24c7a95fc0e584a4f9c8b1b4e84bbbeae16d2d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 6 Feb 2024 18:41:11 +0900 Subject: [PATCH 2/2] chore: align comments Signed-off-by: Taekjin LEE --- .../src/tracker/model/bicycle_tracker.cpp | 4 ++-- .../src/tracker/model/big_vehicle_tracker.cpp | 8 ++++---- .../src/tracker/model/normal_vehicle_tracker.cpp | 8 ++++---- 3 files changed, 10 insertions(+), 10 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 87dfacfa850ca..8133bc7dcf0a0 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -143,8 +143,8 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center + 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 } bool BicycleTracker::predict(const rclcpp::Time & time) 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 8b89b01e7904e..b5a730e93663a 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,8 +158,8 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center + 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 } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -457,8 +457,8 @@ bool BigVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center + 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 return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 8a9507663d631..ca0a2d39c266b 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,8 +158,8 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center + 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 } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -457,8 +457,8 @@ bool NormalVehicleTracker::measureWithShape( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center + 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 return true; }