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

对上交代码进行修改，主要将能量机关去掉，添加了同济的PnP位姿解算，但是同济有个四元数，获取IMU部分没有启用，可能导致精度不够。当前还存在反陀螺功能，修改为逻辑和弹道预测相结合，主要在时间关系上进行调整。

Browse Source
This commit is contained in:
Luo Yufei
2026-03-21 11:57:34 +08:00
commit 56985997ae
80 changed files with 60253 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
armor/include/armor_finder/armor_finder.h Normal file
View File

@@ -0,0 +1,155 @@
//
// Created by xinyang on 19-3-27.
//
#ifndef _ARMOR_FINDER_H_
#define _ARMOR_FINDER_H_
#include <map>
#include <systime.h>
#include <constants.h>
#include <opencv2/core.hpp>
#include <opencv2/tracking/tracking.hpp>
#include <serial.h>
#include <armor_finder/classifier/solver.h>
#include <additions.h>
#define BLOB_RED ENEMY_RED
#define BLOB_BLUE ENEMY_BLUE
#define BOX_RED ENEMY_RED
#define BOX_BLUE ENEMY_BLUE
#define IMAGE_CENTER_X (640)
#define IMAGE_CENTER_Y (512-20)
#define DISTANCE_HEIGHT_5MM (10700.0) // 单位: cm*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
#define B1 1
#define B2 2
#define B3 3
#define B4 4
#define B5 5
#define B7 6
#define B8 7
#define R1 8
#define R2 9
#define R3 10
#define R4 11
#define R5 12
#define R7 13
#define R8 14
extern std::map<int, string> id2name; //装甲板id到名称的map
extern std::map<string, int> name2id; //装甲板名称到id的map
extern std::map<string, int> prior_blue;
extern std::map<string, int> prior_red;
/******************* 灯条类定义 ***********************/
class LightBlob {
public:
cv::RotatedRect rect; //灯条位置
double area_ratio;
double length; //灯条长度
uint8_t blob_color; //灯条颜色
LightBlob(cv::RotatedRect &r, double ratio, uint8_t color) : rect(r), area_ratio(ratio), blob_color(color) {
length = max(rect.size.height, rect.size.width);
};
LightBlob() = default;
};
typedef std::vector<LightBlob> LightBlobs;
/******************* 装甲板类定义　**********************/
class ArmorBox{
public:
typedef enum{
FRONT, SIDE, UNKNOWN
} BoxOrientation;
cv::Rect2d rect;
LightBlobs light_blobs;
uint8_t box_color;
int id;
std::vector<cv::Point2f> points; // 4 corner points for PnP
explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0, const std::vector<cv::Point2f> &pts = {});
cv::Point2f getCenter() const; // 获取装甲板中心
double getBlobsDistance() const; // 获取两个灯条中心间距
double lengthDistanceRatio() const; // 获取灯条中心距和灯条长度的比值
double getBoxDistance() const; // 获取装甲板到摄像头的距离
BoxOrientation getOrientation() const; // 获取装甲板朝向(误差较大，已弃用)
bool operator<(const ArmorBox &box) const; // 装甲板优先级比较
};
typedef std::vector<ArmorBox> ArmorBoxes;
/********************* 自瞄类定义 **********************/
class ArmorFinder{
public:
ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder, const uint8_t &anti_top);
~ArmorFinder() = default;
private:
typedef cv::TrackerKCF TrackerToUse; // Tracker类型定义
typedef enum{
SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
} State; // 自瞄状态枚举定义
systime frame_time; // 当前帧对应时间
const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化
const uint8_t &is_anti_top; // 进入反陀螺，引用外部变量，自动变化
State state; // 自瞄状态对象实例
ArmorBox target_box, last_box; // 目标装甲板
int anti_switch_cnt; // 防止乱切目标计数器
cv::Ptr<cv::Tracker> tracker; // tracker对象实例
Classifier classifier; // CNN分类器对象实例，用于数字识别
int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
int tracking_cnt; // 记录追踪帧数，用于定时退出追踪
Serial &serial; // 串口对象
auto_aim::Solver solver; // PnP求解器
systime last_front_time; // 上次陀螺正对时间
int anti_top_cnt;
RoundQueue<double, 4> top_periodms; // 陀螺周期循环队列
vector<systime> time_seq; // 一个周期内的时间采样点
vector<float> angle_seq; // 一个周期内的角度采样点
float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
float last_yaw, last_pitch;//PID中微分项
float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
bool findArmorBox(const cv::Mat &src, ArmorBox &box);
bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes);
bool stateSearchingTarget(cv::Mat &src); // searching state主函数
bool stateTrackingTarget(cv::Mat &src); // tracking state主函数
bool stateStandBy(); // stand by state主函数（已弃用）
void antiTop(double dist_m, double pitch_imu_deg); // 反小陀螺
cv::Point3f getTarget3D(const ArmorBox &box); // 获取目标的3D坐标 (相对于相机)
bool sendBoxPosition(uint16_t shoot_delay); // 发送装甲板位置
bool shouldFire() const { return can_fire; } // 获取开火建议
public:
void run(cv::Mat &src); // 自瞄主函数
struct HistoryItem {
cv::Point3f pos;
double time;
};
std::vector<HistoryItem> history; // 历史位置缓存，用于速度预测
cv::Point3f target_xyz; // PnP计算出的目标3D坐标 (cm)
bool can_fire; // 是否满足开启射击条件
};
#endif /* _ARMOR_FINDER_H_ */

27
armor/include/armor_finder/classifier/armor.hpp Normal file
View File

@@ -0,0 +1,27 @@
#ifndef AUTO_AIM__ARMOR_HPP
#define AUTO_AIM__ARMOR_HPP
#include <opencv2/core.hpp>
#include <Eigen/Dense>
#include <vector>
namespace auto_aim
{
enum class ArmorType { small, big };
enum class ArmorName { NO, B1, B2, B3, B4, B5, B7, B8, R1, R2, R3, R4, R5, R7, R8, outpost };
struct Armor {
ArmorType type;
ArmorName name;
std::vector<cv::Point2f> points;
Eigen::Vector3d xyz_in_gimbal;
Eigen::Vector3d xyz_in_world;
Eigen::Vector3d ypr_in_gimbal;
Eigen::Vector3d ypr_in_world;
Eigen::Vector3d ypd_in_world;
double yaw_raw;
};
} // namespace auto_aim
#endif // AUTO_AIM__ARMOR_HPP

58
armor/include/armor_finder/classifier/classifier.h Normal file
View File

@@ -0,0 +1,58 @@
//
// Created by xinyang on 19-4-19.
//
// 为了一时方便，使用循环和Eigen自行编写的CNN前向传播类。
// 没有显著的性能损失。
// 但类定义了网络结构，同时实现的操作较少，可扩展性较差
#ifndef _CLASSIFIER_H_
#define _CLASSIFIER_H_
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <vector>
#include <string>
#include <opencv2/core.hpp>
using namespace std;
using namespace Eigen;
class Classifier {
private:
bool state; // 标志分类器是否正确初始化
// 所有网络参数
vector<vector<MatrixXd>> conv1_w, conv2_w, conv3_w;
vector<double> conv1_b, conv2_b, conv3_b;
MatrixXd fc1_w, fc2_w;
VectorXd fc1_b, fc2_b;
// 读取网络参数的函数
vector<vector<MatrixXd>> load_conv_w(const string &file);
vector<double> load_conv_b(const string &file);
MatrixXd load_fc_w(const string &file);
VectorXd load_fc_b(const string &file);
// 目前支持的所有操作
MatrixXd softmax(const MatrixXd &input);
MatrixXd relu(const MatrixXd &input);
MatrixXd leaky_relu(const MatrixXd &input, float alpha);
vector<vector<MatrixXd>> apply_bias(const vector<vector<MatrixXd>> &input, const vector<double> &bias);
vector<vector<MatrixXd>> relu(const vector<vector<MatrixXd>> &input);
vector<vector<MatrixXd>> leaky_relu(const vector<vector<MatrixXd>> &input, float alpha);
vector<vector<MatrixXd>> max_pool(const vector<vector<MatrixXd>> &input, int size);
vector<vector<MatrixXd>> mean_pool(const vector<vector<MatrixXd>> &input, int size);
vector<vector<MatrixXd>> pand(const vector<vector<MatrixXd>> &input, int val);
MatrixXd conv(const MatrixXd &filter, const MatrixXd &input);
vector<vector<MatrixXd>> conv2(const vector<vector<MatrixXd>> &filter, const vector<vector<MatrixXd>> &input);
MatrixXd flatten(const vector<vector<MatrixXd>> &input);
public:
explicit Classifier(const string &folder);
~Classifier() = default;
MatrixXd calculate(const vector<vector<MatrixXd>> &input);
explicit operator bool() const;
int operator()(const cv::Mat &image);
};
#endif /* _CLASSIFIER_H */

48
armor/include/armor_finder/classifier/solver.h Normal file
View File

@@ -0,0 +1,48 @@
#ifndef AUTO_AIM__SOLVER_HPP
#define AUTO_AIM__SOLVER_HPP
#include <Eigen/Dense> // 必须在opencv2/core/eigen.hpp上面
#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>
#include "armor.hpp"
namespace auto_aim
{
class Solver
{
public:
explicit Solver(const std::string & config_path);
Eigen::Matrix3d R_gimbal2world() const;
void set_R_gimbal2world(const Eigen::Quaterniond & q);
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

61
armor/include/show_images/auto_trigger.h Normal file
View File

@@ -0,0 +1,61 @@
#ifndef AUTO_TRIGGER_H
#define AUTO_TRIGGER_H
#include <armor_finder/armor_finder.h>
#include <show_images/ballistic_predicition.h>
#include <config/setconfig.h>
#include <vector>
#include <numeric>
/**
* @brief 自动扳机逻辑类，负责发射时机决策。
*/
class AutoTrigger {
public:
/**
* @brief 自动扳机决策函数
* @param armor_finder 当前自瞄主对象 (用于获取历史轨迹)
* @param v0 弹丸初速度
* @param gimbal_yaw 当前云台实时自瞄角偏移 (Yaw, 度)
* @param gimbal_pitch 当前云台实时自瞄角偏移 (Pitch, 度)
* @return true 目标与预测弹着点重合，建议击发
*/
static bool should_fire(const ArmorFinder &armor_finder, double v0, double gimbal_yaw, double gimbal_pitch) {
if (armor_finder.history.size() < 3) return false; // 轨迹点不足，不预测
// 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)
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 = 200.0 / (dist + 0.001); // 示例阈值：5米处允许40像素误差
return dist_to_center < threshold;
}
};
#endif // AUTO_TRIGGER_H

97
armor/include/show_images/ballistic_predicition.h Normal file
View File

@@ -0,0 +1,97 @@
/*
*** 此文件用于hero弹道着落点位，通过3D建模以及理论发弹速度以及后期轨道修正手段 ***
*** 实现对敌方装甲板的精准打击 ***
*/
#ifndef BALLISTIC_PREDICITION_H
#define BALLISTIC_PREDICITION_H
#include <Eigen/Dense>
#include <opencv2/core.hpp>
/**
* 弹着点计算结果
*/
struct ImpactPoint {
Eigen::Vector3d xyz_in_camera; // 弹着点在相机坐标系中的位置 (m)
double flight_time; // 弹丸飞行时间 (s)
};
/**
* @brief 弹道计算类：根据 IMU 当前 pitch 和追踪装甲板距离，
* 推算弹着点在相机坐标系中的三维坐标。
*
* 坐标系约定（与 solver.cpp 一致）：
* 世界系: x=前, y=左, z=上
* 相机系: z=前, x=右, y=下（OpenCV 标准）
*/
class BallisticSolver {
public:
/**
* @brief 根据水平距离和垂直高度差，迭代求解补偿 pitch 角。
* @param d 水平距离 (m)
* @param y 垂直高度差 (m，目标高于枪口为正)
* @param v0 枪口初速度 (m/s)
* @param k 空气阻力系数
* @return 补偿后的 pitch 角 (度)
*/
static double get_pitch(double d, double y, double v0, double k = 0.1);
/**
* @brief 计算弹丸飞行时间。
* @param d 目标水平距离 (m，由追踪装甲板获得)
* @param pitch 当前 IMU pitch 角 (度，仰为正)
* @param v0 枪口初速度 (m/s)
* @param k 空气阻力系数
* @return 弹丸飞行时间 (s)
*/
static double get_flight_time(double d, double pitch, double v0, double k = 0.1);
/**
* @brief 根据当前 IMU pitch 角推算弹丸在水平距离 d 处的垂直高度。
* @param d 目标水平距离 (m)
* @param pitch 当前 IMU pitch 角 (度，仰为正)
* @param v0 枪口初速度 (m/s)
* @param k 空气阻力系数
* @return 弹着点相对枪口的垂直高度 (m，高于枪口为正)
*/
static double get_impact_y(double d, double pitch, double v0, double k = 0.1);
/**
* [TEST] 用于验证弹道算法正确性：将弹着点反解算到相机坐标系，
* 再通过 project_to_pixel 投影到像素并用红点显示。
* 如果实验结果正确，可直接注释调用处，不影响其他结构。
*
* 坐标变换链：
* xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g)
*
* @param xyz_in_world 目标在世界坐标系中的位置 (m，来自 PnP Solver)
* @param pitch_imu 当前 IMU pitch 角 (度，仰为正)
* @param v0 枪口初速度 (m/s)
* @param R_camera2gimbal 相机到云台的旋转矩阵
* @param R_gimbal2world 云台到世界的旋转矩阵
* @param t_camera2gimbal 相机原点在云台系中的平移向量 (m)
* @param k 空气阻力系数
* @return ImpactPoint { 弹着点相机坐标(m), 飞行时间(s) }
*/
static ImpactPoint get_impact_in_camera(
const Eigen::Vector3d& xyz_in_world,
double pitch_imu,
double v0,
const Eigen::Matrix3d& R_camera2gimbal,
const Eigen::Matrix3d& R_gimbal2world,
const Eigen::Vector3d& t_camera2gimbal,
double k = 0.1);
/**
* [TEST] 将相机坐标系3D点简化投影到像素坐标（针孔模型，无畲变）。
* @param xyz_in_camera 弹着点在相机坐标系中的位置 (m)
* @param camera_matrix OpenCV 相机内参矩阵 (3x3 CV_64F)
* @return 像素坐标
*/
static cv::Point2f project_to_pixel(
const Eigen::Vector3d& xyz_in_camera,
const cv::Mat& camera_matrix);
};
#endif // BALLISTIC_PREDICITION_H

23
armor/include/show_images/show_images.h Normal file
View File

@@ -0,0 +1,23 @@
//
// Created by xinyang on 19-3-27.
//
#ifndef _SHOW_IMAGES_H_
#define _SHOW_IMAGES_H_
#include <opencv2/core.hpp>
#include <armor_finder/armor_finder.h>
//
void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes);
void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &armor_box);
void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs);
void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes);
void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos);
// [TEST] 在图像上标注弹着点（红色小圆点），用于验证弹道算法正确性。
// 如果实验结果正确，直接注释调用处即可，不影响其他结构。
// impact_pixel: 由 BallisticSolver::project_to_pixel() 计算得到的像素坐标。
void showImpactPoint(const std::string& window_name, const cv::Mat& src, cv::Point2f impact_pixel);
#endif /* _SHOW_IMAGES_H_ */