修复PNP解算缺包的问题

armor/src/armor_finder/armor_finder.cpp

@@ -118,11 +118,11 @@ end:
     if (target_box.rect != cv::Rect2d()) {
         auto_aim::Armor armor;
         armor.type = (target_box.id == B1 || target_box.id == R1 || target_box.id == B2 || target_box.id == R2 || target_box.id == B7 || target_box.id == R7 || target_box.id == B8 || target_box.id == R8) ? auto_aim::ArmorType::big : auto_aim::ArmorType::small;
-        armor.name = static_cast<auto_aim::ArmorName>(target_box.id); // This might need mapping
+        armor.name = static_cast<auto_aim::ArmorName>(target_box.id);
         armor.points = target_box.points;
         
-        // 获取当前云台位姿 (假设 mcu_data 有 q)
-        // solver.set_R_gimbal2world(Eigen::Quaterniond(mcu_data.q[0], mcu_data.q[1], mcu_data.q[2], mcu_data.q[3]));
+        // 更新云台位姿 (使用 MCU 回传的 yaw/pitch，单位假设为度，需转为弧度)
+        solver.set_R_gimbal2world(mcu_data.curr_yaw * CV_PI / 180.0, mcu_data.curr_pitch * CV_PI / 180.0);
         
         solver.solve(armor);
         
@@ -139,10 +139,7 @@ end:
 }
 
 cv::Point3f ArmorFinder::getTarget3D(const ArmorBox &box) {
-    double z = box.getBoxDistance(); // 单位: cm
-    double x = z * (box.getCenter().x - IMAGE_CENTER_X) / FOCUS_PIXAL;
-    double y = z * (box.getCenter().y - IMAGE_CENTER_Y) / FOCUS_PIXAL;
-    return cv::Point3f(x, y, z);
+    return target_xyz; // 直接返回 PnP 计算出的结果 (cm)
 }
 
 

armor/src/armor_finder/classifier/solver.cpp

@@ -1,14 +1,53 @@
 #include "armor_finder/classifier/solver.h"
 
 #include <yaml-cpp/yaml.h>
-
+#include <log.h>
+#include <map>
+#include <string>
 #include <vector>
+#include <cmath>
+#include <algorithm>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
 
-#include "tools/logger.hpp"
-#include "tools/math_tools.hpp"
+extern std::map<int, std::string> id2name;
 
 namespace auto_aim
 {
+// ──────────────────────────────────────────────────────────────────────────────
+// Math Helpers (Internalized from tools/math_tools.hpp)
+// ──────────────────────────────────────────────────────────────────────────────
+inline double square(double x) { return x * x; }
+
+inline double limit_rad(double angle)
+{
+  constexpr double two_pi = 2.0 * CV_PI;
+  angle = std::fmod(angle + CV_PI, two_pi);
+  if (angle < 0.0) angle += two_pi;
+  return angle - CV_PI;
+}
+
+inline Eigen::Vector3d eulers(const Eigen::Matrix3d & R, int ax0, int ax1, int ax2)
+{
+  return R.eulerAngles(ax0, ax1, ax2);
+}
+
+inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz)
+{
+  double distance = xyz.norm();
+  double yaw = std::atan2(xyz.y(), xyz.x());
+  double pitch = std::atan2(xyz.z(), std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()));
+  return Eigen::Vector3d(yaw, pitch, distance);
+}
+
+inline double get_abs_angle(const Eigen::Vector2d & a, const Eigen::Vector2d & b)
+{
+  double cos_angle = a.normalized().dot(b.normalized());
+  cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
+  return std::acos(cos_angle);
+}
+
 constexpr double LIGHTBAR_LENGTH = 56e-3;     // m
 constexpr double BIG_ARMOR_WIDTH = 230e-3;    // m
 constexpr double SMALL_ARMOR_WIDTH = 135e-3;  // m
@@ -24,7 +63,11 @@ const std::vector<cv::Point3f> SMALL_ARMOR_POINTS{
   {0, -SMALL_ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2},
   {0, SMALL_ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2}};
 
-Solver::Solver(const std::string & config_path) : R_gimbal2world_(Eigen::Matrix3d::Identity())
+Solver::Solver(const std::string & config_path)
+: R_gimbal2imubody_(Eigen::Matrix3d::Identity()),
+  R_camera2gimbal_(Eigen::Matrix3d::Identity()),
+  t_camera2gimbal_(Eigen::Vector3d::Zero()),
+  R_gimbal2world_(Eigen::Matrix3d::Identity())
 {
   auto yaml = YAML::LoadFile(config_path);
 
@@ -36,7 +79,7 @@ Solver::Solver(const std::string & config_path) : R_gimbal2world_(Eigen::Matrix3
   R_camera2gimbal_ = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(R_camera2gimbal_data.data());
   t_camera2gimbal_ = Eigen::Matrix<double, 3, 1>(t_camera2gimbal_data.data());
   */
-
+ 
   auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
   auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
   Eigen::Matrix<double, 3, 3, Eigen::RowMajor> camera_matrix(camera_matrix_data.data());
@@ -53,6 +96,15 @@ void Solver::set_R_gimbal2world(const Eigen::Quaterniond & q)
   R_gimbal2world_ = R_gimbal2imubody_.transpose() * R_imubody2imuabs * R_gimbal2imubody_;
 }
 
+void Solver::set_R_gimbal2world(double yaw, double pitch, double roll)
+{
+  Eigen::Quaterniond q =
+    Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
+    Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitY()) *
+    Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
+  set_R_gimbal2world(q);
+}
+
 //solvePnP（获得姿态）
 void Solver::solve(Armor & armor) const
 {
@@ -60,10 +112,15 @@ void Solver::solve(Armor & armor) const
     (armor.type == ArmorType::big) ? BIG_ARMOR_POINTS : SMALL_ARMOR_POINTS;
 
   cv::Vec3d rvec, tvec;
-  cv::solvePnP(
+  bool success = cv::solvePnP(
     object_points, armor.points, camera_matrix_, distort_coeffs_, rvec, tvec, false,
     cv::SOLVEPNP_IPPE);
 
+  if (!success) {
+    LOGW("solvePnP failed for armor!");
+    return;
+  }
+
   Eigen::Vector3d xyz_in_camera;
   cv::cv2eigen(tvec, xyz_in_camera);
   armor.xyz_in_gimbal = R_camera2gimbal_ * xyz_in_camera + t_camera2gimbal_;
@@ -75,10 +132,10 @@ void Solver::solve(Armor & armor) const
   cv::cv2eigen(rmat, R_armor2camera);
   Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera;
   Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal;
-  armor.ypr_in_gimbal = tools::eulers(R_armor2gimbal, 2, 1, 0);
-  armor.ypr_in_world = tools::eulers(R_armor2world, 2, 1, 0);
+  armor.ypr_in_gimbal = eulers(R_armor2gimbal, 2, 1, 0);
+  armor.ypr_in_world = eulers(R_armor2world, 2, 1, 0);
 
-  armor.ypd_in_world = tools::xyz2ypd(armor.xyz_in_world);
+  armor.ypd_in_world = xyz2ypd(armor.xyz_in_world);
 
   // 平衡不做yaw优化，因为pitch假设不成立
   auto is_balance = (armor.type == ArmorType::big) &&
@@ -87,6 +144,14 @@ void Solver::solve(Armor & armor) const
   if (is_balance) return;
 
   optimize_yaw(armor);
+
+  LOGM(
+    "Armor: %s, pnp_xyz: (%.3f, %.3f, %.3f), world_xyz: (%.3f, %.3f, %.3f), ypd: (%.1f, %.1f, "
+    "%.3f)",
+    id2name[static_cast<int>(armor.name)].c_str(), armor.xyz_in_gimbal.x(), armor.xyz_in_gimbal.y(),
+    armor.xyz_in_gimbal.z(), armor.xyz_in_world.x(), armor.xyz_in_world.y(), armor.xyz_in_world.z(),
+    armor.ypd_in_world.x() * 180.0 / CV_PI, armor.ypd_in_world.y() * 180.0 / CV_PI,
+    armor.ypd_in_world.z());
 }
 
 std::vector<cv::Point2f> Solver::reproject_armor(
@@ -150,10 +215,10 @@ double Solver::oupost_reprojection_error(Armor armor, const double & pitch)
   cv::cv2eigen(rmat, R_armor2camera);
   Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera;
   Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal;
-  armor.ypr_in_gimbal = tools::eulers(R_armor2gimbal, 2, 1, 0);
-  armor.ypr_in_world = tools::eulers(R_armor2world, 2, 1, 0);
+  armor.ypr_in_gimbal = eulers(R_armor2gimbal, 2, 1, 0);
+  armor.ypr_in_world = eulers(R_armor2world, 2, 1, 0);
 
-  armor.ypd_in_world = tools::xyz2ypd(armor.xyz_in_world);
+  armor.ypd_in_world = xyz2ypd(armor.xyz_in_world);
 
   auto yaw = armor.ypr_in_world[0];
   auto xyz_in_world = armor.xyz_in_world;
@@ -197,16 +262,16 @@ double Solver::oupost_reprojection_error(Armor armor, const double & pitch)
 
 void Solver::optimize_yaw(Armor & armor) const
 {
-  Eigen::Vector3d gimbal_ypr = tools::eulers(R_gimbal2world_, 2, 1, 0);
+  Eigen::Vector3d gimbal_ypr = eulers(R_gimbal2world_, 2, 1, 0);
 
   constexpr double SEARCH_RANGE = 140;  // degree
-  auto yaw0 = tools::limit_rad(gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0);
+  auto yaw0 = limit_rad(gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0);
 
   auto min_error = 1e10;
   auto best_yaw = armor.ypr_in_world[0];
 
   for (int i = 0; i < SEARCH_RANGE; i++) {
-    double yaw = tools::limit_rad(yaw0 + i * CV_PI / 180.0);
+    double yaw = limit_rad(yaw0 + i * CV_PI / 180.0);
     auto error = armor_reprojection_error(armor, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0);
 
     if (error < min_error) {
@@ -241,12 +306,12 @@ double Solver::SJTU_cost(
       (0.5 * ((refs[i] - pts[i]).norm() + (refs[p] - pts[p]).norm()) +
        std::fabs(ref_d.norm() - pt_d.norm())) /
       ref_d.norm();
-    double angular_dis = ref_d.norm() * tools::get_abs_angle(ref_d, pt_d) / ref_d.norm();
+    double angular_dis = ref_d.norm() * get_abs_angle(ref_d, pt_d) / ref_d.norm();
     // 平方可能是为了配合 sin 和 cos
     // 弧度差代价（0 度左右占比应该大）
     double cost_i =
-      tools::square(pixel_dis * std::sin(inclined)) +
-      tools::square(angular_dis * std::cos(inclined)) * 2.0;  // DETECTOR_ERROR_PIXEL_BY_SLOPE
+      square(pixel_dis * std::sin(inclined)) +
+      square(angular_dis * std::cos(inclined)) * 2.0;  // DETECTOR_ERROR_PIXEL_BY_SLOPE
     // 重投影像素误差越大，越相信斜率
     cost += std::sqrt(cost_i);
   }

armor/src/armor_finder/send_target/send_target.cpp

@@ -80,8 +80,8 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     double y_target = -target_xyz.y / 100.0; // 垂直高度差
     double pitch_comp = BallisticSolver::get_pitch(x_target, y_target, MUZZLE_VELOCITY, BALLISTIC_K);
 
-    // 计算是否满足开火条件
-    can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp);
+    // 计算是否满足开火条件 (例如残差小于 1.5 度)
+    can_fire = AutoTrigger::should_fire(yaw, pitch_comp, 1.5);
 
     return sendTarget(serial, yaw, pitch_comp, dist * 100.0, shoot_delay);
 }