fix(multi_object_tracker): fix measurement dimension (#1156)

Signed-off-by: yutaka <purewater0901@gmail.com>
tier4 · Jul 7, 2022 · b2d2e4f · b2d2e4f
1 parent ac17cb9
commit b2d2e4f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 8 deletions.
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
@@ -241,16 +241,16 @@ bool BigVehicleTracker::measureWithPose(
   if (object.kinematics.has_twist) {
     Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);  // predicted state
     ekf_.getX(X_t);
-    const double current_vx = X_t(IDX::VX);
+    const double predicted_vx = X_t(IDX::VX);
     const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x;
 
-    if (std::fabs(current_vx - observed_vx) > velocity_deviation_threshold_) {
-      // Velocity deviation is large
+    if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) {
+      // Velocity deviation is small
       enable_velocity_measurement = true;
     }
   }
   // pos x, pos y, yaw, vx depending on pose measurement
-  const int dim_y = enable_velocity_measurement ? 3 : 4;
+  const int dim_y = enable_velocity_measurement ? 4 : 3;
   double measurement_yaw = tier4_autoware_utils::normalizeRadian(
     tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation));
   {

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
@@ -241,16 +241,16 @@ bool NormalVehicleTracker::measureWithPose(
   if (object.kinematics.has_twist) {
     Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);  // predicted state
     ekf_.getX(X_t);
-    const double current_vx = X_t(IDX::VX);
+    const double predicted_vx = X_t(IDX::VX);
     const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x;
 
-    if (std::fabs(current_vx - observed_vx) > velocity_deviation_threshold_) {
-      // Velocity deviation is large
+    if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) {
+      // Velocity deviation is small
       enable_velocity_measurement = true;
     }
   }
   // pos x, pos y, yaw, vx depending on pose output
-  const int dim_y = enable_velocity_measurement ? 3 : 4;
+  const int dim_y = enable_velocity_measurement ? 4 : 3;
   double measurement_yaw = tier4_autoware_utils::normalizeRadian(
     tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation));
   {