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

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