opencv相机姿态解算程序

#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;
};

#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;
mubi
4000
ao.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));
}

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;
}