Skip to content

Commit 215ae50

Browse files
technolojinpre-commit-ci[bot]
authored and
a-maumau
committed
fix(multi_object_tracker): prevent too large object tracking (autowarefoundation#7159)
* fix: set minimum and maximum object for size update Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: bug of convertConvexHullToBoundingBox Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: return false when footprint is less than 3 points Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: size filter bug Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: try to convert polygon to bbox Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: clean-up Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: bicycle tracker to try bbox convert Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: clean-up Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent b7f1eed commit 215ae50

File tree

5 files changed

+187
-72
lines changed

5 files changed

+187
-72
lines changed

‎perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp

+23-18
Original file line numberDiff line numberDiff line change
@@ -295,40 +295,43 @@ inline void calcAnchorPointOffset(
295295
* @param input_object: input convex hull objects
296296
* @param output_object: output bounding box objects
297297
*/
298-
inline void convertConvexHullToBoundingBox(
298+
inline bool convertConvexHullToBoundingBox(
299299
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
300300
autoware_auto_perception_msgs::msg::DetectedObject & output_object)
301301
{
302-
const Eigen::Vector2d center{
303-
input_object.kinematics.pose_with_covariance.pose.position.x,
304-
input_object.kinematics.pose_with_covariance.pose.position.y};
305-
const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation);
306-
const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix();
302+
// check footprint size
303+
if (input_object.shape.footprint.points.size() < 3) {
304+
return false;
305+
}
307306

307+
// look for bounding box boundary
308308
double max_x = 0;
309309
double max_y = 0;
310310
double min_x = 0;
311311
double min_y = 0;
312312
double max_z = 0;
313-
314-
// look for bounding box boundary
315313
for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) {
316-
Eigen::Vector2d vertex{
317-
input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y};
318-
319-
const Eigen::Vector2d local_vertex = R_inv * (vertex - center);
320-
max_x = std::max(max_x, local_vertex.x());
321-
max_y = std::max(max_y, local_vertex.y());
322-
min_x = std::min(min_x, local_vertex.x());
323-
min_y = std::min(min_y, local_vertex.y());
324-
325-
max_z = std::max(max_z, static_cast<double>(input_object.shape.footprint.points.at(i).z));
314+
const double foot_x = input_object.shape.footprint.points.at(i).x;
315+
const double foot_y = input_object.shape.footprint.points.at(i).y;
316+
const double foot_z = input_object.shape.footprint.points.at(i).z;
317+
max_x = std::max(max_x, foot_x);
318+
max_y = std::max(max_y, foot_y);
319+
min_x = std::min(min_x, foot_x);
320+
min_y = std::min(min_y, foot_y);
321+
max_z = std::max(max_z, foot_z);
326322
}
327323

328324
// calc bounding box state
329325
const double length = max_x - min_x;
330326
const double width = max_y - min_y;
331327
const double height = max_z;
328+
329+
// calc new center
330+
const Eigen::Vector2d center{
331+
input_object.kinematics.pose_with_covariance.pose.position.x,
332+
input_object.kinematics.pose_with_covariance.pose.position.y};
333+
const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation);
334+
const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix();
332335
const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0};
333336
const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center;
334337

@@ -341,6 +344,8 @@ inline void convertConvexHullToBoundingBox(
341344
output_object.shape.dimensions.x = length;
342345
output_object.shape.dimensions.y = width;
343346
output_object.shape.dimensions.z = height;
347+
348+
return true;
344349
}
345350

346351
inline bool getMeasurementYaw(

‎perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp

+35-15
Original file line numberDiff line numberDiff line change
@@ -71,10 +71,12 @@ BicycleTracker::BicycleTracker(
7171
} else {
7272
bounding_box_ = {1.0, 0.5, 1.7};
7373
}
74-
// set minimum size
75-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
76-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
77-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
74+
// set maximum and minimum size
75+
constexpr double max_size = 5.0;
76+
constexpr double min_size = 0.3;
77+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
78+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
79+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
7880

7981
// Set motion model parameters
8082
{
@@ -172,7 +174,9 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb
172174
// OBJECT SHAPE MODEL
173175
// convert to bounding box if input is convex shape
174176
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
175-
utils::convertConvexHullToBoundingBox(object, updating_object);
177+
if (!utils::convertConvexHullToBoundingBox(object, updating_object)) {
178+
updating_object = object;
179+
}
176180
} else {
177181
updating_object = object;
178182
}
@@ -226,22 +230,38 @@ bool BicycleTracker::measureWithPose(
226230
bool BicycleTracker::measureWithShape(
227231
const autoware_auto_perception_msgs::msg::DetectedObject & object)
228232
{
233+
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
229234
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
230235
// do not update shape if the input is not a bounding box
231-
return true;
236+
return false;
232237
}
233238

234-
constexpr double gain = 0.1;
235-
constexpr double gain_inv = 1.0 - gain;
239+
// check bound box size abnormality
240+
constexpr double size_max = 30.0; // [m]
241+
constexpr double size_min = 0.1; // [m]
242+
if (
243+
bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max ||
244+
bbox_object.shape.dimensions.z > size_max) {
245+
return false;
246+
} else if (
247+
bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min ||
248+
bbox_object.shape.dimensions.z < size_min) {
249+
return false;
250+
}
236251

237252
// update object size
238-
bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
239-
bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
240-
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
241-
// set minimum size
242-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
243-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
244-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
253+
constexpr double gain = 0.1;
254+
constexpr double gain_inv = 1.0 - gain;
255+
bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x;
256+
bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y;
257+
bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z;
258+
259+
// set maximum and minimum size
260+
constexpr double max_size = 5.0;
261+
constexpr double min_size = 0.3;
262+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
263+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
264+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
245265

246266
// update motion model
247267
motion_model_.updateExtendedState(bounding_box_.length);

‎perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

+43-13
Original file line numberDiff line numberDiff line change
@@ -79,16 +79,24 @@ BigVehicleTracker::BigVehicleTracker(
7979
last_input_bounding_box_ = bounding_box_;
8080
} else {
8181
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
82-
utils::convertConvexHullToBoundingBox(object, bbox_object);
83-
bounding_box_ = {
84-
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
85-
bbox_object.shape.dimensions.z};
82+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
83+
RCLCPP_WARN(
84+
logger_,
85+
"BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box.");
86+
bounding_box_ = {6.0, 2.0, 2.0}; // default value
87+
} else {
88+
bounding_box_ = {
89+
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
90+
bbox_object.shape.dimensions.z};
91+
}
8692
last_input_bounding_box_ = bounding_box_;
8793
}
88-
// set minimum size
89-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
90-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
91-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
94+
// set maximum and minimum size
95+
constexpr double max_size = 30.0;
96+
constexpr double min_size = 1.0;
97+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
98+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
99+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
92100

93101
// Set motion model parameters
94102
{
@@ -195,7 +203,12 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
195203
// convert to bounding box if input is convex shape
196204
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
197205
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
198-
utils::convertConvexHullToBoundingBox(object, bbox_object);
206+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
207+
RCLCPP_WARN(
208+
logger_,
209+
"BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
210+
bbox_object = object;
211+
}
199212
} else {
200213
bbox_object = object;
201214
}
@@ -306,6 +319,20 @@ bool BigVehicleTracker::measureWithPose(
306319
bool BigVehicleTracker::measureWithShape(
307320
const autoware_auto_perception_msgs::msg::DetectedObject & object)
308321
{
322+
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
323+
// do not update shape if the input is not a bounding box
324+
return false;
325+
}
326+
327+
// check object size abnormality
328+
constexpr double size_max = 40.0; // [m]
329+
constexpr double size_min = 1.0; // [m]
330+
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
331+
return false;
332+
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
333+
return false;
334+
}
335+
309336
constexpr double gain = 0.1;
310337
constexpr double gain_inv = 1.0 - gain;
311338

@@ -315,10 +342,13 @@ bool BigVehicleTracker::measureWithShape(
315342
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
316343
last_input_bounding_box_ = {
317344
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
318-
// set minimum size
319-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
320-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
321-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
345+
346+
// set maximum and minimum size
347+
constexpr double max_size = 30.0;
348+
constexpr double min_size = 1.0;
349+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
350+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
351+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
322352

323353
// update motion model
324354
motion_model_.updateExtendedState(bounding_box_.length);

‎perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

+45-13
Original file line numberDiff line numberDiff line change
@@ -79,16 +79,25 @@ NormalVehicleTracker::NormalVehicleTracker(
7979
last_input_bounding_box_ = bounding_box_;
8080
} else {
8181
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
82-
utils::convertConvexHullToBoundingBox(object, bbox_object);
83-
bounding_box_ = {
84-
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
85-
bbox_object.shape.dimensions.z};
82+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
83+
RCLCPP_WARN(
84+
logger_,
85+
"NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding "
86+
"box.");
87+
bounding_box_ = {3.0, 2.0, 1.8}; // default value
88+
} else {
89+
bounding_box_ = {
90+
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
91+
bbox_object.shape.dimensions.z};
92+
}
8693
last_input_bounding_box_ = bounding_box_;
8794
}
88-
// set minimum size
89-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
90-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
91-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
95+
// set maximum and minimum size
96+
constexpr double max_size = 20.0;
97+
constexpr double min_size = 1.0;
98+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
99+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
100+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
92101

93102
// Set motion model parameters
94103
{
@@ -195,7 +204,13 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda
195204
// convert to bounding box if input is convex shape
196205
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
197206
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
198-
utils::convertConvexHullToBoundingBox(object, bbox_object);
207+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
208+
RCLCPP_WARN(
209+
logger_,
210+
"NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
211+
bbox_object = object;
212+
}
213+
199214
} else {
200215
bbox_object = object;
201216
}
@@ -306,6 +321,20 @@ bool NormalVehicleTracker::measureWithPose(
306321
bool NormalVehicleTracker::measureWithShape(
307322
const autoware_auto_perception_msgs::msg::DetectedObject & object)
308323
{
324+
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
325+
// do not update shape if the input is not a bounding box
326+
return false;
327+
}
328+
329+
// check object size abnormality
330+
constexpr double size_max = 30.0; // [m]
331+
constexpr double size_min = 1.0; // [m]
332+
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
333+
return false;
334+
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
335+
return false;
336+
}
337+
309338
constexpr double gain = 0.1;
310339
constexpr double gain_inv = 1.0 - gain;
311340

@@ -315,10 +344,13 @@ bool NormalVehicleTracker::measureWithShape(
315344
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
316345
last_input_bounding_box_ = {
317346
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
318-
// set minimum size
319-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
320-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
321-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
347+
348+
// set maximum and minimum size
349+
constexpr double max_size = 20.0;
350+
constexpr double min_size = 1.0;
351+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
352+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
353+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
322354

323355
// update motion model
324356
motion_model_.updateExtendedState(bounding_box_.length);

0 commit comments

Comments
 (0)