From b00b3ec349fd0f421dad45690fa7dd3f590401d9 Mon Sep 17 00:00:00 2001 From: lyf <169361657@qq.com> Date: Sat, 21 Mar 2026 17:21:23 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8DPNP=E8=A7=A3=E7=AE=97?= =?UTF-8?q?=E7=BC=BA=E5=8C=85=E7=9A=84=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/armor_finder/classifier/solver.h | 4 + armor/src/armor_finder/armor_finder.cpp | 11 +- armor/src/armor_finder/classifier/solver.cpp | 101 ++++++++++++++---- .../armor_finder/send_target/send_target.cpp | 4 +- 4 files changed, 93 insertions(+), 27 deletions(-) diff --git a/armor/include/armor_finder/classifier/solver.h b/armor/include/armor_finder/classifier/solver.h index 602af72..3d93949 100644 --- a/armor/include/armor_finder/classifier/solver.h +++ b/armor/include/armor_finder/classifier/solver.h @@ -3,7 +3,10 @@ #include // 必须在opencv2/core/eigen.hpp上面 #include +#include #include +#include +#include #include "armor.hpp" @@ -17,6 +20,7 @@ public: Eigen::Matrix3d R_gimbal2world() const; void set_R_gimbal2world(const Eigen::Quaterniond & q); + void set_R_gimbal2world(double yaw, double pitch, double roll = 0.0); void solve(Armor & armor) const; diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 30ad0ae..f09f3e2 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/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(target_box.id); // This might need mapping + armor.name = static_cast(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) } diff --git a/armor/src/armor_finder/classifier/solver.cpp b/armor/src/armor_finder/classifier/solver.cpp index 58b85c0..8b9934a 100644 --- a/armor/src/armor_finder/classifier/solver.cpp +++ b/armor/src/armor_finder/classifier/solver.cpp @@ -1,14 +1,53 @@ #include "armor_finder/classifier/solver.h" #include - +#include +#include +#include #include +#include +#include +#include +#include +#include -#include "tools/logger.hpp" -#include "tools/math_tools.hpp" +extern std::map 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 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(R_camera2gimbal_data.data()); t_camera2gimbal_ = Eigen::Matrix(t_camera2gimbal_data.data()); */ - + auto camera_matrix_data = yaml["camera_matrix"].as>(); auto distort_coeffs_data = yaml["distort_coeffs"].as>(); Eigen::Matrix 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(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 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); } diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 8ab67b9..f148984 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/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); }