reproject force roll and pitch

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 {