Skip to content

Commit

Permalink
fix(multi_object_tracker): tune parameters for pedestrian tracker (#6343
Browse files Browse the repository at this point in the history 
)

* fix: tune parameters for pedestrian initial covariances and process noise

Signed-off-by: Taekjin LEE <[email protected]>

* chore: refactoring comments, aligning trackers

Signed-off-by: Taekjin LEE <[email protected]>

---------

Signed-off-by: Taekjin LEE <[email protected]>
  • Loading branch information
@technolojin
technolojin authored Feb 8, 2024
1 parent 488fca5 commit cab9d31
Show file tree
Hide file tree
Showing 4 changed files with 56 additions and 57 deletions.
18 changes: 5 additions & 13 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 @@ -173,7 +173,7 @@ bool BicycleTracker::predict(const rclcpp::Time & time)

bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
{
/* static bicycle model (constant slip angle, constant velocity)
/* Motion model: static bicycle model (constant slip angle, constant velocity)
*
* w_k = vel_k * sin(slip_k) / l_r
* 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
Expand Down Expand Up @@ -291,12 +291,6 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
return true;
}

/**
* @brief measurement update with ekf on receiving detected_object
*
* @param object : Detected object
* @return bool : measurement is updatable
*/
bool BicycleTracker::measureWithPose(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
Expand Down Expand Up @@ -333,19 +327,16 @@ bool BicycleTracker::measureWithPose(
const int dim_y =
use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output

/* Set measurement matrix */
// Set measurement matrix C and observation vector Y
Eigen::MatrixXd Y(dim_y, 1);
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x;
Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y;

/* Set measurement matrix */
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
C(0, IDX::X) = 1.0; // for pos x
C(1, IDX::Y) = 1.0; // for pos y

/* Set measurement noise covariance */
// Set noise covariance matrix R
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);

if (!object.kinematics.has_position_covariance) {
R(0, 0) = ekf_params_.r_cov_x; // x - x
R(0, 1) = 0.0; // x - y
Expand Down Expand Up @@ -471,6 +462,7 @@ bool BicycleTracker::getTrackedObject(
tmp_ekf_for_no_update.getX(X_t);
tmp_ekf_for_no_update.getP(P);

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

Expand Down
11 changes: 6 additions & 5 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 @@ -188,7 +188,7 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time)

bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
{
/* static bicycle model (constant slip angle, constant velocity)
/* Motion model: static bicycle model (constant slip angle, constant velocity)
*
* w_k = vel_k * sin(slip_k) / l_r
* 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
Expand Down Expand Up @@ -362,19 +362,18 @@ bool BigVehicleTracker::measureWithPose(
last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_,
bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_);

/* Set measurement matrix and noise covariance*/
// Set measurement matrix C and observation vector Y
Eigen::MatrixXd Y(dim_y, 1);
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);

Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x;
Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y;
Y(IDX::YAW, 0) = measurement_yaw;
C(0, IDX::X) = 1.0; // for pos x
C(1, IDX::Y) = 1.0; // for pos y
C(2, IDX::YAW) = 1.0; // for yaw

/* Set measurement noise covariance */
// Set noise covariance matrix R
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
if (!object.kinematics.has_position_covariance) {
const double cos_yaw = std::cos(measurement_yaw);
const double sin_yaw = std::sin(measurement_yaw);
Expand Down Expand Up @@ -459,6 +458,7 @@ bool BigVehicleTracker::measureWithShape(
// update lf, lr
lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m
lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m

return true;
}

Expand Down Expand Up @@ -597,6 +597,7 @@ bool BigVehicleTracker::getTrackedObject(
const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
object.shape.footprint =
tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);

return true;
}

Expand Down
10 changes: 5 additions & 5 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 @@ -188,7 +188,7 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time)

bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
{
/* static bicycle model (constant slip angle, constant velocity)
/* Motion model: static bicycle model (constant slip angle, constant velocity)
*
* w_k = vel_k * sin(slip_k) / l_r
* 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
Expand Down Expand Up @@ -362,19 +362,18 @@ bool NormalVehicleTracker::measureWithPose(
last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_,
bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_);

/* Set measurement matrix and noise covariance*/
// Set measurement matrix C and observation vector Y
Eigen::MatrixXd Y(dim_y, 1);
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);

Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x;
Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y;
Y(IDX::YAW, 0) = measurement_yaw;
C(0, IDX::X) = 1.0; // for pos x
C(1, IDX::Y) = 1.0; // for pos y
C(2, IDX::YAW) = 1.0; // for yaw

/* Set measurement noise covariance */
// Set noise covariance matrix R
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
if (!object.kinematics.has_position_covariance) {
const double cos_yaw = std::cos(measurement_yaw);
const double sin_yaw = std::sin(measurement_yaw);
Expand Down Expand Up @@ -598,6 +597,7 @@ bool NormalVehicleTracker::getTrackedObject(
const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
object.shape.footprint =
tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);

return true;
}

Expand Down
74 changes: 40 additions & 34 deletions perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp"

#include "multi_object_tracker/utils/utils.hpp"
#include "object_recognition_utils/object_recognition_utils.hpp"

#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
#include <tier4_autoware_utils/math/normalization.hpp>
Expand All @@ -35,6 +34,7 @@
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif
#include "object_recognition_utils/object_recognition_utils.hpp"

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
Expand All @@ -52,37 +52,41 @@ PedestrianTracker::PedestrianTracker(
{
object_ = object;

// initialize params
float q_stddev_x = 0.4; // [m/s]
float q_stddev_y = 0.4; // [m/s]
float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s]
float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)]
float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)]
float r_stddev_x = 0.4; // [m]
float r_stddev_y = 0.4; // [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
float p0_stddev_x = 1.0; // [m/s]
float p0_stddev_y = 1.0; // [m/s]
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s]
float p0_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)]
float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)]
ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
// Initialize parameters
// measurement noise covariance
float r_stddev_x = 0.4; // [m]
float r_stddev_y = 0.4; // [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [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);
// initial state covariance
float p0_stddev_x = 2.0; // [m]
float p0_stddev_y = 2.0; // [m]
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad]
float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s]
float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s]
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0);
// process noise covariance
float q_stddev_x = 0.4; // [m/s]
float q_stddev_y = 0.4; // [m/s]
float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s]
float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)]
float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)]
ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
// limitations
max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s]
max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s]

// initialize X matrix
// initialize state vector X
Eigen::MatrixXd X(ekf_params_.dim_x, 1);
X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
Expand All @@ -95,7 +99,7 @@ PedestrianTracker::PedestrianTracker(
X(IDX::WZ) = 0.0;
}

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

bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
{
/* == Nonlinear model ==
/* Motion model: Constant turn-rate motion model (CTRV)
*
* x_{k+1} = x_k + vx_k * cos(yaw_k) * dt
* y_{k+1} = y_k + vx_k * sin(yaw_k) * dt
Expand All @@ -164,7 +168,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
*
*/

/* == Linearized model ==
/* Jacobian Matrix
*
* A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0]
* [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0]
Expand All @@ -173,30 +177,30 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
* [ 0, 0, 0, 0, 1]
*/

// X t
// Current state vector X t
Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
ekf.getX(X_t);
const double cos_yaw = std::cos(X_t(IDX::YAW));
const double sin_yaw = std::sin(X_t(IDX::YAW));
const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));

// X t+1
// Predict state vector X t+1
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw)
X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw)
X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega
X_next_t(IDX::VX) = X_t(IDX::VX);
X_next_t(IDX::WZ) = X_t(IDX::WZ);

// A
// State transition matrix A
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt;
A(IDX::X, IDX::VX) = cos_yaw * dt;
A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt;
A(IDX::Y, IDX::VX) = sin_yaw * dt;
A(IDX::YAW, IDX::WZ) = dt;

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

/* Set measurement matrix */
// Set measurement matrix C and observation vector Y
Eigen::MatrixXd Y(dim_y, 1);
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
Y << object.kinematics.pose_with_covariance.pose.position.x,
object.kinematics.pose_with_covariance.pose.position.y;

/* Set measurement matrix */
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
C(0, IDX::X) = 1.0; // for pos x
C(1, IDX::Y) = 1.0; // for pos y
// C(2, IDX::YAW) = 1.0; // for yaw

/* Set measurement noise covariance */
// Set noise covariance matrix R
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
if (!object.kinematics.has_position_covariance) {
R(0, 0) = ekf_params_.r_cov_x; // x - x
Expand All @@ -270,6 +272,8 @@ bool PedestrianTracker::measureWithPose(
// R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y];
// R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
}

// ekf update
if (!ekf_.update(Y, C, R)) {
RCLCPP_WARN(logger_, "Cannot update");
}
Expand Down Expand Up @@ -345,7 +349,7 @@ bool PedestrianTracker::getTrackedObject(
object.object_id = getUUID();
object.classification = getClassification();

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

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

Expand Down Expand Up @@ -399,6 +404,7 @@ bool PedestrianTracker::getTrackedObject(
constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative

twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
Expand Down

0 comments on commit cab9d31

Please sign in to comment.