From 2e0bfa767d88a44e446f0e7400750e2a8ed2f276 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 3 Oct 2024 10:20:21 +0900 Subject: [PATCH] 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 #123 --- .../lib/uncertainty/uncertainty_processor.cpp | 43 +++++++++++++------ .../src/multi_object_tracker_node.cpp | 2 +- 2 files changed, 31 insertions(+), 14 deletions(-) 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 3ab6b548f3bf6..b8134d841a696 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -17,6 +17,9 @@ #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" +#include +#include + namespace autoware::multi_object_tracker { namespace uncertainty @@ -137,14 +140,13 @@ 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 @@ -152,16 +154,31 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte 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 } } 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 5eb45bcb60927..f72697b7e245c 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 @@ -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);