（转载）用OPENCV写的求旋转矩阵R和平移向量t的代码

网址：http://changfengdaxia1234.blog.163.com/blog/static/6916831200882111244621/

三维重建求外参R t候选值(1)

生活学习 2008-09-21 11:02:44

#include <cvcam.h>
#include <cv.h>
#include "highgui.h"
#include <stdio.h>
#include <vector>
#include <math.h>

using namespace std;

void PrintMat(CvMat *A)
{
int i,j;
//printf("/nMatrix=:");
for(i=0;i<A->rows;i++)
{
printf("/n");
switch(CV_MAT_DEPTH(A->type))
{
case CV_32F:
case CV_64F:
for(j=0;j<A->cols;j++)
printf("%9.3f",(float)cvGetReal2D(A,i,j));
break;
case CV_8U:
case CV_16U:
for(j=0;j<A->cols;j++)
printf("%6d",(int)cvGetReal2D(A,i,j));
break;
default:
break;
}
}
printf("/n");
}

int main()
{

char pointsList1[1500];
char pointsList2[1500];
sprintf(pointsList1,"%s","left.txt");
sprintf(pointsList2,"%s","right.txt");

FILE* inputFiles[2];

//declare useful typedefs
typedef vector <CvPoint2D32f> CVPOINT2D32F_VECTOR;
std::vector <CVPOINT2D32F_VECTOR> allPoints;

//open input files
inputFiles[0] = fopen(pointsList1,"r");
if ( inputFiles[0] == NULL ) {
printf("%s not found/n",pointsList1);
return -1;
}

inputFiles[1] = fopen(pointsList2,"r");
if ( inputFiles[1] == NULL ) {
printf("%s not found/n",pointsList2);
return -1;
}

//first read in the point pairs
for (int i = 0; i < 2; i++) {

CVPOINT2D32F_VECTOR points;
CvPoint2D32f temp;
double x,y;
while ( fscanf(inputFiles[i],"%lf %lf",&x,&y) == 2 ) {

temp.x =x;
temp.y =y;

points.push_back(temp);
}
allPoints.push_back(points);
}

//make sure that they are equal length
if ( allPoints[0].size() != allPoints[1].size() ) {
printf("input files must have same number of points/n");
}

//transfer the vector of points to the appropriate opencv matrix structures
int i1,i2;
i2 =0;
int numPoints = allPoints[0].size();
CvMat* points1;
CvMat* points2;
CvMat* status;
CvMat* fundMatr;
points1 = cvCreateMat(2,numPoints,CV_32F);
points2 = cvCreateMat(2,numPoints,CV_32F);
status = cvCreateMat(1,numPoints,CV_32F);

for ( i1 = 0; i1 < numPoints; i1++) {

cvSetReal2D(points1,0,i1,allPoints[0][i1].x/1);
cvSetReal2D(points1,1,i1,allPoints[0][i1].y/1);

cvSetReal2D(points2,0,i1,allPoints[1][i1].x/1);
cvSetReal2D(points2,1,i1,allPoints[1][i1].y/1);
}

//create the output fundamental matrix
fundMatr = cvCreateMat(3,3,CV_64FC1);

//see opencv manual for other options in computing the fundamental matrix
int num = cvFindFundamentalMat(points1,points2,fundMatr,CV_FM_8POINT,1.0,0.9999,status);

if( num == 1 )
{
//printf("Fundamental matrix was found/n");
printf("Fundamental matrix F:");
PrintMat(fundMatr); //基础矩阵F
//计算本质矩阵E=K2*FK1
CvMat K1,F,E,K2;
double a[]={1100,0,384,
0,1100,288,
0,0,1};
double b[9],c[9],d[9];
cvInitMatHeader(&K1,3,3,CV_64FC1,a,CV_AUTOSTEP);
cvInitMatHeader(&F,3,3,CV_64FC1,b,CV_AUTOSTEP);
cvInitMatHeader(&E,3,3,CV_64FC1,c,CV_AUTOSTEP);
cvInitMatHeader(&K2,3,3,CV_64FC1,d,CV_AUTOSTEP);
printf("Inside parameter K:");
PrintMat(&K1);
F=*fundMatr;
cvTranspose(&K1,&K2);
cvMul(&K2,&F,&E,1);
cvMul(&E,&K1,&E,1);
printf("Intrinsical matrix E=K^FK:");
PrintMat(&E);

//计算R|t候选值
double a1[9],b1[9],c1[9],d1[9],e1[9],f1[9];
CvMat A,D,U,V,M,V2;
//从缓存中为矩阵赋值
cvInitMatHeader(&A,3,3,CV_64FC1,a1,CV_AUTOSTEP);
cvInitMatHeader(&D,3,3,CV_64FC1,b1,CV_AUTOSTEP);
cvInitMatHeader(&M,3,3,CV_64FC1,c1,CV_AUTOSTEP);
cvInitMatHeader(&U,3,3,CV_64FC1,d1,CV_AUTOSTEP);
cvInitMatHeader(&V,3,3,CV_64FC1,e1,CV_AUTOSTEP);
cvInitMatHeader(&V2,3,3,CV_64FC1,f1,CV_AUTOSTEP);
A=E;
printf("A=");PrintMat(&A);

//Mc=svd(E),表示E=U*D*V'
cvSVD(&A,&D,&U,&V,0); //返回U和V
printf("D=svd(E)=");PrintMat(&D);
printf("U=");PrintMat(&U);
printf("V=");PrintMat(&V);
cvTranspose(&V,&V2);
printf("V^=");PrintMat(&V2);
cvmMul(&U,&D,&M);
cvmMul(&M,&V2,&M);
printf("E=UDV^=");PrintMat(&M);

//求E^
double r,s,t,a2[9],b2[9];
r=cvmGet(&D,0,0);
s=cvmGet(&D,1,1);
printf("r=%f ",r);
printf("s=%f/n",s);
t=(r+s)/2;
CvMat D2,E2;
a2[0]=a2[4]=t;
a2[1]=a2[2]=a2[3]=a2[5]=a2[6]=a2[7]=0;
cvInitMatHeader(&D2,3,3,CV_64FC1,a2,CV_AUTOSTEP);
cvInitMatHeader(&E2,3,3,CV_64FC1,b2,CV_AUTOSTEP);
/*cvmSet(&D2,0,0,t);
cvmSet(&D2,1,1,t);*/
cvmSet(&D2,2,2,0);
printf("D^=");PrintMat(&D2);
cvmMul(&U,&D2,&E2);
cvmMul(&E2,&V2,&E2);
printf("E2=UD^V^=");PrintMat(&E2);

//求svd(E2) E2=U2D2V2^
double b3[9],c3[9],d3[9],e3[9];
CvMat D3,U3,V3,V4;
cvInitMatHeader(&D3,3,3,CV_64FC1,b3,CV_AUTOSTEP);
cvInitMatHeader(&U3,3,3,CV_64FC1,c3,CV_AUTOSTEP);
cvInitMatHeader(&V3,3,3,CV_64FC1,d3,CV_AUTOSTEP);
cvInitMatHeader(&V4,3,3,CV_64FC1,e3,CV_AUTOSTEP);
cvSVD(&E2,&D3,&U3,&V3,0); //返回U和V
printf("D2=svd(E2)=");PrintMat(&D3);
printf("U2=");PrintMat(&U3);
printf("V2=");PrintMat(&V3);
cvTranspose(&V3,&V4);
printf("V2^=");PrintMat(&V4);
cvmMul(&U3,&D3,&E2);
cvmMul(&E2,&V4,&E2);
printf("E2=U2D2V2^=");PrintMat(&E2);
//确定R t
double a3[]={0,1,0,
-1,0,0,
0,0,1};
double b4[]={0,0,1},c4[3];
CvMat A2,B,C;
cvInitMatHeader(&A,3,3,CV_64FC1,a3,CV_AUTOSTEP);
cvInitMatHeader(&B,3,1,CV_64FC1,b4,CV_AUTOSTEP);
cvInitMatHeader(&C,3,1,CV_64FC1,c4,CV_AUTOSTEP);
cvmMul(&U3,&A,&E2);
cvmMul(&E2,&V4,&E2);
printf("R(Candidate)=");PrintMat(&E2);
printf("U2=");PrintMat(&U3);
cvmSet(&B,0,0,0);
cvmSet(&B,1,0,0);
cvmSet(&B,2,0,1);
printf("B=");PrintMat(&B);
cvmMul(&U3,&B,&C);
printf("t(Candidate)=U2B");PrintMat(&C);
return 1;
}
else
{
printf("Fundamental matrix was not found/n");
return -1;

}
}
运行效果图：