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