关于PnP测距部分的代码

    /** 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 AngleSolve::pnpSolve(cv::Point2f *p, int type,Eigen::Matrix<double, 3, 1>& R)
{
    std::vector<cv::Point3d> ps;        // 世界坐标系
    std::vector<cv::Point2f> pu;        // 像素坐标系
    double w;                           // 宽度
    double h;                           // 高度
    //=======================宽高选择=======================
    if(type == SMALL)                   // 小装甲板
    {
        w = small_w;
        h = small_h;
//        std::cout << "small" << std::endl;
    }
    else if(type == BIG)                // 大装甲板
    {
//        w = big_w;
//        h = big_h;
        w = small_w;
        h = small_h;
//        std::cout << "BIG" << std::endl;
    }

    tv = Eigen::Vector3d();                 //平移向量
    rv = Eigen::Matrix<double,3,3>();       //旋转矩阵
    //=======================坐标系========================
    /** 世界坐标系 */
    ps = {
            {-w / 2 , -h / 2, 0.},      //左上
            { w / 2 , -h / 2, 0.},      //右上
            { w / 2 ,  h / 2, 0.},      //右下
            {-w / 2 ,  h / 2, 0.}       //左下
    };
    /** 像素坐标系 */
    pu.push_back(p[3]);                          //左上
    pu.push_back(p[2]);                          //右上
    pu.push_back(p[1]);                          //右下
    pu.push_back(p[0]);                          //左下

    cv::Mat rvec;
    cv::Mat tvec;

    // pnp测距
    cv::solvePnP(ps, pu, F_MAT, C_MAT, rvec, tvec/*, SOLVEPNP_IPPE*/);

    Eigen::Matrix<double, 3, 1> R_v;
    cv::cv2eigen(rvec,R_v);
    R = R_v;

    cv::Mat rv_mat;
    cv::Rodrigues(rvec,rv_mat);     // 将旋转向量转为旋转矩阵
    cv::cv2eigen(rv_mat,rv);
    cv::cv2eigen(tvec, tv);

#ifdef SHOW_MEASURE_RRECT
    cv::Mat pnp_check = _src.clone();
    std::cout<<"rv"<<rv<<std::endl;

    //=========================坐标系转换==============================

    cv::Point2f m_lu,m_ld,m_ru,m_rd;             //定义4点坐标
    Eigen::Vector3d imuPoint;                    //世界坐标系
    Eigen::Vector3d armorPoint;                  //相机坐标系

    imuPoint = {-w / 2 , -h / 2, 0.};   //世界坐标系左上
    armorPoint = rv*imuPoint + tv;
    m_lu = cam2pixel(armorPoint);

    imuPoint = {-w / 2 , h / 2, 0.};    //世界坐标系左下
    armorPoint = rv*imuPoint + tv;
    m_ld = cam2pixel(armorPoint);

    imuPoint = {w / 2 , -h / 2, 0.};    //世界坐标系左下
    armorPoint = rv*imuPoint + tv;
    m_ru = cam2pixel(armorPoint);

    imuPoint = {w / 2 , h / 2, 0.};     //世界坐标系左下
    armorPoint = rv*imuPoint + tv;
    m_rd = cam2pixel(armorPoint);

    /** 将坐标用圆形表示 */
    circle(pnp_check,m_lu,3,cv::Scalar(0,255,0),-1);
    circle(pnp_check,m_ld,3,cv::Scalar(255,255,0),-1);
    circle(pnp_check,m_ru,3,cv::Scalar(0,0,255),-1);
    circle(pnp_check,m_rd,3,cv::Scalar(0,255,255),-1);

    /** 将坐标用线连接 */
    line(pnp_check,m_lu,m_ld,cv::Scalar(0,0,0),2);
    line(pnp_check,m_ld,m_rd,cv::Scalar(255,0,0),2);
    line(pnp_check,m_rd,m_ru,cv::Scalar(255,0,255),2);
    line(pnp_check,m_ru,m_lu,cv::Scalar(255,255,0),2);

    /** 输出4点坐标和 */
    std::cout<<"m_lu:"<<m_lu<<std::endl;
    std::cout<<"m_ld:"<<m_ld<<std::endl;
    std::cout<<"m_ru:"<<m_ru<<std::endl;
    std::cout<<"m_rd:"<<m_rd<<std::endl;
    std::cout<<"tvec:"<<cam2pixel(tv)<<std::endl;

    imshow("pnp_check",pnp_check);
#endif
    return tv;
}

SHOW_MEASURE_RRECT 是一个测试点，这里我就不展开讲了，自己观看

我们比赛中分为大、小装甲板，这里是都当小装甲板进行计算了，这样对大装甲板的计算会有一定的误差，虽然不会太大

为什么不区分呢，因为大小装甲板的区别一直是个难题，两者在低分辨率、低曝光下，太过难于区别，小装甲板一旦识别为大装甲板

对测距是一个致命的错误，为什么你可以自己想一想同样的大小，小变大会怎么样，相反，大装甲板识别成小装甲板的影响反而没有很大，当然你可以对装甲板ID为1的特判为大装甲板，这里纯属我后面忘记改了而已

接下来进入正题，这里small_w、small_h是小装甲板的实际大小，这里的实际高宽指的是灯条的高和两个灯条的中心距离

你可以自己量量看

我们的装甲板角点顺序是从右下角开始，逆时针计算，这里可以看到有一个世界坐标系和像素坐标系，两者要一一对应，不然会计算错误

如图： 凑合看看

这里装甲板的中心就是（0，0）也就是3D坐标系的中心，

    ps = {
            {-w / 2 , -h / 2, 0.},      //左上
            { w / 2 , -h / 2, 0.},      //右上
            { w / 2 ,  h / 2, 0.},      //右下
            {-w / 2 ,  h / 2, 0.}       //左下
    };
    /** 像素坐标系 */
    pu.push_back(p[3]);                          //左上
    pu.push_back(p[2]);                          //右上
    pu.push_back(p[1]);                          //右下
    pu.push_back(p[0]);                          //左下

坐标3对应的世界坐标就是（-w / 2 , -h / 2, 0.） 对应x，y，z

所有的像素坐标和3D坐标相对应

bool solvePnP( InputArray objectPoints, InputArray imagePoints,
                            InputArray cameraMatrix, InputArray distCoeffs,
                            OutputArray rvec, OutputArray tvec,
                            bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );

需要的参数就是，像素坐标、世界坐标、相机内参矩阵、相机畸变矩阵

相应得到的就是旋转向量、平移向量，tvec就是我们的世界坐标系