From 677ec582969070cdbae614a6f3b806bf2bf96e8b Mon Sep 17 00:00:00 2001 From: Adam Morrissett Date: Fri, 15 Dec 2023 15:46:55 -0500 Subject: [PATCH] #2215 Use actual covariance matrix The multiple_object_tracker_component was not using incoming detections' covariance matrices. Instead, it was using the zero matrix. The node now uses the detections' covariances. --- .../src/multiple_object_tracker_component.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp index 97e556a835..0905fb2d89 100644 --- a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp +++ b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp @@ -63,7 +63,12 @@ auto make_ctrv_detection( mot::Angle{units::angle::radian_t{yaw}}, units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z}}; - const mot::CtrvStateCovariance covariance = mot::CtrvStateCovariance::Zero(); + mot::CtrvStateCovariance covariance = mot::CtrvStateCovariance::Zero(); + covariance(0, 0) = msg.pose.covariance.at(0); + covariance(1, 1) = msg.pose.covariance.at(7); + covariance(2, 2) = msg.twist.covariance.at(0); + covariance(3, 3) = msg.pose.covariance.at(35); + covariance(4, 4) = msg.twist.covariance.at(35); return mot::CtrvDetection{timestamp, state, covariance, mot::Uuid{msg.id}}; } @@ -96,7 +101,13 @@ auto make_ctra_detection( units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z}, units::acceleration::meters_per_second_squared_t{msg.accel.accel.linear.x}}; - const mot::CtraStateCovariance covariance = mot::CtraStateCovariance::Zero(); + mot::CtraStateCovariance covariance = mot::CtraStateCovariance::Zero(); + covariance(0, 0) = msg.pose.covariance.at(0); + covariance(1, 1) = msg.pose.covariance.at(7); + covariance(2, 2) = msg.twist.covariance.at(0); + covariance(3, 3) = msg.pose.covariance.at(35); + covariance(4, 4) = msg.twist.covariance.at(35); + covariance(5, 5) = msg.accel.covariance.at(0); return mot::CtraDetection{timestamp, state, covariance, mot::Uuid{msg.id}}; }