From 5feea4ff177d2fb78a609622f467b6b893d93fa1 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 30 Sep 2024 17:41:40 +0900
Subject: [PATCH] feat: add Odometry uncertainty to object tracking

---
 .../uncertainty/uncertainty_processor.hpp     |  3 ++
 .../lib/uncertainty/uncertainty_processor.cpp | 31 +++++++++++++++++++
 .../autoware_multi_object_tracker/package.xml |  1 +
 .../src/multi_object_tracker_node.cpp         | 11 +++++++
 4 files changed, 46 insertions(+)

diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp
index c67e71b38a6f9..12bd865795b85 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp
+++ b/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
diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp
index 840879f9bd7aa..cd10d9b76ed46 100644
--- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp
+++ b/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
diff --git a/perception/autoware_multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml
index d7250293df28e..62b1f085554b4 100644
--- a/perception/autoware_multi_object_tracker/package.xml
+++ b/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>
diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp
index 24bf4ef9c123d..b5376a2e10539 100644
--- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp
+++ b/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);