Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(multi_object_tracker): huge tracking object bug fix for x2 #1314

Merged
merged 3 commits into from
May 31, 2024
Merged

fix(multi_object_tracker): huge tracking object bug fix for x2 #1314

Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6b07cbf
fix: catch-up changes
technolojin May 31, 2024
730b8a2
fix: hug object bug fix, limit object size
technolojin May 31, 2024
2465cab
fix: node config fix
technolojin May 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
fix: hug object bug fix, limit object size 
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
  • Loading branch information
@technolojin
technolojin committed May 31, 2024
commit 730b8a2ff0a58170a1692617ad48a2931a442e50
41 changes: 23 additions & 18 deletions perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
@@ -295,40 +295,43 @@ inline void calcAnchorPointOffset(
* @param input_object: input convex hull objects
* @param output_object: output bounding box objects
*/
inline void convertConvexHullToBoundingBox(
inline bool convertConvexHullToBoundingBox(
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
autoware_auto_perception_msgs::msg::DetectedObject & output_object)
{
const Eigen::Vector2d center{
input_object.kinematics.pose_with_covariance.pose.position.x,
input_object.kinematics.pose_with_covariance.pose.position.y};
const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation);
const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix();
// check footprint size
if (input_object.shape.footprint.points.size() < 3) {
return false;
}

// look for bounding box boundary
double max_x = 0;
double max_y = 0;
double min_x = 0;
double min_y = 0;
double max_z = 0;

// look for bounding box boundary
for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) {
Eigen::Vector2d vertex{
input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y};

const Eigen::Vector2d local_vertex = R_inv * (vertex - center);
max_x = std::max(max_x, local_vertex.x());
max_y = std::max(max_y, local_vertex.y());
min_x = std::min(min_x, local_vertex.x());
min_y = std::min(min_y, local_vertex.y());

max_z = std::max(max_z, static_cast<double>(input_object.shape.footprint.points.at(i).z));
const double foot_x = input_object.shape.footprint.points.at(i).x;
const double foot_y = input_object.shape.footprint.points.at(i).y;
const double foot_z = input_object.shape.footprint.points.at(i).z;
max_x = std::max(max_x, foot_x);
max_y = std::max(max_y, foot_y);
min_x = std::min(min_x, foot_x);
min_y = std::min(min_y, foot_y);
max_z = std::max(max_z, foot_z);
}

// calc bounding box state
const double length = max_x - min_x;
const double width = max_y - min_y;
const double height = max_z;

// calc new center
const Eigen::Vector2d center{
input_object.kinematics.pose_with_covariance.pose.position.x,
input_object.kinematics.pose_with_covariance.pose.position.y};
const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation);
const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix();
const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0};
const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center;

@@ -341,6 +344,8 @@ inline void convertConvexHullToBoundingBox(
output_object.shape.dimensions.x = length;
output_object.shape.dimensions.y = width;
output_object.shape.dimensions.z = height;

return true;
}

inline bool getMeasurementYaw(
50 changes: 35 additions & 15 deletions perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
View file
Original file line number Diff line number Diff line change
@@ -67,10 +67,12 @@ BicycleTracker::BicycleTracker(
} else {
bounding_box_ = {1.0, 0.5, 1.7};
}
// 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.3);
// set maximum and minimum size
constexpr double max_size = 5.0;
constexpr double min_size = 0.3;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

// Set motion model parameters
{
@@ -168,7 +170,9 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb
// OBJECT SHAPE MODEL
// convert to bounding box if input is convex shape
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
utils::convertConvexHullToBoundingBox(object, updating_object);
if (!utils::convertConvexHullToBoundingBox(object, updating_object)) {
updating_object = object;
}
} else {
updating_object = object;
}
@@ -222,22 +226,38 @@ bool BicycleTracker::measureWithPose(
bool BicycleTracker::measureWithShape(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
// do not update shape if the input is not a bounding box
return true;
return false;
}

constexpr double gain = 0.1;
constexpr double gain_inv = 1.0 - gain;
// check bound box size abnormality
constexpr double size_max = 30.0; // [m]
constexpr double size_min = 0.1; // [m]
if (
bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max ||
bbox_object.shape.dimensions.z > size_max) {
return false;
} else if (
bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min ||
bbox_object.shape.dimensions.z < size_min) {
return false;
}

// update object size
bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
bounding_box_.height = gain_inv * bounding_box_.height + gain * 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.3);
constexpr double gain = 0.1;
constexpr double gain_inv = 1.0 - gain;
bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x;
bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y;
bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z;

// set maximum and minimum size
constexpr double max_size = 5.0;
constexpr double min_size = 0.3;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

// update motion model
motion_model_.updateExtendedState(bounding_box_.length);
56 changes: 43 additions & 13 deletions perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
View file
Original file line number Diff line number Diff line change
@@ -75,16 +75,24 @@ BigVehicleTracker::BigVehicleTracker(
last_input_bounding_box_ = bounding_box_;
} else {
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
utils::convertConvexHullToBoundingBox(object, bbox_object);
bounding_box_ = {
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
bbox_object.shape.dimensions.z};
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
RCLCPP_WARN(
logger_,
"BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box.");
bounding_box_ = {6.0, 2.0, 2.0}; // default value
} else {
bounding_box_ = {
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
bbox_object.shape.dimensions.z};
}
last_input_bounding_box_ = bounding_box_;
}
// 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.3);
// set maximum and minimum size
constexpr double max_size = 30.0;
constexpr double min_size = 1.0;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

// Set motion model parameters
{
@@ -191,7 +199,12 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
// convert to bounding box if input is convex shape
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);
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
RCLCPP_WARN(
logger_,
"BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
bbox_object = object;
}
} else {
bbox_object = object;
}
@@ -302,6 +315,20 @@ bool BigVehicleTracker::measureWithPose(
bool BigVehicleTracker::measureWithShape(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
// do not update shape if the input is not a bounding box
return false;
}

// check object size abnormality
constexpr double size_max = 40.0; // [m]
constexpr double size_min = 1.0; // [m]
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
return false;
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
return false;
}

constexpr double gain = 0.1;
constexpr double gain_inv = 1.0 - gain;

@@ -311,10 +338,13 @@ bool BigVehicleTracker::measureWithShape(
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
last_input_bounding_box_ = {
object.shape.dimensions.x, object.shape.dimensions.y, 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.3);

// set maximum and minimum size
constexpr double max_size = 30.0;
constexpr double min_size = 1.0;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

// update motion model
motion_model_.updateExtendedState(bounding_box_.length);
58 changes: 45 additions & 13 deletions perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
View file
Original file line number Diff line number Diff line change
@@ -75,16 +75,25 @@ NormalVehicleTracker::NormalVehicleTracker(
last_input_bounding_box_ = bounding_box_;
} else {
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
utils::convertConvexHullToBoundingBox(object, bbox_object);
bounding_box_ = {
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
bbox_object.shape.dimensions.z};
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
RCLCPP_WARN(
logger_,
"NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding "
"box.");
bounding_box_ = {3.0, 2.0, 1.8}; // default value
} else {
bounding_box_ = {
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
bbox_object.shape.dimensions.z};
}
last_input_bounding_box_ = bounding_box_;
}
// 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.3);
// set maximum and minimum size
constexpr double max_size = 20.0;
constexpr double min_size = 1.0;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

// Set motion model parameters
{
@@ -191,7 +200,13 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda
// convert to bounding box if input is convex shape
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);
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
RCLCPP_WARN(
logger_,
"NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
bbox_object = object;
}

} else {
bbox_object = object;
}
@@ -302,6 +317,20 @@ bool NormalVehicleTracker::measureWithPose(
bool NormalVehicleTracker::measureWithShape(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
// do not update shape if the input is not a bounding box
return false;
}

// check object size abnormality
constexpr double size_max = 30.0; // [m]
constexpr double size_min = 1.0; // [m]
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
return false;
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
return false;
}

constexpr double gain = 0.1;
constexpr double gain_inv = 1.0 - gain;

@@ -311,10 +340,13 @@ bool NormalVehicleTracker::measureWithShape(
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
last_input_bounding_box_ = {
object.shape.dimensions.x, object.shape.dimensions.y, 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.3);

// set maximum and minimum size
constexpr double max_size = 20.0;
constexpr double min_size = 1.0;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

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