From 8cfffe8f32a0c9f8e90626235397b5fa71dda383 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Thu, 26 Mar 2026 04:28:25 +0800 Subject: [PATCH] reproject force roll and pitch fix line --- .../armor_detector/armor_reproject.hpp | 17 +++++--------- .../src/armor_pose_estimator.cpp | 22 ++++++++++--------- 2 files changed, 17 insertions(+), 22 deletions(-) diff --git a/src/rm_auto_aim/armor_detector/include/armor_detector/armor_reproject.hpp b/src/rm_auto_aim/armor_detector/include/armor_detector/armor_reproject.hpp index cc0670f..1e94d57 100644 --- a/src/rm_auto_aim/armor_detector/include/armor_detector/armor_reproject.hpp +++ b/src/rm_auto_aim/armor_detector/include/armor_detector/armor_reproject.hpp @@ -98,20 +98,13 @@ static inline std::vector reprojectArmorToImage( auto corners_armor = getArmorCornerPoints(is_big); - // Transform corners from armor frame to camera frame - std::vector corners_cam; - for (const auto& p_armor : corners_armor) { - cv::Point3d p_cam; - p_cam.x = rot_mat(0, 0) * p_armor.x + rot_mat(0, 1) * p_armor.y + rot_mat(0, 2) * p_armor.z + tvec[0]; - p_cam.y = rot_mat(1, 0) * p_armor.x + rot_mat(1, 1) * p_armor.y + rot_mat(1, 2) * p_armor.z + tvec[1]; - p_cam.z = rot_mat(2, 0) * p_armor.x + rot_mat(2, 1) * p_armor.y + rot_mat(2, 2) * p_armor.z + tvec[2]; - corners_cam.push_back(p_cam); - } + // Convert rotation matrix to Rodrigues vector + cv::Mat rvec; + cv::Rodrigues(rot_mat, rvec); - // Project to image plane - cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); + // Project to image plane (cv::projectPoints handles both rotation and translation) std::vector corners_2d; - cv::projectPoints(corners_cam, rvec, tvec, K, dist_coeffs, corners_2d); + cv::projectPoints(corners_armor, rvec, tvec, K, dist_coeffs, corners_2d); return corners_2d; } diff --git a/src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp b/src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp index 044dfcc..5032476 100644 --- a/src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp +++ b/src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp @@ -103,24 +103,26 @@ Eigen::Vector3d ArmorPoseEstimator::rotationMatrixToRPY(const Eigen::Matrix3d &R return rpy; } -// Force rotation to have roll=0 and pitch=15 degrees, keeping original yaw +// Force rotation to have roll=0 and pitch=forced_degrees, keeping original yaw +// Uses quaternion to avoid gimbal lock issues with Euler angles Eigen::Matrix3d forceRPY(const Eigen::Matrix3d& R, double forced_roll_deg, double forced_pitch_deg) { - // Extract current RPY + // Extract current RPY using tf2 (standard ZYX convention) Eigen::Quaterniond q(R); tf2::Quaternion tf_q(q.x(), q.y(), q.z(), q.w()); - Eigen::Vector3d rpy; - tf2::Matrix3x3(tf_q).getRPY(rpy[0], rpy[1], rpy[2]); + double r, p, y; + tf2::Matrix3x3(tf_q).getRPY(r, p, y); - // Keep original yaw, force roll and pitch - double yaw = rpy[2]; + // Keep original yaw, force roll=0 and pitch=forced_pitch_deg double roll = forced_roll_deg * M_PI / 180.0; double pitch = forced_pitch_deg * M_PI / 180.0; - // Reconstruct rotation matrix from RPY (ZYX order: yaw, pitch, roll) + // Reconstruct using quaternion to avoid gimbal lock + tf2::Quaternion q_new; + q_new.setRPY(roll, pitch, y); + Eigen::Matrix3d R_new; - R_new = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) - * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); + R_new = Eigen::Matrix3d(q_new); + return R_new; }