Skip to content

Commit

Permalink
feat(multi_object_tracker): change ekf_params_.use_measurement_covari…
Browse files Browse the repository at this point in the history 
…ance to has_covariance (#1640)

* feat(multi_object_tracker): make use_measurement_covariance enable

Signed-off-by: scepter914 <scepter914@gmail.com>

* change ekf_params_.use_measurement_covariance to has_covariance

Signed-off-by: scepter914 <scepter914@gmail.com>

Signed-off-by: scepter914 <scepter914@gmail.com>
  • Loading branch information
@scepter914
scepter914 authored Aug 27, 2022
1 parent 7f3389e commit 770fce2
Show file tree
Hide file tree
Showing 10 changed files with 14 additions and 67 deletions.
2 changes: 0 additions & 2 deletions ...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,6 @@ class BicycleTracker : public Tracker
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
// if use_measurement_covariance_ is false, use following params
bool use_measurement_covariance;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand Down
2 changes: 0 additions & 2 deletions ...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,6 @@ class BigVehicleTracker : public Tracker
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
// if use_measurement_covariance_ is false, use following params
bool use_measurement_covariance;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand Down
2 changes: 0 additions & 2 deletions ...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,6 @@ class NormalVehicleTracker : public Tracker
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
// if use_measurement_covariance_ is false, use following params
bool use_measurement_covariance;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand Down
2 changes: 0 additions & 2 deletions ...on/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,6 @@ class PedestrianTracker : public Tracker
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
// if use_measurement_covariance_ is false, use following params
bool use_measurement_covariance;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand Down
2 changes: 0 additions & 2 deletions ...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,6 @@ class UnknownTracker : public Tracker
float q_cov_vy;
float p0_cov_vx;
float p0_cov_vy;
// if use_measurement_covariance_ is false, use following params
bool use_measurement_covariance;
float r_cov_x;
float r_cov_y;
float p0_cov_x;
Expand Down
14 changes: 4 additions & 10 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 @@ -50,7 +50,6 @@ BicycleTracker::BicycleTracker(
object_ = object;

// initialize params
ekf_params_.use_measurement_covariance = false;
float q_stddev_x = 0.6; // [m/s]
float q_stddev_y = 0.6; // [m/s]
float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s]
Expand Down Expand Up @@ -95,11 +94,8 @@ BicycleTracker::BicycleTracker(

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
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 (!object.kinematics.has_position_covariance) {
const double cos_yaw = std::cos(X(IDX::YAW));
const double sin_yaw = std::sin(X(IDX::YAW));
const double sin_2yaw = std::sin(2.0f * X(IDX::YAW));
Expand Down Expand Up @@ -252,10 +248,8 @@ bool BicycleTracker::measureWithPose(

/* 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) {

if (!object.kinematics.has_position_covariance) {
R(0, 0) = ekf_params_.r_cov_x; // x - x
R(0, 1) = 0.0; // x - y
R(1, 1) = ekf_params_.r_cov_y; // y - y
Expand Down
17 changes: 3 additions & 14 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 @@ -48,7 +48,6 @@ BigVehicleTracker::BigVehicleTracker(
object_ = object;

// initialize params
ekf_params_.use_measurement_covariance = false;
float q_stddev_x = 1.5; // [m/s]
float q_stddev_y = 1.5; // [m/s]
float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s]
Expand Down Expand Up @@ -96,11 +95,7 @@ BigVehicleTracker::BigVehicleTracker(

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
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 (!object.kinematics.has_position_covariance) {
const double cos_yaw = std::cos(X(IDX::YAW));
const double sin_yaw = std::sin(X(IDX::YAW));
const double sin_2yaw = std::sin(2.0f * X(IDX::YAW));
Expand Down Expand Up @@ -283,11 +278,7 @@ bool BigVehicleTracker::measureWithPose(
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) {
if (!object.kinematics.has_position_covariance) {
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);
Expand All @@ -312,9 +303,7 @@ bool BigVehicleTracker::measureWithPose(
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) {
if (!object.kinematics.has_twist_covariance) {
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];
Expand Down
17 changes: 3 additions & 14 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 @@ -48,7 +48,6 @@ NormalVehicleTracker::NormalVehicleTracker(
object_ = object;

// initialize params
ekf_params_.use_measurement_covariance = false;
float q_stddev_x = 1.0; // object coordinate [m/s]
float q_stddev_y = 1.0; // object coordinate [m/s]
float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s]
Expand Down Expand Up @@ -96,11 +95,7 @@ NormalVehicleTracker::NormalVehicleTracker(

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
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 (!object.kinematics.has_position_covariance) {
const double cos_yaw = std::cos(X(IDX::YAW));
const double sin_yaw = std::sin(X(IDX::YAW));
const double sin_2yaw = std::sin(2.0f * X(IDX::YAW));
Expand Down Expand Up @@ -283,11 +278,7 @@ bool NormalVehicleTracker::measureWithPose(
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) {
if (!object.kinematics.has_position_covariance) {
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);
Expand All @@ -313,9 +304,7 @@ bool NormalVehicleTracker::measureWithPose(
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) {
if (!object.kinematics.has_twist_covariance) {
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];
Expand Down
12 changes: 2 additions & 10 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 @@ -50,7 +50,6 @@ PedestrianTracker::PedestrianTracker(
object_ = object;

// initialize params
ekf_params_.use_measurement_covariance = false;
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]
Expand Down Expand Up @@ -95,11 +94,7 @@ PedestrianTracker::PedestrianTracker(

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
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 (!object.kinematics.has_position_covariance) {
const double cos_yaw = std::cos(X(IDX::YAW));
const double sin_yaw = std::sin(X(IDX::YAW));
const double sin_2yaw = std::sin(2.0f * X(IDX::YAW));
Expand Down Expand Up @@ -255,10 +250,7 @@ bool PedestrianTracker::measureWithPose(

/* 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) {
if (!object.kinematics.has_position_covariance) {
R(0, 0) = ekf_params_.r_cov_x; // x - x
R(0, 1) = 0.0; // x - y
R(1, 1) = ekf_params_.r_cov_y; // y - y
Expand Down
11 changes: 2 additions & 9 deletions perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@ UnknownTracker::UnknownTracker(
object_ = object;

// initialize params
ekf_params_.use_measurement_covariance = false;
float q_stddev_x = 0.0; // [m/s]
float q_stddev_y = 0.0; // [m/s]
float q_stddev_vx = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)]
Expand Down Expand Up @@ -80,10 +79,7 @@ UnknownTracker::UnknownTracker(

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
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) {
if (!object.kinematics.has_position_covariance) {
// Rotate the covariance matrix according to the vehicle yaw
// because p0_cov_x and y are in the vehicle coordinate system.
P(IDX::X, IDX::X) = ekf_params_.p0_cov_x;
Expand Down Expand Up @@ -193,10 +189,7 @@ bool UnknownTracker::measureWithPose(

/* 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) {
if (!object.kinematics.has_position_covariance) {
R(0, 0) = ekf_params_.r_cov_x; // x - x
R(0, 1) = 0.0; // x - y
R(1, 1) = ekf_params_.r_cov_y; // y - y
Expand Down

0 comments on commit 770fce2

Please sign in to comment.