Skip to content

Commit

Permalink
fix: bicycle size update bug fix
Browse files Browse the repository at this point in the history
  • Loading branch information
@technolojin
technolojin committed Jun 5, 2024
1 parent 1fafa9d commit e588c07
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 50 deletions.
33 changes: 15 additions & 18 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 @@ -58,12 +58,12 @@ BicycleTracker::BicycleTracker(

// Initialize parameters
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
double r_stddev_x = 0.5; // in vehicle coordinate [m]
double r_stddev_y = 0.4; // in vehicle coordinate [m]
double r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad]
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
const float r_stddev_x = 0.5; // in vehicle coordinate [m]
const float r_stddev_y = 0.4; // in vehicle coordinate [m]
const float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad]
ekf_params_.r_cov_x = r_stddev_x * r_stddev_x;
ekf_params_.r_cov_y = r_stddev_y * r_stddev_y;
ekf_params_.r_cov_yaw = r_stddev_yaw * r_stddev_yaw;

// OBJECT SHAPE MODEL
if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
Expand Down Expand Up @@ -169,16 +169,14 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & /*self_transform*/) const
{
autoware_auto_perception_msgs::msg::DetectedObject updating_object;
autoware_auto_perception_msgs::msg::DetectedObject updating_object = object;

// OBJECT SHAPE MODEL
// convert to bounding box if input is convex shape
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
if (!utils::convertConvexHullToBoundingBox(object, updating_object)) {
updating_object = object;
}
} else {
updating_object = object;
}

// UNCERTAINTY MODEL
Expand Down Expand Up @@ -231,8 +229,7 @@ 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) {
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;
}
Expand All @@ -241,21 +238,21 @@ bool BicycleTracker::measureWithShape(
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) {
object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max ||
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) {
object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min ||
object.shape.dimensions.z < size_min) {
return false;
}

// update object size
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;
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 maximum and minimum size
constexpr double max_size = 5.0;
Expand Down
28 changes: 13 additions & 15 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 @@ -59,14 +59,14 @@ BigVehicleTracker::BigVehicleTracker(

// Initialize parameters
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
float r_stddev_x = 0.5; // in vehicle coordinate [m]
float r_stddev_y = 0.4; // in vehicle coordinate [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad]
float r_stddev_vel = 1.0; // in object coordinate [m/s]
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0);
const float r_stddev_x = 0.5; // in vehicle coordinate [m]
const float r_stddev_y = 0.4; // in vehicle coordinate [m]
const float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad]
const float r_stddev_vel = 1.0; // in object coordinate [m/s]
ekf_params_.r_cov_x = r_stddev_x * r_stddev_x;
ekf_params_.r_cov_y = r_stddev_y * r_stddev_y;
ekf_params_.r_cov_yaw = r_stddev_yaw * r_stddev_yaw;
ekf_params_.r_cov_vel = r_stddev_vel * r_stddev_vel;

// velocity deviation threshold
// if the predicted velocity is close to the observed velocity,
Expand Down Expand Up @@ -189,11 +189,6 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
{
autoware_auto_perception_msgs::msg::DetectedObject updating_object = object;

// current (predicted) state
const double tracked_x = motion_model_.getStateElement(IDX::X);
const double tracked_y = motion_model_.getStateElement(IDX::Y);
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);

// OBJECT SHAPE MODEL
// convert to bounding box if input is convex shape
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
Expand All @@ -204,10 +199,13 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
"BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
bbox_object = object;
}
} else {
bbox_object = object;
}

// current (predicted) state
const double tracked_x = motion_model_.getStateElement(IDX::X);
const double tracked_y = motion_model_.getStateElement(IDX::Y);
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);

// get offset measurement
const int nearest_corner_index = utils::getNearestCornerOrSurface(
tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);
Expand Down
31 changes: 14 additions & 17 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 @@ -59,14 +59,14 @@ NormalVehicleTracker::NormalVehicleTracker(

// Initialize parameters
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
float r_stddev_x = 0.5; // in vehicle coordinate [m]
float r_stddev_y = 0.4; // in vehicle coordinate [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad]
float r_stddev_vel = 1.0; // in object coordinate [m/s]
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0);
const float r_stddev_x = 0.5; // in vehicle coordinate [m]
const float r_stddev_y = 0.4; // in vehicle coordinate [m]
const float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad]
const float r_stddev_vel = 1.0; // in object coordinate [m/s]
ekf_params_.r_cov_x = r_stddev_x * r_stddev_x;
ekf_params_.r_cov_y = r_stddev_y * r_stddev_y;
ekf_params_.r_cov_yaw = r_stddev_yaw * r_stddev_yaw;
ekf_params_.r_cov_vel = r_stddev_vel * r_stddev_vel;

// velocity deviation threshold
// if the predicted velocity is close to the observed velocity,
Expand Down Expand Up @@ -190,26 +190,23 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda
{
autoware_auto_perception_msgs::msg::DetectedObject updating_object = object;

// current (predicted) state
const double tracked_x = motion_model_.getStateElement(IDX::X);
const double tracked_y = motion_model_.getStateElement(IDX::Y);
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);

// OBJECT SHAPE MODEL
// convert to bounding box if input is convex shape
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
autoware_auto_perception_msgs::msg::DetectedObject bbox_object = object;
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
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;
}

// current (predicted) state
const double tracked_x = motion_model_.getStateElement(IDX::X);
const double tracked_y = motion_model_.getStateElement(IDX::Y);
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);

// get offset measurement
const int nearest_corner_index = utils::getNearestCornerOrSurface(
tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);
Expand Down

0 comments on commit e588c07

Please sign in to comment.