Skip to content

Commit e588c07

Browse files
technolojintechnolojin
committed
fix: bicycle size update bug fix
1 parent 1fafa9d commit e588c07

File tree

3 files changed

+42
-50
lines changed

3 files changed

+42
-50
lines changed

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

+15-18
Original file line numberDiff line numberDiff line change
@@ -58,12 +58,12 @@ BicycleTracker::BicycleTracker(
5858

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

6868
// OBJECT SHAPE MODEL
6969
if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
@@ -169,16 +169,14 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb
169169
const autoware_auto_perception_msgs::msg::DetectedObject & object,
170170
const geometry_msgs::msg::Transform & /*self_transform*/) const
171171
{
172-
autoware_auto_perception_msgs::msg::DetectedObject updating_object;
172+
autoware_auto_perception_msgs::msg::DetectedObject updating_object = object;
173173

174174
// OBJECT SHAPE MODEL
175175
// convert to bounding box if input is convex shape
176176
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
177177
if (!utils::convertConvexHullToBoundingBox(object, updating_object)) {
178178
updating_object = object;
179179
}
180-
} else {
181-
updating_object = object;
182180
}
183181

184182
// UNCERTAINTY MODEL
@@ -231,8 +229,7 @@ bool BicycleTracker::measureWithPose(
231229
bool BicycleTracker::measureWithShape(
232230
const autoware_auto_perception_msgs::msg::DetectedObject & object)
233231
{
234-
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
235-
if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
232+
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
236233
// do not update shape if the input is not a bounding box
237234
return false;
238235
}
@@ -241,21 +238,21 @@ bool BicycleTracker::measureWithShape(
241238
constexpr double size_max = 30.0; // [m]
242239
constexpr double size_min = 0.1; // [m]
243240
if (
244-
bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max ||
245-
bbox_object.shape.dimensions.z > size_max) {
241+
object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max ||
242+
object.shape.dimensions.z > size_max) {
246243
return false;
247244
} else if (
248-
bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min ||
249-
bbox_object.shape.dimensions.z < size_min) {
245+
object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min ||
246+
object.shape.dimensions.z < size_min) {
250247
return false;
251248
}
252249

253250
// update object size
254251
constexpr double gain = 0.1;
255252
constexpr double gain_inv = 1.0 - gain;
256-
bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x;
257-
bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y;
258-
bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z;
253+
bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
254+
bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
255+
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
259256

260257
// set maximum and minimum size
261258
constexpr double max_size = 5.0;

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

+13-15
Original file line numberDiff line numberDiff line change
@@ -59,14 +59,14 @@ BigVehicleTracker::BigVehicleTracker(
5959

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

7171
// velocity deviation threshold
7272
// if the predicted velocity is close to the observed velocity,
@@ -189,11 +189,6 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
189189
{
190190
autoware_auto_perception_msgs::msg::DetectedObject updating_object = object;
191191

192-
// current (predicted) state
193-
const double tracked_x = motion_model_.getStateElement(IDX::X);
194-
const double tracked_y = motion_model_.getStateElement(IDX::Y);
195-
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);
196-
197192
// OBJECT SHAPE MODEL
198193
// convert to bounding box if input is convex shape
199194
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
@@ -204,10 +199,13 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin
204199
"BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
205200
bbox_object = object;
206201
}
207-
} else {
208-
bbox_object = object;
209202
}
210203

204+
// current (predicted) state
205+
const double tracked_x = motion_model_.getStateElement(IDX::X);
206+
const double tracked_y = motion_model_.getStateElement(IDX::Y);
207+
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);
208+
211209
// get offset measurement
212210
const int nearest_corner_index = utils::getNearestCornerOrSurface(
213211
tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);

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

+14-17
Original file line numberDiff line numberDiff line change
@@ -59,14 +59,14 @@ NormalVehicleTracker::NormalVehicleTracker(
5959

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

7171
// velocity deviation threshold
7272
// if the predicted velocity is close to the observed velocity,
@@ -190,26 +190,23 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda
190190
{
191191
autoware_auto_perception_msgs::msg::DetectedObject updating_object = object;
192192

193-
// current (predicted) state
194-
const double tracked_x = motion_model_.getStateElement(IDX::X);
195-
const double tracked_y = motion_model_.getStateElement(IDX::Y);
196-
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);
197-
198193
// OBJECT SHAPE MODEL
199194
// convert to bounding box if input is convex shape
200-
autoware_auto_perception_msgs::msg::DetectedObject bbox_object;
195+
autoware_auto_perception_msgs::msg::DetectedObject bbox_object = object;
201196
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
202197
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
203198
RCLCPP_WARN(
204199
logger_,
205200
"NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box.");
206201
bbox_object = object;
207202
}
208-
209-
} else {
210-
bbox_object = object;
211203
}
212204

205+
// current (predicted) state
206+
const double tracked_x = motion_model_.getStateElement(IDX::X);
207+
const double tracked_y = motion_model_.getStateElement(IDX::Y);
208+
const double tracked_yaw = motion_model_.getStateElement(IDX::YAW);
209+
213210
// get offset measurement
214211
const int nearest_corner_index = utils::getNearestCornerOrSurface(
215212
tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);

0 commit comments

Comments
 (0)