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

solver.h & armor_finder.h: 新增了提取相机内参的接口，以及存储 2D 大框中心点 (target_area_center) 和过滤后的绝杀深度 (z_min_filtered)。

Browse Source 
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]）。
This commit is contained in:
Liu Lida
2026-03-21 20:03:47 +08:00
parent 1588d242cd
commit 52301bcf8d
6 changed files with 87 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
armor/include/armor_finder/armor_finder.h
View File

@@ -110,6 +110,9 @@ private:
const uint8_t &is_anti_top; // 进入反陀螺，引用外部变量，自动变化
State state; // 自瞄状态对象实例
ArmorBox target_box, last_box; // 目标装甲板
cv::Point2f target_area_center; // 目标大框像素中心
double z_min_filtered = 0; // 目标深度极小值
ArmorBoxes current_target_boxes; // 聚类后的同一车体装甲板
int anti_switch_cnt; // 防止乱切目标计数器
cv::Ptr<cv::Tracker> tracker; // tracker对象实例
Classifier classifier; // CNN分类器对象实例，用于数字识别

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

@@ -17,6 +17,7 @@ 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);

56
armor/include/show_images/auto_trigger.h
View File

@@ -21,40 +21,32 @@ public:
* @return true 目标与预测弹着点重合，建议击发
*/
static bool should_fire(const ArmorFinder &armor_finder, double v0, double gimbal_yaw, double gimbal_pitch, double threshold_deg = -1.0) {
if (armor_finder.history.size() < 3) return false; // 轨迹点不足，不预测
if (armor_finder.last_box.rect == cv::Rect2d()) return false;
cv::Point2f curr_center = armor_finder.target_box.getCenter();
cv::Point2f last_center = armor_finder.last_box.getCenter();
cv::Point2f area_center = armor_finder.target_area_center;
// 1. 获取目标当前的 3D 状态 (相对于相机)
const auto &latest = armor_finder.history.back();
double dist = latest.pos.z / 100.0; // cm -> m
// 2. 估计目标速度 (简单线性回归或两点差分)
const auto &first = armor_finder.history.front();
double dt = latest.time - first.time;
if (dt <= 0) return false;
cv::Point3f velocity = (latest.pos - first.pos) * (1.0 / dt); // cm/s
// 3. 计算飞行时间 (单位: s)
// 横向速度估计 (采用帧差分，工业相机一般 ~100 fps)
double vx = curr_center.x - last_center.x;
if (std::abs(vx) < 1.0) return false;
// 计算这块装甲板滑到大框中心所需的预测帧数
double frames_to_pass = (area_center.x - curr_center.x) / vx;
// 若不是朝着中心滑动 (已偏离)，不激发
if (frames_to_pass < 0) return false;
// 弹道时间预判
double dist = armor_finder.z_min_filtered / 100.0; // cm -> m
double t_flight = BallisticSolver::get_flight_time(dist, -gimbal_pitch, v0, BALLISTIC_K);
double t_total = t_flight + SYSTEM_DELAY / 1000.0;
// 4. 预测目标未来的位置 (cm)
cv::Point3f pos_pred = latest.pos + velocity * t_total;
// 5. 将预测位置映射回像素坐标 (用于判断重合)
double x_pred_pixel = pos_pred.x * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_X;
double y_pred_pixel = pos_pred.y * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_Y;
// 6. 推断弹着点偏移 (像素级)
// 理想打击点应该就在相机中心 (自瞄已经对准补差后的位置)
// 但是我们需要判断当前云台的 "指向误差" 是否足够小
double dist_to_center = sqrt(pow(x_pred_pixel - IMAGE_CENTER_X, 2) + pow(y_pred_pixel - IMAGE_CENTER_Y, 2));
// 7. 动态阈值判定 (依据目标距离和装甲板大小)
// 距离越远，允许的像素误差越小
double threshold = (threshold_deg > 0) ? (threshold_deg * FOCUS_PIXAL * PI / 180.0 / 100.0) : (200.0 / (dist + 0.001));
return dist_to_center < threshold;
double t_total = t_flight + SYSTEM_DELAY / 1000.0;
// 将总延迟时间换算为帧数偏移 (以 100FPS)
double delay_frames = t_total * 100.0;
// 如果剩余到达中心时间 = 飞弹+机械延迟时间 左右，开火！
return std::abs(frames_to_pass - delay_frames) < 3.0; // 容差 +/- 3帧
}
};