fix: transform to world first, add odometry covariance later

perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp

@@ -158,7 +158,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte
   // ego position uncertainty, position covariance + motion covariance
   Eigen::MatrixXd m_cov_ego_pose = Eigen::MatrixXd(2, 2);
   m_cov_ego_pose << odom_pose_cov[0], odom_pose_cov[1], odom_pose_cov[6], odom_pose_cov[7];
-  m_cov_ego_pose = m_cov_ego_pose + m_rot_ego.transpose() * m_cov_motion * m_rot_ego;
+  m_cov_ego_pose = m_cov_ego_pose + m_rot_ego * m_cov_motion * m_rot_ego.transpose();
 
   // ego yaw uncertainty, position covariance + yaw motion covariance
   const double & cov_ego_yaw = odom_pose_cov[35];
@@ -178,7 +178,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte
       object_pose_cov[XYZRPY_COV_IDX::Y_X], object_pose_cov[XYZRPY_COV_IDX::Y_Y];
     const double dx = object_pose.position.x - odom_pose.position.x;
     const double dy = object_pose.position.y - odom_pose.position.y;
-    const double r = std::sqrt(dx * dx + dy * dy);
+    const double r2 = dx * dx + dy * dy;
     const double theta = std::atan2(dy, dx);
 
     // 1-a. add odometry position uncertainty to the object position covariance
@@ -190,7 +190,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte
       // uncertainty is proportional to the distance between the object and the odometry
       // and the uncertainty orientation is vertical to the vector of the odometry position to the
       // object
-      const double cov_by_yaw = cov_ego_yaw * r * r;
+      const double cov_by_yaw = cov_ego_yaw * r2;
       // rotate the covariance matrix, add the yaw uncertainty, and rotate back
       Eigen::MatrixXd m_rot_theta = Eigen::Rotation2D(theta).toRotationMatrix();
       Eigen::MatrixXd m_cov_rot = m_rot_theta.transpose() * m_pose_cov * m_rot_theta;
@@ -219,7 +219,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte
 
     // 2-b. add odometry yaw rate uncertainty to the object linear twist covariance
     {
-      const double cov_by_yaw_rate = cov_yaw_rate * r * r;
+      const double cov_by_yaw_rate = cov_yaw_rate * r2;
       Eigen::MatrixXd m_rot_theta = Eigen::Rotation2D(theta).toRotationMatrix();
       Eigen::MatrixXd m_twist_cov_rot = m_rot_theta.transpose() * m_twist_cov * m_rot_theta;
       m_twist_cov_rot(1, 1) += cov_by_yaw_rate;  // yaw rate uncertainty is added to y-y element

perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp

@@ -275,39 +275,48 @@ void MultiObjectTracker::runProcess(
     return;
   }
 
+  // Transform the objects to the world frame
+  DetectedObjects transformed_objects;
+  if (!autoware::object_recognition_utils::transformObjects(
+        input_objects, world_frame_id_, tf_buffer_, transformed_objects)) {
+    return;
+  }
+
   // the object uncertainty
-  DetectedObjects input_objects_with_uncertainty = input_objects;
   if (enable_odometry_uncertainty_) {
     // Create a modeled odometry message
     nav_msgs::msg::Odometry odometry;
     odometry.header.stamp = measurement_time + rclcpp::Duration::from_seconds(0.001);
-    auto & odom_pose_cov = odometry.pose.covariance;
-    odom_pose_cov[0] = 0.1;     // x-x
-    odom_pose_cov[7] = 0.1;     // y-y
-    odom_pose_cov[35] = 0.001;  // yaw-yaw
 
+    // set odometry pose from self_transform
+    auto & odom_pose = odometry.pose.pose;
+    odom_pose.position.x = self_transform->translation.x;
+    odom_pose.position.y = self_transform->translation.y;
+    odom_pose.position.z = self_transform->translation.z;
+    odom_pose.orientation = self_transform->rotation;
+
+    // set odometry twist
     auto & odom_twist = odometry.twist.twist;
     odom_twist.linear.x = 10.0;  // m/s
     odom_twist.linear.y = 0.1;   // m/s
     odom_twist.angular.z = 0.1;  // rad/s
 
+    // model the uncertainty
+    auto & odom_pose_cov = odometry.pose.covariance;
+    odom_pose_cov[0] = 0.1;     // x-x
+    odom_pose_cov[7] = 0.1;     // y-y
+    odom_pose_cov[35] = 0.0001;  // yaw-yaw
+
     auto & odom_twist_cov = odometry.twist.covariance;
     odom_twist_cov[0] = 2.0;     // x-x [m^2/s^2]
     odom_twist_cov[7] = 0.2;     // y-y [m^2/s^2]
     odom_twist_cov[35] = 0.001;  // yaw-yaw [rad^2/s^2]
 
     // Add the odometry uncertainty to the object uncertainty
-    uncertainty::addOdometryUncertainty(odometry, input_objects_with_uncertainty);
+    uncertainty::addOdometryUncertainty(odometry, transformed_objects);
   }
   // Normalize the object uncertainty
-  uncertainty::normalizeUncertainty(input_objects_with_uncertainty);
-
-  // Transform the objects to the world frame
-  DetectedObjects transformed_objects;
-  if (!autoware::object_recognition_utils::transformObjects(
-        input_objects_with_uncertainty, world_frame_id_, tf_buffer_, transformed_objects)) {
-    return;
-  }
+  uncertainty::normalizeUncertainty(transformed_objects);
 
   /* prediction */
   processor_->predict(measurement_time);