Skip to content

Commit

Permalink
fix: hug object bug fix, limit object size
Browse files Browse the repository at this point in the history 
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
  • Loading branch information
@technolojin
technolojin committed May 31, 2024
1 parent 6b07cbf commit 730b8a2
Show file tree
Hide file tree
Showing 5 changed files with 187 additions and 73 deletions.
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
Expand Up @@ -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;

Expand All @@ -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(
Expand Down
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
Expand Up @@ -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
{
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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);
Expand Down
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
Expand Up @@ -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
{
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;

Expand All @@ -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);
Expand Down
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
Expand Up @@ -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
{
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;

Expand All @@ -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);
Expand Down
Loading

0 comments on commit 730b8a2

Please sign in to comment.