19
19
#include " multi_object_tracker/tracker/model/pedestrian_tracker.hpp"
20
20
21
21
#include " multi_object_tracker/utils/utils.hpp"
22
- #include " object_recognition_utils/object_recognition_utils.hpp"
23
22
24
23
#include < tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
25
24
#include < tier4_autoware_utils/math/normalization.hpp>
35
34
#else
36
35
#include < tf2_geometry_msgs/tf2_geometry_msgs.hpp>
37
36
#endif
37
+ #include " object_recognition_utils/object_recognition_utils.hpp"
38
38
39
39
#define EIGEN_MPL2_ONLY
40
40
#include < Eigen/Core>
@@ -52,37 +52,41 @@ PedestrianTracker::PedestrianTracker(
52
52
{
53
53
object_ = object;
54
54
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]
74
60
ekf_params_.r_cov_x = std::pow (r_stddev_x, 2.0 );
75
61
ekf_params_.r_cov_y = std::pow (r_stddev_y, 2.0 );
76
62
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]
77
69
ekf_params_.p0_cov_x = std::pow (p0_stddev_x, 2.0 );
78
70
ekf_params_.p0_cov_y = std::pow (p0_stddev_y, 2.0 );
79
71
ekf_params_.p0_cov_yaw = std::pow (p0_stddev_yaw, 2.0 );
80
72
ekf_params_.p0_cov_vx = std::pow (p0_stddev_vx, 2.0 );
81
73
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
82
86
max_vx_ = tier4_autoware_utils::kmph2mps (10 ); // [m/s]
83
87
max_wz_ = tier4_autoware_utils::deg2rad (30 ); // [rad/s]
84
88
85
- // initialize X matrix
89
+ // initialize state vector X
86
90
Eigen::MatrixXd X (ekf_params_.dim_x , 1 );
87
91
X (IDX::X) = object.kinematics .pose_with_covariance .pose .position .x ;
88
92
X (IDX::Y) = object.kinematics .pose_with_covariance .pose .position .y ;
@@ -95,7 +99,7 @@ PedestrianTracker::PedestrianTracker(
95
99
X (IDX::WZ) = 0.0 ;
96
100
}
97
101
98
- // initialize P matrix
102
+ // initialize state covariance matrix P
99
103
Eigen::MatrixXd P = Eigen::MatrixXd::Zero (ekf_params_.dim_x , ekf_params_.dim_x );
100
104
if (!object.kinematics .has_position_covariance ) {
101
105
const double cos_yaw = std::cos (X (IDX::YAW));
@@ -154,7 +158,7 @@ bool PedestrianTracker::predict(const rclcpp::Time & time)
154
158
155
159
bool PedestrianTracker::predict (const double dt, KalmanFilter & ekf) const
156
160
{
157
- /* == Nonlinear model ==
161
+ /* Motion model: Constant turn-rate motion model (CTRV)
158
162
*
159
163
* x_{k+1} = x_k + vx_k * cos(yaw_k) * dt
160
164
* y_{k+1} = y_k + vx_k * sin(yaw_k) * dt
@@ -164,7 +168,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
164
168
*
165
169
*/
166
170
167
- /* == Linearized model ==
171
+ /* Jacobian Matrix
168
172
*
169
173
* A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0]
170
174
* [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0]
@@ -173,30 +177,30 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
173
177
* [ 0, 0, 0, 0, 1]
174
178
*/
175
179
176
- // X t
180
+ // Current state vector X t
177
181
Eigen::MatrixXd X_t (ekf_params_.dim_x , 1 ); // predicted state
178
182
ekf.getX (X_t);
179
183
const double cos_yaw = std::cos (X_t (IDX::YAW));
180
184
const double sin_yaw = std::sin (X_t (IDX::YAW));
181
185
const double sin_2yaw = std::sin (2 .0f * X_t (IDX::YAW));
182
186
183
- // X t+1
187
+ // Predict state vector X t+1
184
188
Eigen::MatrixXd X_next_t (ekf_params_.dim_x , 1 ); // predicted state
185
189
X_next_t (IDX::X) = X_t (IDX::X) + X_t (IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw)
186
190
X_next_t (IDX::Y) = X_t (IDX::Y) + X_t (IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw)
187
191
X_next_t (IDX::YAW) = X_t (IDX::YAW) + (X_t (IDX::WZ)) * dt; // dyaw = omega
188
192
X_next_t (IDX::VX) = X_t (IDX::VX);
189
193
X_next_t (IDX::WZ) = X_t (IDX::WZ);
190
194
191
- // A
195
+ // State transition matrix A
192
196
Eigen::MatrixXd A = Eigen::MatrixXd::Identity (ekf_params_.dim_x , ekf_params_.dim_x );
193
197
A (IDX::X, IDX::YAW) = -X_t (IDX::VX) * sin_yaw * dt;
194
198
A (IDX::X, IDX::VX) = cos_yaw * dt;
195
199
A (IDX::Y, IDX::YAW) = X_t (IDX::VX) * cos_yaw * dt;
196
200
A (IDX::Y, IDX::VX) = sin_yaw * dt;
197
201
A (IDX::YAW, IDX::WZ) = dt;
198
202
199
- // Q
203
+ // Process noise covariance Q
200
204
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero (ekf_params_.dim_x , ekf_params_.dim_x );
201
205
// Rotate the covariance matrix according to the vehicle yaw
202
206
// because q_cov_x and y are in the vehicle coordinate system.
@@ -240,18 +244,16 @@ bool PedestrianTracker::measureWithPose(
240
244
// if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false;
241
245
// }
242
246
243
- /* Set measurement matrix */
247
+ // Set measurement matrix C and observation vector Y
244
248
Eigen::MatrixXd Y (dim_y, 1 );
249
+ Eigen::MatrixXd C = Eigen::MatrixXd::Zero (dim_y, ekf_params_.dim_x );
245
250
Y << object.kinematics .pose_with_covariance .pose .position .x ,
246
251
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 );
250
252
C (0 , IDX::X) = 1.0 ; // for pos x
251
253
C (1 , IDX::Y) = 1.0 ; // for pos y
252
254
// C(2, IDX::YAW) = 1.0; // for yaw
253
255
254
- /* Set measurement noise covariance */
256
+ // Set noise covariance matrix R
255
257
Eigen::MatrixXd R = Eigen::MatrixXd::Zero (dim_y, dim_y);
256
258
if (!object.kinematics .has_position_covariance ) {
257
259
R (0 , 0 ) = ekf_params_.r_cov_x ; // x - x
@@ -270,6 +272,8 @@ bool PedestrianTracker::measureWithPose(
270
272
// R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y];
271
273
// R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
272
274
}
275
+
276
+ // ekf update
273
277
if (!ekf_.update (Y, C, R)) {
274
278
RCLCPP_WARN (logger_, " Cannot update" );
275
279
}
@@ -345,7 +349,7 @@ bool PedestrianTracker::getTrackedObject(
345
349
object.object_id = getUUID ();
346
350
object.classification = getClassification ();
347
351
348
- // predict kinematics
352
+ // predict state
349
353
KalmanFilter tmp_ekf_for_no_update = ekf_;
350
354
const double dt = (time - last_update_time_).seconds ();
351
355
if (0.001 /* 1msec*/ < dt) {
@@ -356,6 +360,7 @@ bool PedestrianTracker::getTrackedObject(
356
360
tmp_ekf_for_no_update.getX (X_t);
357
361
tmp_ekf_for_no_update.getP (P);
358
362
363
+ /* put predicted pose and twist to output object */
359
364
auto & pose_with_cov = object.kinematics .pose_with_covariance ;
360
365
auto & twist_with_cov = object.kinematics .twist_with_covariance ;
361
366
@@ -399,6 +404,7 @@ bool PedestrianTracker::getTrackedObject(
399
404
constexpr double vz_cov = 0.1 * 0.1 ; // TODO(yukkysaito) Currently tentative
400
405
constexpr double wx_cov = 0.1 * 0.1 ; // TODO(yukkysaito) Currently tentative
401
406
constexpr double wy_cov = 0.1 * 0.1 ; // TODO(yukkysaito) Currently tentative
407
+
402
408
twist_with_cov.covariance [utils::MSG_COV_IDX::X_X] = P (IDX::VX, IDX::VX);
403
409
twist_with_cov.covariance [utils::MSG_COV_IDX::Y_Y] = vy_cov;
404
410
twist_with_cov.covariance [utils::MSG_COV_IDX::Z_Z] = vz_cov;
0 commit comments