fix(autoware_lidar_centerpoint): fix twist covariance orientation (#8996

) * fix(autoware_lidar_centerpoint): fix covariance converter considering the twist covariance matrix is based on the object coordinate Signed-off-by: Taekjin LEE <[email protected]> fix style * fix: update test of box3DToDetectedObject function Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]>
autowarefoundation · Oct 1, 2024 · 6a24683 · 6a24683
1 parent a2446ca
commit 6a24683
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 9 deletions.
diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp
@@ -35,7 +35,7 @@ uint8_t getSemanticType(const std::string & class_name);
 
 std::array<double, 36> convertPoseCovarianceMatrix(const Box3D & box3d);
 
-std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d);
+std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw);
 
 bool isCarLikeVehicleLabel(const uint8_t label);
 

diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp
@@ -50,7 +50,7 @@ void box3DToDetectedObject(
 
   // pose and shape
   // mmdet3d yaw format to ros yaw format
-  float yaw = -box3d.yaw - autoware::universe_utils::pi / 2;
+  const float yaw = -box3d.yaw - autoware::universe_utils::pi / 2;
   obj.kinematics.pose_with_covariance.pose.position =
     autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z);
   obj.kinematics.pose_with_covariance.pose.orientation =
@@ -67,6 +67,8 @@ void box3DToDetectedObject(
   if (has_twist) {
     float vel_x = box3d.vel_x;
     float vel_y = box3d.vel_y;
+
+    // twist of the object is based on the object coordinate system
     geometry_msgs::msg::Twist twist;
     twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y;
     twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y;
@@ -76,7 +78,7 @@ void box3DToDetectedObject(
     obj.kinematics.has_twist = has_twist;
     if (has_variance) {
       obj.kinematics.has_twist_covariance = has_variance;
-      obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d);
+      obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d, yaw);
     }
   }
 }
@@ -113,12 +115,21 @@ std::array<double, 36> convertPoseCovarianceMatrix(const Box3D & box3d)
   return pose_covariance;
 }
 
-std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d)
+std::array<double, 36> convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw)
 {
   using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+
+  // twist covariance matrix is based on the object coordinate system
   std::array<double, 36> twist_covariance{};
-  twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance;
-  twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance;
+  const float cos_yaw = std::cos(yaw);
+  const float sin_yaw = std::sin(yaw);
+  twist_covariance[POSE_IDX::X_X] =
+    box3d.vel_x_variance * cos_yaw * cos_yaw + box3d.vel_y_variance * sin_yaw * sin_yaw;
+  twist_covariance[POSE_IDX::X_Y] =
+    (box3d.vel_y_variance - box3d.vel_x_variance) * sin_yaw * cos_yaw;
+  twist_covariance[POSE_IDX::Y_X] = twist_covariance[POSE_IDX::X_Y];
+  twist_covariance[POSE_IDX::Y_Y] =
+    box3d.vel_x_variance * sin_yaw * sin_yaw + box3d.vel_y_variance * cos_yaw * cos_yaw;
   return twist_covariance;
 }
 

diff --git a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp
@@ -125,13 +125,14 @@ TEST(TestSuite, convertPoseCovarianceMatrix)
 TEST(TestSuite, convertTwistCovarianceMatrix)
 {
   autoware::lidar_centerpoint::Box3D box3d;
-  box3d.vel_x_variance = 0.1;
+  box3d.vel_x_variance = 0.5;
   box3d.vel_y_variance = 0.2;
+  float yaw = 0;
 
   std::array<double, 36> twist_covariance =
-    autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d);
+    autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d, yaw);
 
-  EXPECT_FLOAT_EQ(twist_covariance[0], 0.1);
+  EXPECT_FLOAT_EQ(twist_covariance[0], 0.5);
   EXPECT_FLOAT_EQ(twist_covariance[7], 0.2);
 }