feat: Add odometry heading uncertainty to object pose covariance

feat: Rotate object pose covariance matrix to account for yaw uncertainty

Rotate the object pose covariance matrix in the uncertainty_processor.cpp file to account for the yaw uncertainty. This ensures that the covariance matrix accurately represents the position uncertainty of the object.

Refactor the code to rotate the covariance matrix using Eigen's Rotation2D class. The yaw uncertainty is added to the y-y element of the rotated covariance matrix. Finally, update the object_pose_cov array with the updated covariance values.

Closes autowarefoundation#123

perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp

@@ -17,6 +17,9 @@
 
 #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp"
 
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
 namespace autoware::multi_object_tracker
 {
 namespace uncertainty
@@ -137,31 +140,45 @@ void normalizeUncertainty(DetectedObjects & detected_objects)
 
 void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects)
 {
-  // auto & pose = odometry.pose.pose;
+  auto & pose = odometry.pose.pose;
   auto & pose_cov = odometry.pose.covariance;
 
   for (auto & object : detected_objects.objects) {
     auto & object_pose_cov = object.kinematics.pose_with_covariance.covariance;
 
-    // add odometry position uncertainty to the position covariance
-    // add number is not correct. this is for PoC
+    // 1. add odometry position uncertainty to the position covariance
     // object position and it covariance is based on the world frame (map)
     object_pose_cov[XYZRPY_COV_IDX::X_X] += pose_cov[0];       // x-x
     object_pose_cov[XYZRPY_COV_IDX::X_Y] += pose_cov[1];       // x-y
     object_pose_cov[XYZRPY_COV_IDX::Y_X] += pose_cov[6];       // y-x
     object_pose_cov[XYZRPY_COV_IDX::Y_Y] += pose_cov[7];       // y-y
     object_pose_cov[XYZRPY_COV_IDX::YAW_YAW] += pose_cov[35];  // yaw-yaw
 
-    // // add odometry heading uncertainty to the yaw covariance
-    // // 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    
-    // auto & object_pose = object.kinematics.pose_with_covariance.pose;
-    // const double dx = object_pose.position.x - pose.position.x;
-    // const double dy = object_pose.position.y - pose.position.y;
-    // const double r = std::sqrt(dx * dx + dy * dy);
-    // const double cov_yaw = pose_cov[35] * r * r;
-
-
+    // 2. add odometry heading uncertainty to the yaw covariance
+    // 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    
+    auto & object_pose = object.kinematics.pose_with_covariance.pose;
+    const double dx = object_pose.position.x - pose.position.x;
+    const double dy = object_pose.position.y - pose.position.y;
+    const double r = std::sqrt(dx * dx + dy * dy);
+    const double theta = std::atan2(dy, dx);
+    const double cov_yaw = pose_cov[35] * r * r;
+
+    // get the position covariance of the object
+    Eigen::MatrixXd cov = Eigen::MatrixXd(2, 2);
+    cov << pose_cov[0], pose_cov[1], pose_cov[6], pose_cov[7];
+    // rotate the covariance matrix, add the yaw uncertainty, and rotate back
+    Eigen::MatrixXd Rot = Eigen::Rotation2D(theta).toRotationMatrix();
+    Eigen::MatrixXd cov_rot = Rot.transpose() * cov * Rot;
+    cov_rot(1,1) += cov_yaw; // yaw uncertainty is added to y-y element
+    cov = Rot * cov_rot * Rot.transpose();
+    // update the covariance matrix
+    object_pose_cov[XYZRPY_COV_IDX::X_X] = cov(0,0);
+    object_pose_cov[XYZRPY_COV_IDX::X_Y] = cov(0,1);
+    object_pose_cov[XYZRPY_COV_IDX::Y_X] = cov(1,0);
+    object_pose_cov[XYZRPY_COV_IDX::Y_Y] = cov(1,1);
+
+    // 3. add odometry velocity uncertainty to the velocity covariance
 
   }
 }

perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp

@@ -284,7 +284,7 @@ void MultiObjectTracker::runProcess(
     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.1;  // yaw-yaw
+    odom_pose_cov[35] = 0.001;  // yaw-yaw
   }
   uncertainty::addOdometryUncertainty(odometry, input_objects_with_uncertainty);