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

armor/include/armor_finder/classifier/armor.hpp

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

armor/include/armor_finder/classifier/classifier.h

@@ -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 */

armor/include/armor_finder/classifier/solver.h

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