Li Da
52301bcf8d
find_armor_box.cpp: 废弃了单一块板子的限制，将同一分类的板子全拿来算出包含自转幅度的巨型像素框正中点。
armor_finder.cpp: 让这辆车当前所有能看见的板子全部去跑 PnP 获得深度，取物理上离你最近的深度 $Z$，结合上一步的框中心点，逆推换算出一个堪称完美的 3D 虚拟准星点。
send_target.cpp: 彻底移除了容易抽搐的差值 PID 算法。现在直接根据虚拟准星算出最纯净的绝对偏航角（Yaw）和下坠角（Pitch）发给下位机，云台将彻底锁死！
auto_trigger.h: 抛弃容易受抖动破坏的三维测速。改用极其丝滑的二维图像像素帧差分测速：计算某一块板子滑到中心线刚好等于引力滞空飞行的时间，完美触发一击绝杀。
⚠️ 终极提醒（切记！切记！）： 算法已经全部就位，但是这套神算法的基础基石——物理高度落差需要你手动填入。 请务必拿尺子去量一下车上摄像头和发弹孔的落差，修改 others/solver_config.yml 中的： t_camera2gimbal: [0, 0, 0] 为真实厘米数（例如 [0, -10, 5]）。
53 lines
1.5 KiB
C++
53 lines
1.5 KiB
C++
#ifndef AUTO_AIM__SOLVER_HPP
|
|
#define AUTO_AIM__SOLVER_HPP
|
|
|
|
#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"
|
|
|
|
namespace auto_aim
|
|
{
|
|
class Solver
|
|
{
|
|
public:
|
|
explicit Solver(const std::string & config_path);
|
|
|
|
const cv::Mat& get_camera_matrix() const { return camera_matrix_; }
|
|
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;
|
|
|
|
std::vector<cv::Point2f> reproject_armor(
|
|
const Eigen::Vector3d & xyz_in_world, double yaw, ArmorType type, ArmorName name) const;
|
|
|
|
double oupost_reprojection_error(Armor armor, const double & picth);
|
|
|
|
std::vector<cv::Point2f> world2pixel(const std::vector<cv::Point3f> & worldPoints);
|
|
|
|
private:
|
|
cv::Mat camera_matrix_;
|
|
cv::Mat distort_coeffs_;
|
|
Eigen::Matrix3d R_gimbal2imubody_;
|
|
Eigen::Matrix3d R_camera2gimbal_;
|
|
Eigen::Vector3d t_camera2gimbal_;
|
|
Eigen::Matrix3d R_gimbal2world_;
|
|
|
|
void optimize_yaw(Armor & armor) const;
|
|
|
|
double armor_reprojection_error(const Armor & armor, double yaw, const double & inclined) const;
|
|
double SJTU_cost(
|
|
const std::vector<cv::Point2f> & cv_refs, const std::vector<cv::Point2f> & cv_pts,
|
|
const double & inclined) const;
|
|
};
|
|
|
|
} // namespace auto_aim
|
|
|
|
#endif // AUTO_AIM__SOLVER_HPP
|