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 index de901b06e83c3..9a17cad91efb8 100644 --- 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 convertPoseCovarianceMatrix(const Box3D & box3d); -std::array convertTwistCovarianceMatrix(const Box3D & box3d); +std::array 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 index fc383a8d1da00..feeab969e88bd 100644 --- 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 convertPoseCovarianceMatrix(const Box3D & box3d) return pose_covariance; } -std::array convertTwistCovarianceMatrix(const Box3D & box3d) +std::array 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 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 index 555d820ec3644..5605d2df6a9d9 100644 --- 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 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); }