From e3027bcd54ee230ea2c1ee50abb8f162d3c76ca8 Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 14 Apr 2022 15:16:13 +0900 Subject: [PATCH 1/5] add velocity observation to the normal vehicle tracker --- .../tracker/model/normal_vehicle_tracker.hpp | 1 + .../tracker/model/normal_vehicle_tracker.cpp | 108 ++++++++++++------ 2 files changed, 76 insertions(+), 33 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index bdaef7309c916..5d6cac7430671 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -54,6 +54,7 @@ class NormalVehicleTracker : public Tracker float r_cov_x; float r_cov_y; float r_cov_yaw; + float r_cov_vx; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 818912aa7b6b8..303b2f9a846c7 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -49,6 +49,7 @@ NormalVehicleTracker::NormalVehicleTracker( float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] + float r_stddev_vx = 1.0; // object coordinate [m/s] float p0_stddev_x = 1.0; // object coordinate [m/s] float p0_stddev_y = 0.3; // object coordinate [m/s] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] @@ -62,6 +63,7 @@ NormalVehicleTracker::NormalVehicleTracker( 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); + ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); 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); @@ -230,7 +232,7 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output + const int dim_y = object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { @@ -245,43 +247,83 @@ bool NormalVehicleTracker::measureWithPose( } } - /* Set measurement matrix */ + /* Set measurement matrix and noise covariance*/ Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw; - - /* 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 */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if ( - !ekf_params_.use_measurement_covariance || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw + + if(object.kinematics.has_twist) { + Y << object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw, object.kinematics.twist_with_covariance.twist.linear.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 + C(3, IDX::VX) = 1.0; // for vx + + /* Set measurement noise covariance */ + if ( + !ekf_params_.use_measurement_covariance || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0 || + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) { + const double cos_yaw = std::cos(measurement_yaw); + const double sin_yaw = std::sin(measurement_yaw); + const double sin_2yaw = std::sin(2.0f * measurement_yaw); + R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + R(1, 0) = R(0, 1); // y - x + R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw + R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + } else { + R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; + R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; + R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; + 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]; + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - 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]; + Y << object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y, 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 */ + if ( + !ekf_params_.use_measurement_covariance || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0 ) { + const double cos_yaw = std::cos(measurement_yaw); + const double sin_yaw = std::sin(measurement_yaw); + const double sin_2yaw = std::sin(2.0f * measurement_yaw); + R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + R(1, 0) = R(0, 1); // y - x + R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw + } else { + R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; + R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; + R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; + 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]; + } } + + + // update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } From e3a2c4d84b0da23b08537e548bad3534af5fbdfb Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 14 Apr 2022 15:17:14 +0900 Subject: [PATCH 2/5] fix format --- .../src/tracker/model/normal_vehicle_tracker.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 303b2f9a846c7..2f751cfccaa61 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -232,7 +232,8 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - const int dim_y = object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output + const int dim_y = + object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { @@ -252,13 +253,14 @@ bool NormalVehicleTracker::measureWithPose( Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if(object.kinematics.has_twist) { + if (object.kinematics.has_twist) { Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw, object.kinematics.twist_with_covariance.twist.linear.x; + object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw, + object.kinematics.twist_with_covariance.twist.linear.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 - C(3, IDX::VX) = 1.0; // for vx + C(3, IDX::VX) = 1.0; // for vx /* Set measurement noise covariance */ if ( @@ -300,7 +302,7 @@ bool NormalVehicleTracker::measureWithPose( !ekf_params_.use_measurement_covariance || object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0 ) { + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0) { const double cos_yaw = std::cos(measurement_yaw); const double sin_yaw = std::sin(measurement_yaw); const double sin_2yaw = std::sin(2.0f * measurement_yaw); @@ -322,7 +324,6 @@ bool NormalVehicleTracker::measureWithPose( } } - // update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); From 17b8a93d8e315362ef72a331bd609cb3b9faa844 Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 14 Apr 2022 21:51:53 +0900 Subject: [PATCH 3/5] update --- .../tracker/model/normal_vehicle_tracker.cpp | 96 +++++++------------ 1 file changed, 35 insertions(+), 61 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 2f751cfccaa61..0bc62f297b302 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -253,75 +253,49 @@ bool NormalVehicleTracker::measureWithPose( 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) = object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = 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 */ + if ( + !ekf_params_.use_measurement_covariance || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 || + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0) { + const double cos_yaw = std::cos(measurement_yaw); + const double sin_yaw = std::sin(measurement_yaw); + const double sin_2yaw = std::sin(2.0f * measurement_yaw); + R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + R(1, 0) = R(0, 1); // y - x + R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw + } else { + R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; + R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; + R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; + 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]; + } + if (object.kinematics.has_twist) { - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw, - object.kinematics.twist_with_covariance.twist.linear.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 + Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; C(3, IDX::VX) = 1.0; // for vx - /* Set measurement noise covariance */ - if ( - !ekf_params_.use_measurement_covariance || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0 || + if(!ekf_params_.use_measurement_covariance || object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw R(3, 3) = ekf_params_.r_cov_vx; // vx -vx } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - 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]; R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } - } else { - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y, 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 */ - if ( - !ekf_params_.use_measurement_covariance || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 || - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - 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]; - } } // update From 46d74ad77b64589aaaffdeb80c66a75653fb4af4 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 15 Apr 2022 01:36:13 +0900 Subject: [PATCH 4/5] add big vehicle tracker --- .../tracker/model/big_vehicle_tracker.hpp | 1 + .../src/tracker/model/big_vehicle_tracker.cpp | 29 +++++++++++++++---- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 991d9940a0f1e..6232c9f09bea7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -54,6 +54,7 @@ class BigVehicleTracker : public Tracker float r_cov_x; float r_cov_y; float r_cov_yaw; + float r_cov_vx; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 0c58c83ae722d..50a64fa7d5974 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -49,6 +49,7 @@ BigVehicleTracker::BigVehicleTracker( float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float r_stddev_vx = 1.0; // [m/s] float p0_stddev_x = 1.5; // [m] float p0_stddev_y = 0.5; // [m] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -62,6 +63,7 @@ BigVehicleTracker::BigVehicleTracker( 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); + ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); 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); @@ -230,7 +232,8 @@ bool BigVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output + const int dim_y = + object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { @@ -247,17 +250,17 @@ bool BigVehicleTracker::measureWithPose( /* Set measurement matrix */ Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw; - - /* Set measurement matrix */ 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) = object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = 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 */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if ( !ekf_params_.use_measurement_covariance || object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || @@ -282,6 +285,20 @@ bool BigVehicleTracker::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]; } + + if (object.kinematics.has_twist) { + Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VX) = 1.0; // for vx + + if ( + !ekf_params_.use_measurement_covariance || + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) { + R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + } else { + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + } + if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } From ab7d56c8a5f1435f7b89e76195416bfaed239488 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 15 Apr 2022 01:42:51 +0900 Subject: [PATCH 5/5] fix format --- .../src/tracker/model/normal_vehicle_tracker.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 0bc62f297b302..3dddd2d8cc54d 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -288,11 +288,12 @@ bool NormalVehicleTracker::measureWithPose( if (object.kinematics.has_twist) { Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + C(3, IDX::VX) = 1.0; // for vx - if(!ekf_params_.use_measurement_covariance || + if ( + !ekf_params_.use_measurement_covariance || object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vx; // vx -vx } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; }