（转载）用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;
        
    }
}
运行效果图：