feat: add Odometry uncertainty to object tracking

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp

@@ -23,6 +23,7 @@
 #include <autoware/object_recognition_utils/object_recognition_utils.hpp>
 
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 
 namespace autoware::multi_object_tracker
 {
@@ -34,6 +35,7 @@ using autoware::multi_object_tracker::object_model::ObjectModel;
 using autoware_perception_msgs::msg::DetectedObject;
 using autoware_perception_msgs::msg::DetectedObjects;
 using autoware_perception_msgs::msg::ObjectClassification;
+using nav_msgs::msg::Odometry;
 
 ObjectModel decodeObjectModel(const ObjectClassification & object_class);
 
@@ -44,6 +46,7 @@ object_model::StateCovariance covarianceFromObjectClass(
 
 void normalizeUncertainty(DetectedObjects & detected_objects);
 
+void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects);
 }  // namespace uncertainty
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp

@@ -135,5 +135,36 @@ void normalizeUncertainty(DetectedObjects & detected_objects)
   }
 }
 
+void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects)
+{
+  // 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
+    // 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;
+
+
+
+  }
+}
+
 }  // namespace uncertainty
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/package.xml

@@ -21,6 +21,7 @@
   <depend>eigen</depend>
   <depend>glog</depend>
   <depend>mussp</depend>
+  <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>

perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp

@@ -277,6 +277,17 @@ void MultiObjectTracker::runProcess(
   // Model the object uncertainty if it is empty
   DetectedObjects input_objects_with_uncertainty = uncertainty::modelUncertainty(input_objects);
 
+  // Add the odometry uncertainty to the object uncertainty
+  nav_msgs::msg::Odometry odometry;
+  // nav_msgs::msg::Odometry::SharedPtr odometry_msg = std::make_shared<nav_msgs::msg::Odometry>(odometry);
+  {
+    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
+  }
+  uncertainty::addOdometryUncertainty(odometry, input_objects_with_uncertainty);
+
   // Normalize the object uncertainty
   uncertainty::normalizeUncertainty(input_objects_with_uncertainty);