Skip to content

Commit cab9d31

Browse files
technolojintechnolojin
authored
fix(multi_object_tracker): tune parameters for pedestrian tracker (#6343)
* fix: tune parameters for pedestrian initial covariances and process noise Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring comments, aligning trackers Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 488fca5 commit cab9d31

File tree

4 files changed

+56
-57
lines changed

4 files changed

+56
-57
lines changed

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

+5-13
Original file line numberDiff line numberDiff line change
@@ -173,7 +173,7 @@ bool BicycleTracker::predict(const rclcpp::Time & time)
173173

174174
bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
175175
{
176-
/* static bicycle model (constant slip angle, constant velocity)
176+
/* Motion model: static bicycle model (constant slip angle, constant velocity)
177177
*
178178
* w_k = vel_k * sin(slip_k) / l_r
179179
* x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt
@@ -291,12 +291,6 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
291291
return true;
292292
}
293293

294-
/**
295-
* @brief measurement update with ekf on receiving detected_object
296-
*
297-
* @param object : Detected object
298-
* @return bool : measurement is updatable
299-
*/
300294
bool BicycleTracker::measureWithPose(
301295
const autoware_auto_perception_msgs::msg::DetectedObject & object)
302296
{
@@ -333,19 +327,16 @@ bool BicycleTracker::measureWithPose(
333327
const int dim_y =
334328
use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output
335329

336-
/* Set measurement matrix */
330+
// Set measurement matrix C and observation vector Y
337331
Eigen::MatrixXd Y(dim_y, 1);
332+
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
338333
Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x;
339334
Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y;
340-
341-
/* Set measurement matrix */
342-
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
343335
C(0, IDX::X) = 1.0; // for pos x
344336
C(1, IDX::Y) = 1.0; // for pos y
345337

346-
/* Set measurement noise covariance */
338+
// Set noise covariance matrix R
347339
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
348-
349340
if (!object.kinematics.has_position_covariance) {
350341
R(0, 0) = ekf_params_.r_cov_x; // x - x
351342
R(0, 1) = 0.0; // x - y
@@ -471,6 +462,7 @@ bool BicycleTracker::getTrackedObject(
471462
tmp_ekf_for_no_update.getX(X_t);
472463
tmp_ekf_for_no_update.getP(P);
473464

465+
/* put predicted pose and twist to output object */
474466
auto & pose_with_cov = object.kinematics.pose_with_covariance;
475467
auto & twist_with_cov = object.kinematics.twist_with_covariance;
476468

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

+6-5
Original file line numberDiff line numberDiff line change
@@ -188,7 +188,7 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time)
188188

189189
bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
190190
{
191-
/* static bicycle model (constant slip angle, constant velocity)
191+
/* Motion model: static bicycle model (constant slip angle, constant velocity)
192192
*
193193
* w_k = vel_k * sin(slip_k) / l_r
194194
* x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt
@@ -362,19 +362,18 @@ bool BigVehicleTracker::measureWithPose(
362362
last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_,
363363
bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_);
364364

365-
/* Set measurement matrix and noise covariance*/
365+
// Set measurement matrix C and observation vector Y
366366
Eigen::MatrixXd Y(dim_y, 1);
367367
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
368-
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
369-
370368
Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x;
371369
Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y;
372370
Y(IDX::YAW, 0) = measurement_yaw;
373371
C(0, IDX::X) = 1.0; // for pos x
374372
C(1, IDX::Y) = 1.0; // for pos y
375373
C(2, IDX::YAW) = 1.0; // for yaw
376374

377-
/* Set measurement noise covariance */
375+
// Set noise covariance matrix R
376+
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
378377
if (!object.kinematics.has_position_covariance) {
379378
const double cos_yaw = std::cos(measurement_yaw);
380379
const double sin_yaw = std::sin(measurement_yaw);
@@ -459,6 +458,7 @@ bool BigVehicleTracker::measureWithShape(
459458
// update lf, lr
460459
lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m
461460
lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m
461+
462462
return true;
463463
}
464464

@@ -597,6 +597,7 @@ bool BigVehicleTracker::getTrackedObject(
597597
const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
598598
object.shape.footprint =
599599
tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
600+
600601
return true;
601602
}
602603

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

+5-5
Original file line numberDiff line numberDiff line change
@@ -188,7 +188,7 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time)
188188

189189
bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
190190
{
191-
/* static bicycle model (constant slip angle, constant velocity)
191+
/* Motion model: static bicycle model (constant slip angle, constant velocity)
192192
*
193193
* w_k = vel_k * sin(slip_k) / l_r
194194
* x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt
@@ -362,19 +362,18 @@ bool NormalVehicleTracker::measureWithPose(
362362
last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_,
363363
bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_);
364364

365-
/* Set measurement matrix and noise covariance*/
365+
// Set measurement matrix C and observation vector Y
366366
Eigen::MatrixXd Y(dim_y, 1);
367367
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
368-
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
369-
370368
Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x;
371369
Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y;
372370
Y(IDX::YAW, 0) = measurement_yaw;
373371
C(0, IDX::X) = 1.0; // for pos x
374372
C(1, IDX::Y) = 1.0; // for pos y
375373
C(2, IDX::YAW) = 1.0; // for yaw
376374

377-
/* Set measurement noise covariance */
375+
// Set noise covariance matrix R
376+
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
378377
if (!object.kinematics.has_position_covariance) {
379378
const double cos_yaw = std::cos(measurement_yaw);
380379
const double sin_yaw = std::sin(measurement_yaw);
@@ -598,6 +597,7 @@ bool NormalVehicleTracker::getTrackedObject(
598597
const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
599598
object.shape.footprint =
600599
tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
600+
601601
return true;
602602
}
603603

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

+40-34
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@
1919
#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp"
2020

2121
#include "multi_object_tracker/utils/utils.hpp"
22-
#include "object_recognition_utils/object_recognition_utils.hpp"
2322

2423
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
2524
#include <tier4_autoware_utils/math/normalization.hpp>
@@ -35,6 +34,7 @@
3534
#else
3635
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
3736
#endif
37+
#include "object_recognition_utils/object_recognition_utils.hpp"
3838

3939
#define EIGEN_MPL2_ONLY
4040
#include <Eigen/Core>
@@ -52,37 +52,41 @@ PedestrianTracker::PedestrianTracker(
5252
{
5353
object_ = object;
5454

55-
// initialize params
56-
float q_stddev_x = 0.4; // [m/s]
57-
float q_stddev_y = 0.4; // [m/s]
58-
float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s]
59-
float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)]
60-
float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)]
61-
float r_stddev_x = 0.4; // [m]
62-
float r_stddev_y = 0.4; // [m]
63-
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
64-
float p0_stddev_x = 1.0; // [m/s]
65-
float p0_stddev_y = 1.0; // [m/s]
66-
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s]
67-
float p0_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)]
68-
float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)]
69-
ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
70-
ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
71-
ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
72-
ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
73-
ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
55+
// Initialize parameters
56+
// measurement noise covariance
57+
float r_stddev_x = 0.4; // [m]
58+
float r_stddev_y = 0.4; // [m]
59+
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
7460
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
7561
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
7662
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
63+
// initial state covariance
64+
float p0_stddev_x = 2.0; // [m]
65+
float p0_stddev_y = 2.0; // [m]
66+
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad]
67+
float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s]
68+
float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s]
7769
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
7870
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
7971
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
8072
ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
8173
ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0);
74+
// process noise covariance
75+
float q_stddev_x = 0.4; // [m/s]
76+
float q_stddev_y = 0.4; // [m/s]
77+
float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s]
78+
float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)]
79+
float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)]
80+
ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
81+
ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
82+
ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
83+
ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
84+
ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
85+
// limitations
8286
max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s]
8387
max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s]
8488

85-
// initialize X matrix
89+
// initialize state vector X
8690
Eigen::MatrixXd X(ekf_params_.dim_x, 1);
8791
X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
8892
X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
@@ -95,7 +99,7 @@ PedestrianTracker::PedestrianTracker(
9599
X(IDX::WZ) = 0.0;
96100
}
97101

98-
// initialize P matrix
102+
// initialize state covariance matrix P
99103
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
100104
if (!object.kinematics.has_position_covariance) {
101105
const double cos_yaw = std::cos(X(IDX::YAW));
@@ -154,7 +158,7 @@ bool PedestrianTracker::predict(const rclcpp::Time & time)
154158

155159
bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
156160
{
157-
/* == Nonlinear model ==
161+
/* Motion model: Constant turn-rate motion model (CTRV)
158162
*
159163
* x_{k+1} = x_k + vx_k * cos(yaw_k) * dt
160164
* y_{k+1} = y_k + vx_k * sin(yaw_k) * dt
@@ -164,7 +168,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
164168
*
165169
*/
166170

167-
/* == Linearized model ==
171+
/* Jacobian Matrix
168172
*
169173
* A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0]
170174
* [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0]
@@ -173,30 +177,30 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
173177
* [ 0, 0, 0, 0, 1]
174178
*/
175179

176-
// X t
180+
// Current state vector X t
177181
Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
178182
ekf.getX(X_t);
179183
const double cos_yaw = std::cos(X_t(IDX::YAW));
180184
const double sin_yaw = std::sin(X_t(IDX::YAW));
181185
const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));
182186

183-
// X t+1
187+
// Predict state vector X t+1
184188
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
185189
X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw)
186190
X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw)
187191
X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega
188192
X_next_t(IDX::VX) = X_t(IDX::VX);
189193
X_next_t(IDX::WZ) = X_t(IDX::WZ);
190194

191-
// A
195+
// State transition matrix A
192196
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
193197
A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt;
194198
A(IDX::X, IDX::VX) = cos_yaw * dt;
195199
A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt;
196200
A(IDX::Y, IDX::VX) = sin_yaw * dt;
197201
A(IDX::YAW, IDX::WZ) = dt;
198202

199-
// Q
203+
// Process noise covariance Q
200204
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
201205
// Rotate the covariance matrix according to the vehicle yaw
202206
// because q_cov_x and y are in the vehicle coordinate system.
@@ -240,18 +244,16 @@ bool PedestrianTracker::measureWithPose(
240244
// if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false;
241245
// }
242246

243-
/* Set measurement matrix */
247+
// Set measurement matrix C and observation vector Y
244248
Eigen::MatrixXd Y(dim_y, 1);
249+
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
245250
Y << object.kinematics.pose_with_covariance.pose.position.x,
246251
object.kinematics.pose_with_covariance.pose.position.y;
247-
248-
/* Set measurement matrix */
249-
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
250252
C(0, IDX::X) = 1.0; // for pos x
251253
C(1, IDX::Y) = 1.0; // for pos y
252254
// C(2, IDX::YAW) = 1.0; // for yaw
253255

254-
/* Set measurement noise covariance */
256+
// Set noise covariance matrix R
255257
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
256258
if (!object.kinematics.has_position_covariance) {
257259
R(0, 0) = ekf_params_.r_cov_x; // x - x
@@ -270,6 +272,8 @@ bool PedestrianTracker::measureWithPose(
270272
// R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y];
271273
// R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
272274
}
275+
276+
// ekf update
273277
if (!ekf_.update(Y, C, R)) {
274278
RCLCPP_WARN(logger_, "Cannot update");
275279
}
@@ -345,7 +349,7 @@ bool PedestrianTracker::getTrackedObject(
345349
object.object_id = getUUID();
346350
object.classification = getClassification();
347351

348-
// predict kinematics
352+
// predict state
349353
KalmanFilter tmp_ekf_for_no_update = ekf_;
350354
const double dt = (time - last_update_time_).seconds();
351355
if (0.001 /*1msec*/ < dt) {
@@ -356,6 +360,7 @@ bool PedestrianTracker::getTrackedObject(
356360
tmp_ekf_for_no_update.getX(X_t);
357361
tmp_ekf_for_no_update.getP(P);
358362

363+
/* put predicted pose and twist to output object */
359364
auto & pose_with_cov = object.kinematics.pose_with_covariance;
360365
auto & twist_with_cov = object.kinematics.twist_with_covariance;
361366

@@ -399,6 +404,7 @@ bool PedestrianTracker::getTrackedObject(
399404
constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
400405
constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
401406
constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
407+
402408
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
403409
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
404410
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;

0 commit comments

Comments
 (0)