Skip to content

Commit 730b8a2

Browse files
technolojintechnolojin
committed
fix: hug object bug fix, limit object size
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 6b07cbf commit 730b8a2

File tree

5 files changed

+187
-73
lines changed

5 files changed

+187
-73
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
@@ -67,10 +67,12 @@ BicycleTracker::BicycleTracker(
6767
} else {
6868
bounding_box_ = {1.0, 0.5, 1.7};
6969
}
70-
// set minimum size
71-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
72-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
73-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
70+
// set maximum and minimum size
71+
constexpr double max_size = 5.0;
72+
constexpr double min_size = 0.3;
73+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
74+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
75+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
7476

7577
// Set motion model parameters
7678
{
@@ -168,7 +170,9 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb
168170
// OBJECT SHAPE MODEL
169171
// convert to bounding box if input is convex shape
170172
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
171-
utils::convertConvexHullToBoundingBox(object, updating_object);
173+
if (!utils::convertConvexHullToBoundingBox(object, updating_object)) {
174+
updating_object = object;
175+
}
172176
} else {
173177
updating_object = object;
174178
}
@@ -222,22 +226,38 @@ bool BicycleTracker::measureWithPose(
222226
bool BicycleTracker::measureWithShape(
223227
const autoware_auto_perception_msgs::msg::DetectedObject & object)
224228
{
229+
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
225230
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
226231
// do not update shape if the input is not a bounding box
227-
return true;
232+
return false;
228233
}
229234

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

233248
// update object size
234-
bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
235-
bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
236-
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
237-
// set minimum size
238-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
239-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
240-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
249+
constexpr double gain = 0.1;
250+
constexpr double gain_inv = 1.0 - gain;
251+
bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x;
252+
bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y;
253+
bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z;
254+
255+
// set maximum and minimum size
256+
constexpr double max_size = 5.0;
257+
constexpr double min_size = 0.3;
258+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
259+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
260+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
241261

242262
// update motion model
243263
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
@@ -75,16 +75,24 @@ BigVehicleTracker::BigVehicleTracker(
7575
last_input_bounding_box_ = bounding_box_;
7676
} else {
7777
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
78-
utils::convertConvexHullToBoundingBox(object, bbox_object);
79-
bounding_box_ = {
80-
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
81-
bbox_object.shape.dimensions.z};
78+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
79+
RCLCPP_WARN(
80+
logger_,
81+
"BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box.");
82+
bounding_box_ = {6.0, 2.0, 2.0}; // default value
83+
} else {
84+
bounding_box_ = {
85+
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
86+
bbox_object.shape.dimensions.z};
87+
}
8288
last_input_bounding_box_ = bounding_box_;
8389
}
84-
// set minimum size
85-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
86-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
87-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
90+
// set maximum and minimum size
91+
constexpr double max_size = 30.0;
92+
constexpr double min_size = 1.0;
93+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
94+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
95+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
8896

8997
// Set motion model parameters
9098
{
@@ -191,7 +199,12 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
191199
// convert to bounding box if input is convex shape
192200
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
193201
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
194-
utils::convertConvexHullToBoundingBox(object, bbox_object);
202+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
203+
RCLCPP_WARN(
204+
logger_,
205+
"BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
206+
bbox_object = object;
207+
}
195208
} else {
196209
bbox_object = object;
197210
}
@@ -302,6 +315,20 @@ bool BigVehicleTracker::measureWithPose(
302315
bool BigVehicleTracker::measureWithShape(
303316
const autoware_auto_perception_msgs::msg::DetectedObject & object)
304317
{
318+
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
319+
// do not update shape if the input is not a bounding box
320+
return false;
321+
}
322+
323+
// check object size abnormality
324+
constexpr double size_max = 40.0; // [m]
325+
constexpr double size_min = 1.0; // [m]
326+
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
327+
return false;
328+
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
329+
return false;
330+
}
331+
305332
constexpr double gain = 0.1;
306333
constexpr double gain_inv = 1.0 - gain;
307334

@@ -311,10 +338,13 @@ bool BigVehicleTracker::measureWithShape(
311338
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
312339
last_input_bounding_box_ = {
313340
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
314-
// set minimum size
315-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
316-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
317-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
341+
342+
// set maximum and minimum size
343+
constexpr double max_size = 30.0;
344+
constexpr double min_size = 1.0;
345+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
346+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
347+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
318348

319349
// update motion model
320350
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
@@ -75,16 +75,25 @@ NormalVehicleTracker::NormalVehicleTracker(
7575
last_input_bounding_box_ = bounding_box_;
7676
} else {
7777
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
78-
utils::convertConvexHullToBoundingBox(object, bbox_object);
79-
bounding_box_ = {
80-
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
81-
bbox_object.shape.dimensions.z};
78+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
79+
RCLCPP_WARN(
80+
logger_,
81+
"NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding "
82+
"box.");
83+
bounding_box_ = {3.0, 2.0, 1.8}; // default value
84+
} else {
85+
bounding_box_ = {
86+
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
87+
bbox_object.shape.dimensions.z};
88+
}
8289
last_input_bounding_box_ = bounding_box_;
8390
}
84-
// set minimum size
85-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
86-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
87-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
91+
// set maximum and minimum size
92+
constexpr double max_size = 20.0;
93+
constexpr double min_size = 1.0;
94+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
95+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
96+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
8897

8998
// Set motion model parameters
9099
{
@@ -191,7 +200,13 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda
191200
// convert to bounding box if input is convex shape
192201
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
193202
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
194-
utils::convertConvexHullToBoundingBox(object, bbox_object);
203+
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
204+
RCLCPP_WARN(
205+
logger_,
206+
"NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
207+
bbox_object = object;
208+
}
209+
195210
} else {
196211
bbox_object = object;
197212
}
@@ -302,6 +317,20 @@ bool NormalVehicleTracker::measureWithPose(
302317
bool NormalVehicleTracker::measureWithShape(
303318
const autoware_auto_perception_msgs::msg::DetectedObject & object)
304319
{
320+
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
321+
// do not update shape if the input is not a bounding box
322+
return false;
323+
}
324+
325+
// check object size abnormality
326+
constexpr double size_max = 30.0; // [m]
327+
constexpr double size_min = 1.0; // [m]
328+
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
329+
return false;
330+
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
331+
return false;
332+
}
333+
305334
constexpr double gain = 0.1;
306335
constexpr double gain_inv = 1.0 - gain;
307336

@@ -311,10 +340,13 @@ bool NormalVehicleTracker::measureWithShape(
311340
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
312341
last_input_bounding_box_ = {
313342
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
314-
// set minimum size
315-
bounding_box_.length = std::max(bounding_box_.length, 0.3);
316-
bounding_box_.width = std::max(bounding_box_.width, 0.3);
317-
bounding_box_.height = std::max(bounding_box_.height, 0.3);
343+
344+
// set maximum and minimum size
345+
constexpr double max_size = 20.0;
346+
constexpr double min_size = 1.0;
347+
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
348+
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
349+
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
318350

319351
// update motion model
320352
motion_model_.updateExtendedState(bounding_box_.length);

0 commit comments

Comments
 (0)