opencv相机姿态解算程序

1.头文件

#pragma once

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>
using namespace std;
using namespace cv;

class JiaoDuJieSuan
{
public:
    JiaoDuJieSuan();

    //获得旋转矩形角点并保存在rectPoint2f
    void huoDeJuXingJiaoDian(RotatedRect rect,Mat img);

    void qiuJieJiaoDu(float &pitch, float &yaw);

    void drawPoints(Mat img);
private:
    Mat cameraMatrix;
    Mat distortionCoefficients;
    Mat rvecs;
    Mat tvecs;
    vector<Point2f> rectPoint2f;
    vector<Point3f> muBiaoPoint3f;
    float muBiaoWidth;
    float muBiaoHeight;
};

2.源文件

#include "jiaodujiesuan.hpp"

JiaoDuJieSuan::JiaoDuJieSuan()
{
    string filename = "out_Arlco_Camera_data.xml";
    FileStorage fs(filename, FileStorage::READ);
    if(!fs.isOpened()){
        cout <<"no such file"<<endl;
        return;
    }
    fs["distortion_coefficients"] >> distortionCoefficients;
    fs["camera_matrix"] >> cameraMatrix;
//    cout<<cameraMatrix.cols<<" "<<cameraMatrix.rows;
//    imshow("aa",cameraMatrix);
    fs.release();
    muBiaoWidth = 100;
    muBiaoHeight = 40;

    muBiaoPoint3f.push_back(Point3f(-muBiaoWidth/2,-muBiaoHeight/2,0));
    muBiaoPoint3f.push_back(Point3f(muBiaoWidth/2,-muBiaoHeight/2,0));
    muBiaoPoint3f.push_back(Point3f(muBiaoWidth/2,muBiaoHeight/2,0));
    muBiaoPoint3f.push_back(Point3f(-muBiaoWidth/2,muBiaoHeight/2,0));
//    for(int i = 0; i < muBiaoPoint3f.size(); i++){
//        cout<<muBiaoPoint3f[i]<<endl;
//    }
}

void JiaoDuJieSuan::huoDeJuXingJiaoDian(RotatedRect rect,Mat img)
{
    Point2f pt;
//    rect.points(pt);
//    for(int i = 0; i < 4; i++){
//        cout<<pt[i]<<endl;
//    }

    const double pi = 3.1415926;
    double dangle = rect.angle;
    while(dangle > 90)
        dangle -= 180;

    double a = dangle*pi/180;
    double w = rect.size.width / (cos(a)*2);
    double h = w * muBiaoHeight / muBiaoWidth;

    pt.x = rect.center.x - w;
    pt.y = rect.center.y - h;
    rectPoint2f.push_back(pt);

    pt.x = rect.center.x + w;
    pt.y = rect.center.y - h;
    rectPoint2f.push_back(pt);

    pt.x = rect.center.x + w;
    pt.y = rect.center.y + h;
    rectPoint2f.push_back(pt);

    pt.x = rect.center.x - w;
    pt.y = rect.center.y + h;
    rectPoint2f.push_back(pt);

    line(img, rectPoint2f[0], rectPoint2f[1], CV_RGB(0, 0, 255), 2, 8, 0);
    line(img, rectPoint2f[1], rectPoint2f[2], CV_RGB(0, 0, 255), 2, 8, 0);
    line(img, rectPoint2f[2], rectPoint2f[3], CV_RGB(0, 0, 255), 2, 8, 0);
    line(img, rectPoint2f[3], rectPoint2f[0], CV_RGB(0, 0, 255), 2, 8, 0);

    //    cout<<w;
}

void JiaoDuJieSuan::qiuJieJiaoDu(float &pitch, float &yaw)
{
    rvecs = Mat::zeros(3,1,CV_64FC1);
    tvecs = Mat::zeros(3,1,CV_64FC1);
    solvePnP(muBiaoPoint3f,rectPoint2f,cameraMatrix,distortionCoefficients,rvecs,tvecs,false,SOLVEPNP_ITERATIVE);
//    cout<<rvecs.size<<endl;

    uchar *ptrbegin = tvecs.data;
    const uchar *ptrend = tvecs.data + 3;
//    for(; ptrbegin != ptrend; ptrbegin++){
//        cout<<(int)(*ptrbegin)<<endl;
//    }

    float x,y,z;
    const float pi = 3.1415926;
    x = *(ptrend-3);
    y = *(ptrend-2);
    z = *(ptrend-1);
    cout<<x<<" "<<y<<" "<<z<<endl;
    pitch = atan(y/z);
    yaw = atan(x/z);
    pitch = 180*pitch/pi;
    yaw =  180*yaw/pi;
}

void JiaoDuJieSuan::drawPoints(Mat img)
{
    vector<Point2f> tuxiang;
    vector<Point3f> mubiao;
    mubiao.push_back(Point3f(0,0,0));
    mubiao.push_back(Point3f(100,0,0));
    mubiao.push_back(Point3f(0,100,0));
    mubiao.push_back(Point3f(0,0,100));

    projectPoints(mubiao,rvecs,tvecs,cameraMatrix,distortionCoefficients,tuxiang);
    circle(img,tuxiang[2],3,Scalar(0,0,255),1,LINE_8,0);
    line(img,tuxiang[0],tuxiang[1],Scalar(0,0,255));
    line(img,tuxiang[0],tuxiang[2],Scalar(0,255,0));
    line(img,tuxiang[0],tuxiang[3],Scalar(255,0,0));
}

3.main文件

int main()
{
    Mat img = imread("2.jpg");
    Mat imggray;
    ZhuangJia zj;
        zj.erZhiHua(img,imggray,15);
        vector<RotatedRect> vr;
        vr = zj.xunZhaoJuXing(imggray);
        vr = zj.niHeMuBiao(vr);
        RotatedRect rr;
        JiaoDuJieSuan jdjs;
        float pitch, yaw;
        if(vr.size()>0){
            rr = zj.xunZhaoZuiJia(vr);
            jdjs.huoDeJuXingJiaoDian(rr,img);
            jdjs.qiuJieJiaoDu(pitch,yaw);
            jdjs.drawPoints(img);
            cout<<pitch<<" "<<yaw<<endl;
        }
    waitKey(0);
    return 0;
}