Skip to content

Commit b89d0ae

Browse files
technolojintechnolojin
committed
fix: bicycle size update bug fix
1 parent 99b9758 commit b89d0ae

File tree

3 files changed

+43
-51
lines changed

3 files changed

+43
-51
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_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_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_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
@@ -229,8 +227,7 @@ bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect
229227

230228
bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object)
231229
{
232-
autoware_perception_msgs::msg::DetectedObject bbox_object;
233-
if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
230+
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
234231
// do not update shape if the input is not a bounding box
235232
return false;
236233
}
@@ -239,21 +236,21 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec
239236
constexpr double size_max = 30.0; // [m]
240237
constexpr double size_min = 0.1; // [m]
241238
if (
242-
bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max ||
243-
bbox_object.shape.dimensions.z > size_max) {
239+
object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max ||
240+
object.shape.dimensions.z > size_max) {
244241
return false;
245242
} else if (
246-
bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min ||
247-
bbox_object.shape.dimensions.z < size_min) {
243+
object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min ||
244+
object.shape.dimensions.z < size_min) {
248245
return false;
249246
}
250247

251248
// update object size
252249
constexpr double gain = 0.1;
253250
constexpr double gain_inv = 1.0 - gain;
254-
bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x;
255-
bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y;
256-
bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z;
251+
bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
252+
bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
253+
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
257254

258255
// set maximum and minimum size
259256
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_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje
189189
{
190190
autoware_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_perception_msgs::msg::DetectedObject bbox_object;
@@ -204,10 +199,13 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje
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

+15-18
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_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO
190190
{
191191
autoware_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_perception_msgs::msg::DetectedObject bbox_object;
201-
if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
195+
autoware_auto_perception_msgs::msg::DetectedObject bbox_object = object;
196+
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)