VSAG/vision_hero
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修复PNP解算缺包的问题

Browse Source
This commit is contained in:
Luo Yufei
2026-03-21 17:21:23 +08:00
parent caa28299e0
commit b00b3ec349
4 changed files with 93 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
armor/include/armor_finder/classifier/solver.h
View File

@@ -3,7 +3,10 @@
#include <Eigen/Dense> // 必须在opencv2/core/eigen.hpp上面
#include <Eigen/Geometry>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <string>
#include <vector>
#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;

11
armor/src/armor_finder/armor_finder.cpp
View File

@@ -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)
}

99
armor/src/armor_finder/classifier/solver.cpp
View File

@@ -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);
@@ -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);
}

4
armor/src/armor_finder/send_target/send_target.cpp
View File

@@ -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);
}