From f51415a345faaa034060e152f6ecd9c9795b2004 Mon Sep 17 00:00:00 2001
From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Date: Fri, 15 Apr 2022 11:24:37 +0900
Subject: [PATCH] feat(multi_object_tracker): add velocity observation (#696)

* add velocity observation to the normal vehicle tracker

* fix format

* update

* add big vehicle tracker

* fix format
---
 .../tracker/model/big_vehicle_tracker.hpp     |  1 +
 .../tracker/model/normal_vehicle_tracker.hpp  |  1 +
 .../src/tracker/model/big_vehicle_tracker.cpp | 29 +++++++++++++----
 .../tracker/model/normal_vehicle_tracker.cpp  | 32 +++++++++++++++----
 4 files changed, 50 insertions(+), 13 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/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/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");
   }
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..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
@@ -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,8 @@ 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,19 +248,19 @@ 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);
+  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,21 @@ bool NormalVehicleTracker::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];
+    }
+  }
+
+  // update
   if (!ekf_.update(Y, C, R)) {
     RCLCPP_WARN(logger_, "Cannot update");
   }