AngleSolve 篇代码

这一部分的代码比较多，而且有一些是目前没用的，只是写了这么一个功能留着使用，会相应解释出现的原因

    /** 坐标系转换 */
    cv::Point2f imu2pixel(Eigen::Vector3d imu_pos);                     // 世界坐标转换为像素坐标
    Eigen::Vector3d pixel2imu(Armor &armor);                            // 像素坐标转换为世界坐标
    Eigen::Vector3d pixel2imu_buff(cv::Point2f *p, int type);           // 像素坐标转换为世界坐标(BUFF)
    /** 四元数旋转矩阵 */
    Eigen::Matrix3d quaternionToRotationMatrix(float quaternion[4]);    // 四元数转换为旋转矩阵
    Eigen::Matrix3d quaternionToRotationMatrix_buff();                  // 四元数转换为旋转矩阵(BUFF)
    Eigen::Matrix3d EulerAngle2RotationMatrix(double pitch,double roll,double yaw); // 解算旋转矩阵
    /** 重投影 */
    void AngleSolve_show(Armor &armor);                                 // 绘制装甲板朝向角
    double Armor_Angle(Armor &armor);                                   // 计算装甲板朝向角度
    double World_projection(Armor &armor,double Angle);                 // 计算重投影后装甲板4点距离差
    //** 枪管角度解算 */
    void Init(float data[3], float *quat);                              // 更新数据
    Eigen::Vector3d Barrel_Solve(Eigen::Vector3d position);             // 解算枪管移动角度

    cv::Point2f cam2pixel(Eigen::Vector3d cam_pos);             // 相机坐标系转换为像素坐标系
    Eigen::Vector3d cam2imu(Eigen::Vector3d cam_pos);           // 相机坐标系转换为世界坐标系
    Eigen::Vector3d imu2cam(Eigen::Vector3d imu_pos);           // 世界坐标系转换为相机坐标系

private:
    //========================函数部分========================
    /** pnp测距 */
    Eigen::Vector3d pnpSolve(cv::Point2f *p, int type,Eigen::Matrix<double, 3, 1>& R);         // 测距函数
    Eigen::Vector3d pnpSolve_buff(cv::Point2f *p, int type);    // 测距函数(buff)

    /** 世界坐标系转换为像素坐标系(调用接口) */

    /** 像素坐标系转换为世界坐标系(调用接口) */
    Eigen::Vector3d pixel2cam(Armor &armor);                    // 像素坐标系转换为相机坐标系
    Eigen::Vector3d pixel2cam_buff(cv::Point2f *p, int type);   // 像素坐标系转换为相机坐标系(BUFF)

    /** 抬升角度计算 */
    float airResistanceSolve(Eigen::Vector3d Pos);
    float BulletModel(float x, float v, float angle);

我只对函数部分进行讲解，参数的结构和组成请自行理解，代码也有相应注释

记住，AngleSolve只是一个工具类，为其他类提供服务，并不作为任务运行在程序中