From 7446feae201a02eaafce57e626a7247e19767a47 Mon Sep 17 00:00:00 2001
From: cyy_mac <cyy@mac.com>
Date: Thu, 26 Mar 2026 04:01:43 +0800
Subject: [PATCH] reproject force roll and pitch

---
 .../src/armor_pose_estimator.cpp              | 24 +++++++++++++++++++
 1 file changed, 24 insertions(+)

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 89e2d02..e0ee969 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
@@ -52,6 +52,9 @@ std::vector<rm_interfaces::msg::Armor> ArmorPoseEstimator::extractArmorPoses(
 
       double armor_roll = rotationMatrixToRPY(R_gimbal_camera_ * R)[0] * 180 / M_PI;
 
+      // Force roll=0, pitch=15 degrees for reprojection consistency
+      R = forceRPY(R, 0.0, 15.0);
+
       if (use_ba_ && std::abs(armor_roll) < 15) {
         // Use BA alogorithm to optimize the pose from PnP
         // solveBa() will modify the rotation_matrix
@@ -97,6 +100,27 @@ Eigen::Vector3d ArmorPoseEstimator::rotationMatrixToRPY(const Eigen::Matrix3d &R
   return rpy;
 }
 
+// Force rotation to have roll=0 and pitch=15 degrees, keeping original yaw
+Eigen::Matrix3d forceRPY(const Eigen::Matrix3d& R, double forced_roll_deg, double forced_pitch_deg) {
+  // Extract current RPY
+  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]);
+
+  // Keep original yaw, force roll and pitch
+  double yaw = rpy[2];
+  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)
+  Eigen::Matrix3d R_new;
+  R_new = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
+        * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
+        * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
+  return R_new;
+}
+
 void ArmorPoseEstimator::sortPnPResult(const Armor &armor,
                                        std::vector<cv::Mat> &rvecs,
                                        std::vector<cv::Mat> &tvecs) const {