refactor(multi_object_tracker): add updateStatePoseVel method to BicycleMotionModel and update measurement logic in VehicleTracker

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp

@@ -94,6 +94,10 @@ class BicycleMotionModel : public MotionModel
     const double & x, const double & y, const double & yaw,
     const std::array<double, 36> & pose_cov);
 
+  bool updateStatePoseVel(
+    const double & x, const double & y, const std::array<double, 36> & pose_cov, const double & vel,
+    const std::array<double, 36> & twist_cov);
+
   bool updateStatePoseHeadVel(
     const double & x, const double & y, const double & yaw, const std::array<double, 36> & pose_cov,
     const double & vel, const std::array<double, 36> & twist_cov);

perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp

@@ -135,22 +135,20 @@ VehicleTracker::VehicleTracker(
 
 bool VehicleTracker::predict(const rclcpp::Time & time)
 {
-  bool success = motion_model_.predictState(time);
-  if (!success) {
-    return false;
-  }
-  return true;
+  return motion_model_.predictState(time);
 }
 
 bool VehicleTracker::measureWithPose(const types::DynamicObject & object)
 {
-  // current (predicted) state
-  const double tracked_vel = motion_model_.getStateElement(IDX::VEL);
+  // get measurement yaw angle to update
+  bool is_yaw_available =
+    object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE;
 
   // velocity capability is checked only when the object has velocity measurement
   // and the predicted velocity is close to the observed velocity
   bool is_velocity_available = false;
   if (object.kinematics.has_twist) {
+    const double tracked_vel = motion_model_.getStateElement(IDX::VEL);
     const double & observed_vel = object.twist.linear.x;
     if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) {
       // Velocity deviation is small
@@ -166,11 +164,21 @@ bool VehicleTracker::measureWithPose(const types::DynamicObject & object)
     const double yaw = tf2::getYaw(object.pose.orientation);
     const double vel = object.twist.linear.x;
 
-    if (is_velocity_available) {
+    if (is_yaw_available && is_velocity_available) {
+      // update with yaw angle and velocity
       is_updated = motion_model_.updateStatePoseHeadVel(
         x, y, yaw, object.pose_covariance, vel, object.twist_covariance);
-    } else {
+    } else if (is_yaw_available && !is_velocity_available) {
+      // update with yaw angle, but without velocity
       is_updated = motion_model_.updateStatePoseHead(x, y, yaw, object.pose_covariance);
+    } else if (!is_yaw_available && is_velocity_available) {
+      // update without yaw angle, but with velocity
+      is_updated = motion_model_.updateStatePoseVel(
+        x, y, object.pose_covariance, vel, object.twist_covariance);
+    } else {
+      // update without yaw angle and velocity
+      is_updated = motion_model_.updateStatePose(
+        x, y, object.pose_covariance);  // update without yaw angle and velocity
     }
     motion_model_.limitStates();
   }
@@ -206,6 +214,9 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object)
   object_extension.y = gain_inv * object_extension.y + gain * object.shape.dimensions.y;
   object_extension.z = gain_inv * object_extension.z + gain * object.shape.dimensions.z;
 
+  // set shape type, which is bounding box
+  object_.shape.type = object.shape.type;
+
   // set maximum and minimum size
   limitObjectExtension(object_model_);
 

perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp

@@ -176,6 +176,35 @@ bool BicycleMotionModel::updateStatePoseHead(
   return ekf_.update(Y, C, R);
 }
 
+bool BicycleMotionModel::updateStatePoseVel(
+  const double & x, const double & y, const std::array<double, 36> & pose_cov, const double & vel,
+  const std::array<double, 36> & twist_cov)
+{
+  // check if the state is initialized
+  if (!checkInitialized()) return false;
+
+  // update state, with velocity
+  constexpr int DIM_Y = 4;
+
+  // update state
+  Eigen::MatrixXd Y(DIM_Y, 1);
+  Y << x, y, vel;
+
+  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM);
+  C(0, IDX::X) = 1.0;
+  C(1, IDX::Y) = 1.0;
+  C(2, IDX::VEL) = 1.0;
+
+  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y);
+  R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X];
+  R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y];
+  R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X];
+  R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y];
+  R(2, 2) = twist_cov[XYZRPY_COV_IDX::X_X];
+
+  return ekf_.update(Y, C, R);
+}
+
 bool BicycleMotionModel::updateStatePoseHeadVel(
   const double & x, const double & y, const double & yaw, const std::array<double, 36> & pose_cov,
   const double & vel, const std::array<double, 36> & twist_cov)