(非常好)计算匹配点的三维坐标

#include <stdio.h>
#include <highgui.h>
#include "cxcore.h"
#include <cvcam.h>
#include <cv.h>
#include <vector>
#include <math.h>
 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( int argc, char** argv )
 {
cvseq* seq;
cvseqreader reader;
cvfilestorage* fs = cvopenfilestorage( "upimg_matched_points.yml", 0, cv_storage_read );
cvfilenode* parent = cvgetfilenodebyname( fs, 0, "upimg_matched_points" );
cvfilenode* data = cvgetfilenodebyname( fs, parent, "data" );
seq = data->data.seq;
cvstartreadseq( seq, &reader, 0 );
int total = seq->total;
int value1,value2;
int i;
int count=0;
//printf("total1=%d\n",total/2);
cvmat* points1;
cvmat* points2;
points1 = cvcreatemat(2,total/2,cv_32f);
points2 = cvcreatemat(2,total/2,cv_32f);
int j=0,k=0;
for(i=0;i<total;i++)
{
cvfilenode* pt = (cvfilenode*)reader.ptr;
value1 = pt->data.i;
if(i%2==0)
{
cvsetreal2d(points1,0,j,value1);
j++;
}
//printf("%d ",value1);
i++;
cv_next_seq_elem(seq->elem_size, reader );
pt = (cvfilenode*)reader.ptr;
value2 = pt->data.i;
if(i%2==1)
{
cvsetreal2d(points1,1,k,value2);
k++;
}               
// printf("%d\n",value2);
cv_next_seq_elem(seq->elem_size, reader );
}
printf("total1=%d\n",total/2);
cvseq* seq1;
cvseqreader reader1;
cvfilestorage* fs1 = cvopenfilestorage( "downimg_matched_points.yml", 0, cv_storage_read );
cvfilenode* parent1 = cvgetfilenodebyname( fs1, 0, "downimg_matched_points" );
cvfilenode* data1 = cvgetfilenodebyname( fs1, parent1, "data" );
seq1 = data1->data.seq;
cvstartreadseq( seq1, &reader1, 0 );
int total1 = seq1->total;
double value3,value4;
int count1=0;
int p=0,q=0;
for(i=0;i<total1;i++)
{
cvfilenode* pt = (cvfilenode*)reader1.ptr;
value3 = pt->data.i;
if(i%2==0)
{
cvsetreal2d(points2,0,p,value3);
p++;
}
//printf("%d ",value3);
i++;
cv_next_seq_elem(seq1->elem_size, reader1 );
pt = (cvfilenode*)reader1.ptr;
value4 = pt->data.i;
if(i%2==1)
{
cvsetreal2d(points2,1,q,value4);
q++;
}
//printf("%d\n",value4);
cv_next_seq_elem(seq1->elem_size, reader1 );
}
printf("total2=%d\n",total1/2);
cvmat* fundmatr;
cvmat* status;
status = cvcreatemat(1,total/2,cv_32f);
//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);
//printmat(points2);
if( num == 1 )
{
printf("fundamental matrix was found\n");
printmat(fundmatr);
//计算本质矩阵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);
cvinitmatheader(&k1,3,3,cv_64fc1,a,cv_autostep);

//计算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;                //将本质矩阵e付给a
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,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 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);


//计算匹配点的三维坐标值p1=k*[i|0] p2=k*[r|t]
cvmat *p1,*p2,k,r,t_1,ma,mb;
p1=cvcreatemat(3,4,cv_64fc1);
p2=cvcreatemat(3,4,cv_64fc1);
k=k1,r=e2,t_1=c;
//printmat(&r);
double a5[]={1,0,0,0,
0,1,0,0,
0,0,1,0};
double b5[]={cvmget(&r,0,0),cvmget(&r,0,1),cvmget(&r,0,2),cvmget(&t_1,0,0),
cvmget(&r,1,0),cvmget(&r,1,1),cvmget(&r,1,2),cvmget(&t_1,1,0),
cvmget(&r,2,0),cvmget(&r,2,1),cvmget(&r,2,2),cvmget(&t_1,2,0)};
cvinitmatheader(&ma,3,4,cv_64fc1,a5,cv_autostep);
cvinitmatheader(&mb,3,4,cv_64fc1,b5,cv_autostep);
cvmmul(&k,&ma,p1);
cvmmul(&k,&mb,p2);
//printmat(&r);
//printmat(&ma);
printf("p1=");printmat(p1);
printf("p2=");printmat(p2);


//读取图像坐标点
cvpoint3d32f store[2000];
int count_3d=0;
printf("3d_points:\n");
for(i=0;i<total/2;i++)    
{   
double u1_1=cvmget(points1,0,i), v1_1=cvmget(points1,1,i);
double u2_1=cvmget(points2,0,i), v2_1=cvmget(points2,1,i);
cvmat a_a;
double c5[]={cvmget(p1,2,0)*u1_1-cvmget(p1,0,0),cvmget(p1,2,1)*u1_1-cvmget(p1,0,1),cvmget(p1,2,2)*u1_1-cvmget(p1,0,2),cvmget(p1,2,3)*u1_1-cvmget(p1,0,3),
cvmget(p1,2,0)*v1_1-cvmget(p1,1,0),cvmget(p1,2,1)*v1_1-cvmget(p1,1,1),cvmget(p1,2,2)*v1_1-cvmget(p1,1,2),cvmget(p1,2,3)*v1_1-cvmget(p1,1,3),
cvmget(p2,2,0)*u2_1-cvmget(p2,0,0),cvmget(p2,2,1)*u2_1-cvmget(p2,0,1),cvmget(p2,2,2)*u2_1-cvmget(p2,0,2),cvmget(p2,2,3)*u2_1-cvmget(p2,0,3),
cvmget(p2,2,0)*v2_1-cvmget(p2,1,0),cvmget(p2,2,1)*v2_1-cvmget(p2,1,1),cvmget(p2,2,2)*v2_1-cvmget(p2,1,2),cvmget(p2,2,3)*v2_1-cvmget(p2,1,3)};
cvinitmatheader(&a_a,4,4,cv_64fc1,c5,cv_autostep);
//printf("%f\n",cvmget(p1,2,0)*u1_1-cvmget(p1,0,0));
//printf("a=");printmat(&a_a);
cvmat *u_u,*v_v,*s_s;
s_s=cvcreatemat(4,4,cv_64fc1);
u_u=cvcreatemat(4,4,cv_64fc1);
v_v=cvcreatemat(4,4,cv_64fc1);
cvsvd(&a_a,s_s,u_u,v_v,0);
//printf("v=");printmat(v_v);


//存储三维坐标点


store[i].x=cvmget(v_v,0,3);
store[i].y=cvmget(v_v,1,3);
store[i].z=cvmget(v_v,2,3);
if(count_3d<100)
{
printf("x=%f  ",store[i].x);
printf("y=%f  ",store[i].y);
printf("z=%f\n",store[i].z);
count_3d++;
}
}
printf("......\n");
return 1;
}
else
{
printf("fundamental matrix was not found\n");
return -1;


}
printf("\n");
cvwaitkey(0);
return 0;

 }