hw#6 : projective reconstruction ece661 · step ii : image rectification 1. from the estimated...
TRANSCRIPT
HW#6 : Projective Reconstruction ECE661
WONJO JUNG Civil Engineering Dept. Purdue University
Input images ( 800 x 600 pixels)
Step I : Determine the fundamental matrix
1. manually select at least 8 correspondences { ' }i ix x↔ from the given stereo pair 2. normalization : ˆ ˆ, ' ' 'i i i ix Tx x T x= = 3. find the fundamental matrix
[ ]1 2 3
4 5 6
7 8 9
min , subject to 1 using SVD
, ' ' ' ' ' ' 1
ˆ
ith row i i i i i i i i i i i i
Af f
where A x x x y x y x y y y x y
f f fF f f f
f f f
=
=
⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
4. constraint enforcement
ˆ , where ( , , ) satisfying .
ˆThen ' ( , ,0)
T
T
F UDV D diag r s t r s t
F Udiag r s V
= = ≥ ≥
=
5. denormalization ˆ' 'TF T F T= 6. refine F using the Gold Standard algorithm
a. choose camera matrices [ ]| 0P I= and [ ]' | 'X
P e F e⎡ ⎤= ⎣ ⎦ , where 'e is obtained
from F . b. From the correspondence 'i ix x↔ , P and 'P , determine an estimate of ˆ
iX
c. The correspondence consistent with F is obtained as ˆ ˆˆ ˆ, ' 'i i i ix PX x P X= = i. Minimize cost function 2 2ˆ ˆ( , ) ( ' , ' )i i i i
id x x d x x+∑ using LM algorithm
over n points(3D) and the elements of 'P ; total 3n+12 parameters. 7. then epiplar lines are
' , 'Tl Fe l F e= =
Figure 1. original left image and manually selected 10 correspondences
Figure 2. original right image and manually selected 10 correspondences
Step II : Image Rectification
1. from the estimated fundamental matrix F and epipoles , 'e e , select a projective transformation 'H that maps the epipole 'e to the point at infinity, (1,0,0)T .
a. Translation matrix 0
0 0 0
1 00 1 , in this implementation 0.0 0 1
xT y x y
−⎡ ⎤⎢ ⎥= − = =⎢ ⎥⎢ ⎥⎣ ⎦
b. Rotation matrix 1 ' '
cos sin 0sin cos 0 , where tan ( / ).
0 0 1x yR e e
θ θθ θ θ −
−⎡ ⎤⎢ ⎥= =⎢ ⎥⎢ ⎥⎣ ⎦
c. 1 0 0
ˆ ˆ0 1 0 , where ' , ' '1/ 0 1
xG f e e R ef
⎡ ⎤⎢ ⎥= = =⎢ ⎥⎢ ⎥−⎣ ⎦
d. Then 'H GRT= e. Finding the matching projective transformation H that minimizes the least-squares
distance. This means finding AH minimizing the cost function 2ˆ ˆ( , ' )A i ii
d H x x∑ .
[ ] 0 0ˆ ˆ' , ' , , ' ' 'i i i iXwhere F e M H H M x H x x H x= = = =
And this problem is equivalent to minimizing 2ˆ ˆ ˆ( ' )i i ii
f ax by c x= + + −∑ .
Then 0f f fa b c∂ ∂ ∂
= = =∂ ∂ ∂
. Solve
2
2
ˆ ˆ ˆ ˆ ˆ ˆ '
ˆ ˆ ˆ ˆ ˆ ˆ '
ˆ ˆ ˆ1 '
i i i i i ii i i i
i i i i i ii i i i
i i ii i i i
x x y x x xa
x y y y b y xc
x y x
⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ =⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎣ ⎦⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦
∑ ∑ ∑ ∑∑ ∑ ∑ ∑∑ ∑ ∑ ∑
.
Then 0 1 00 0 1
A
a b cH
⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
f. 0AH H H=
2. Resample the images according to the projective transformation H and 'H
Figure 3. Rectified images
Figure 4. epipolar lines on the right image
Another test image
Epipolar geometry
Resampled image
Resampled right image with epipolar lines
Step III : 3D reconstruction
1. detect edges using the Canny operator 2. find corresponding points ˆ ˆ 'i ix x↔ using NCC through the banded area 3. From the correspondence 'i ix x↔ , we have P and 'P from the STEP I and H and 'H
from the STEP II . Since ˆi ix HPX= and ˆ ' ' 'i ix H P X= , From the correspondence
'i ix x↔ , determine an estimate of ˆiX using linear triangulation method.
4. display using the Matlab
Left image Right image
Overlapped image
( Left image )
( Right image )
Point correspondences
Display 3D plots
200250100
200
300
400
500
600
700
800
900
X
Y
Z
Point correspondences
Display 3D plots
100
15050
100
150
200
250
300
350
X
Y
Z
Source Code <main function> void main() { /////////////////////////////////////////////////////////////// // 1 . read stereo images /////////////////////////////////////////////////////////////// IplImage* imgL = cvLoadImage("01.jpg",1); IplImage* imgR = cvLoadImage("02.jpg",1); /////////////////////////////////////////////////////////////// // 2. select corresponding points (at least 8 points) /////////////////////////////////////////////////////////////// // corridor int Npts=10; CvMat* ManualptsL = cvCreateMat(3,Npts,CV_64FC1); // manually selected correspondences in the Left image CvMat* ManualptsR = cvCreateMat(3,Npts,CV_64FC1); // manually selected correspondences in the Left image // point#1 cvmSet(ManualptsL,0,0,625); cvmSet(ManualptsL,1,0,562); cvmSet(ManualptsL,2,0,1); // point#2 cvmSet(ManualptsL,0,1,709); cvmSet(ManualptsL,1,1,233); cvmSet(ManualptsL,2,1,1); // point#3 cvmSet(ManualptsL,0,2,599); cvmSet(ManualptsL,1,2,118); cvmSet(ManualptsL,2,2,1); // point#4 cvmSet(ManualptsL,0,3,486); cvmSet(ManualptsL,1,3,142); cvmSet(ManualptsL,2,3,1); // point#5 cvmSet(ManualptsL,0,4,343); cvmSet(ManualptsL,1,4,142); cvmSet(ManualptsL,2,4,1); // point#6 cvmSet(ManualptsL,0,5,436); cvmSet(ManualptsL,1,5,229); cvmSet(ManualptsL,2,5,1); // point#7 cvmSet(ManualptsL,0,6,388); cvmSet(ManualptsL,1,6,228); cvmSet(ManualptsL,2,6,1); // point#8 cvmSet(ManualptsL,0,7,326); cvmSet(ManualptsL,1,7,391); cvmSet(ManualptsL,2,7,1); // point#9 cvmSet(ManualptsL,0,8,199); cvmSet(ManualptsL,1,8,513); cvmSet(ManualptsL,2,8,1); // point#10 cvmSet(ManualptsL,0,9,352); cvmSet(ManualptsL,1,9,309); cvmSet(ManualptsL,2,9,1); // right image // point#1 cvmSet(ManualptsR,0,0,570); cvmSet(ManualptsR,1,0,569); cvmSet(ManualptsR,2,0,1); // point#2 cvmSet(ManualptsR,0,1,619);
cvmSet(ManualptsR,1,1,222); cvmSet(ManualptsR,2,1,1); // point#3 cvmSet(ManualptsR,0,2,541); cvmSet(ManualptsR,1,2,99); cvmSet(ManualptsR,2,2,1); // point#4 cvmSet(ManualptsR,0,3,454); cvmSet(ManualptsR,1,3,128); cvmSet(ManualptsR,2,3,1); // point#5 cvmSet(ManualptsR,0,4,309); cvmSet(ManualptsR,1,4,128); cvmSet(ManualptsR,2,4,1); // point#6 cvmSet(ManualptsR,0,5,439); cvmSet(ManualptsR,1,5,218); cvmSet(ManualptsR,2,5,1); // point#7 cvmSet(ManualptsR,0,6,391); cvmSet(ManualptsR,1,6,217); cvmSet(ManualptsR,2,6,1); // point#8 cvmSet(ManualptsR,0,7,304); cvmSet(ManualptsR,1,7,382); cvmSet(ManualptsR,2,7,1); // point#9 cvmSet(ManualptsR,0,8,109); cvmSet(ManualptsR,1,8,518); cvmSet(ManualptsR,2,8,1); // point#10 cvmSet(ManualptsR,0,9,337); cvmSet(ManualptsR,1,9,299); cvmSet(ManualptsR,2,9,1); /* // ISS building int Npts=12; CvMat* ManualptsL = cvCreateMat(3,Npts,CV_64FC1); // manually selected correspondences in the Left image CvMat* ManualptsR = cvCreateMat(3,Npts,CV_64FC1); // manually selected correspondences in the Left image // point#1 cvmSet(ManualptsL,0,0,94); cvmSet(ManualptsL,1,0,265); cvmSet(ManualptsL,2,0,1); // point#2 cvmSet(ManualptsL,0,1,216); cvmSet(ManualptsL,1,1,210); cvmSet(ManualptsL,2,1,1); // point#3 cvmSet(ManualptsL,0,2,398); cvmSet(ManualptsL,1,2,107); cvmSet(ManualptsL,2,2,1); // point#4 cvmSet(ManualptsL,0,3,648); cvmSet(ManualptsL,1,3,164); cvmSet(ManualptsL,2,3,1); // point#5 cvmSet(ManualptsL,0,4,692); cvmSet(ManualptsL,1,4,265); cvmSet(ManualptsL,2,4,1); // point#6 cvmSet(ManualptsL,0,5,595); cvmSet(ManualptsL,1,5,488); cvmSet(ManualptsL,2,5,1); // point#7 cvmSet(ManualptsL,0,6,187); cvmSet(ManualptsL,1,6,382); cvmSet(ManualptsL,2,6,1); // point#8 cvmSet(ManualptsL,0,7,370); cvmSet(ManualptsL,1,7,284);
cvmSet(ManualptsL,2,7,1); // point#9 cvmSet(ManualptsL,0,8,183); cvmSet(ManualptsL,1,8,494); cvmSet(ManualptsL,2,8,1); // point#10 cvmSet(ManualptsL,0,9,727); cvmSet(ManualptsL,1,9,523); cvmSet(ManualptsL,2,9,1); // point#11 cvmSet(ManualptsL,0,10,498); cvmSet(ManualptsL,1,10,287); cvmSet(ManualptsL,2,10,1); // point#12 cvmSet(ManualptsL,0,11,381); cvmSet(ManualptsL,1,11,59); cvmSet(ManualptsL,2,11,1); // right image // point#1 cvmSet(ManualptsR,0,0,109); cvmSet(ManualptsR,1,0,300); cvmSet(ManualptsR,2,0,1); // point#2 cvmSet(ManualptsR,0,1,199); cvmSet(ManualptsR,1,1,245); cvmSet(ManualptsR,2,1,1); // point#3 cvmSet(ManualptsR,0,2,354); cvmSet(ManualptsR,1,2,142); cvmSet(ManualptsR,2,2,1); // point#4 cvmSet(ManualptsR,0,3,608); cvmSet(ManualptsR,1,3,193); cvmSet(ManualptsR,2,3,1); // point#5 cvmSet(ManualptsR,0,4,652); cvmSet(ManualptsR,1,4,292); cvmSet(ManualptsR,2,4,1); // point#6 cvmSet(ManualptsR,0,5,553); cvmSet(ManualptsR,1,5,516); cvmSet(ManualptsR,2,5,1); // point#7 cvmSet(ManualptsR,0,6,182); cvmSet(ManualptsR,1,6,413); cvmSet(ManualptsR,2,6,1); // point#8 cvmSet(ManualptsR,0,7,329); cvmSet(ManualptsR,1,7,315); cvmSet(ManualptsR,2,7,1); // point#9 cvmSet(ManualptsR,0,8,177); cvmSet(ManualptsR,1,8,522); cvmSet(ManualptsR,2,8,1); // point#10 cvmSet(ManualptsR,0,9,689); cvmSet(ManualptsR,1,9,549); cvmSet(ManualptsR,2,9,1); // point#11 cvmSet(ManualptsR,0,10,454); cvmSet(ManualptsR,1,10,317); cvmSet(ManualptsR,2,10,1); // point#12 cvmSet(ManualptsR,0,11,334); cvmSet(ManualptsR,1,11,98); cvmSet(ManualptsR,2,11,1); */ /////////////////////////////////////////////////////////////// // Determine Fundamental matrix
/////////////////////////////////////////////////////////////// CvMat* F = cvCreateMat(3,3,CV_64FC1); CvMat* Ft = cvCreateMat(3,3,CV_64FC1); CvMat* eL = cvCreateMat(3,1,CV_64FC1); CvMat* eR = cvCreateMat(3,1,CV_64FC1); CvMat* M = cvCreateMat(3,3,CV_64FC1); CvMat* PL = cvCreateMat(3,4,CV_64FC1); CvMat* PR = cvCreateMat(3,4,CV_64FC1); //determineF_8pts(F,eL,eR,M,ManualptsL,ManualptsR,Npts,imgL,imgR,1); //setCamerasbyF(F,eR,PL,PR); determineF_GSA(F,eL,eR,M,PL,PR,ManualptsL,ManualptsR,Npts,imgL,imgR,0); //cvWaitKey(0); cvTranspose(F,Ft); // check epipoles and F int i; CvMat* pt1 = cvCreateMat(3,1,CV_64FC1); CvMat* pt2 = cvCreateMat(1,3,CV_64FC1); CvMat* Tcheck=cvCreateMat(1,3,CV_64FC1); CvMat* check =cvCreateMat(1,1,CV_64FC1); CvMat* check2=cvCreateMat(3,1,CV_64FC1); printf("check fundamental matrix\n"); for(i=0;i<Npts;i++) { cvmSet(pt1,0,0,cvmGet(ManualptsL,0,i)); cvmSet(pt1,1,0,cvmGet(ManualptsL,1,i)); cvmSet(pt1,2,0,cvmGet(ManualptsL,2,i)); cvmSet(pt2,0,0,cvmGet(ManualptsR,0,i)); cvmSet(pt2,0,1,cvmGet(ManualptsR,1,i)); cvmSet(pt2,0,2,cvmGet(ManualptsR,2,i)); cvMatMul(pt2,F,Tcheck); cvMatMul(Tcheck,pt1,check); printf("%2d]x'Fx=%f\n",i,cvmGet(check,0,0)); } printf("check epipoles\n"); cvMatMul(F,eL,check2); printf("Fe=[%f,%f,%f]\n",cvmGet(check2,0,0),cvmGet(check2,1,0),cvmGet(check2,2,0)); printf("check epipoles\n"); cvMatMul(Ft,eR,check2); printf("Fe=[%f,%f,%f]\n",cvmGet(check2,0,0),cvmGet(check2,1,0),cvmGet(check2,2,0)); /////////////////////////////////////////////////////////////// // Image Rectification /////////////////////////////////////////////////////////////// CvMat* HL = cvCreateMat(3,3,CV_64FC1); CvMat* HR = cvCreateMat(3,3,CV_64FC1); // edited imgL = cvLoadImage("01.jpg",0); imgR = cvLoadImage("02.jpg",0); char filenameL[]="rectifiedL.jpg"; char filenameR[]="rectifiedR.jpg"; RectificationH(imgL,imgR,F,M,eL,eR,ManualptsL,ManualptsR,Npts,HL,HR,filenameL,filenameR,0); //cvWaitKey(0); /////////////////////////////////////////////////////////////// // apply canny edge detector /////////////////////////////////////////////////////////////// IplImage* RectL = cvLoadImage(filenameL,0); IplImage* RectR = cvLoadImage(filenameR,0); IplImage* CannyL = cvCreateImage( cvGetSize(RectL), 8, 1 ); IplImage* CannyR = cvCreateImage( cvGetSize(RectR), 8, 1 );
cvCanny( RectL, CannyL, 50, 300, 3 ); cvCanny( RectR, CannyR, 50, 300, 3 ); cvSaveImage("CannyL.jpg", CannyL); cvSaveImage("CannyR.jpg", CannyR); //cvNamedWindow( "CannyL", 1 ); //cvShowImage( "CannyL", CannyL ); //cvNamedWindow( "CannyR", 1 ); //cvShowImage( "CannyR", CannyR ); RectL = cvLoadImage(filenameL,1); RectR = cvLoadImage(filenameR,1);
/////////////////////////////////////////////////////////////// // matching /////////////////////////////////////////////////////////////// CvPoint Draw; int j,k,l,ii,jj; dnode *ptr; init_list(); int kstart,kend,lstart,lend; int band=2; int horiz=10; int maxk,maxl,prel; double mean1,mean2,stdv1,stdv2; int Template=9; double thereshold=.85; int Nmatched=0; int RemoveBoundary=100; for(i=RemoveBoundary;i<CannyL->height-RemoveBoundary;i++) { if(i/25==i/25.) printf("line #%d\n",i); int initial=0; for(j=RemoveBoundary;j<CannyL->width-RemoveBoundary;j++) { if((unsigned char)CannyL->imageData[i*CannyL->width+j]>0) { // setup search area using bandwidth and thereshold for the horizontal direction if(i-band<0) { kstart=0; kend=i+band; } else if(i+band>=CannyL->height) { kstart=i-band; kend=CannyL->height-1; } else { kstart=i-band; kend=i+band; } if(j-horiz<0) { lstart=0; lend=j+horiz; } else if(j+horiz>=CannyL->width) { lstart=j-horiz; lend=CannyL->width-1; } else { // ISS building //lstart=j-10; //lend=j+5; // Corridor lstart=j-30; lend=j+5; } ///////////////////////// // search double NCCmax=0; double NCCtemp=0; double predist; double currentdist; for(k=kstart;k<=kend;k++) { for(l=lstart;l<=lend;l++) {
if((unsigned char)CannyR->imageData[k*CannyR->width+l]>0) { // calculate NCC mean1=0; mean2=0; //for(ii=-(Template/2);ii<Template/2.;ii++) //{ //for(jj=-(Template/2);jj<Template/2.;jj++) //{ for(ii=0;ii<Template;ii++) { for(jj=0;jj<Template;jj++) { mean1=mean1+(unsigned char)RectL->imageData[(i+ii)*RectL->width+(j+jj)]; mean2=mean2+(unsigned char)RectR->imageData[(k+ii)*RectR->width+(l+jj)]; } } mean1=mean1/(Template*Template); mean2=mean2/(Template*Template); stdv1=0; stdv2=0; //for(ii=-(Template/2);ii<Template/2.;ii++) //{ //for(jj=-(Template/2);jj<Template/2.;jj++) //{ for(ii=0;ii<Template;ii++) { for(jj=0;jj<Template;jj++) { stdv1=stdv1+pow((unsigned char)RectL->imageData[(i+ii)*RectL->width+(j+jj)]-mean1,2); stdv2=stdv2+pow((unsigned char)RectR->imageData[(k+ii)*RectR->width+(l+jj)]-mean2,2); } } double NCC=0; //for(ii=-(Template/2);ii<Template/2.;ii++) //{ //for(jj=-(Template/2);jj<Template/2.;jj++) //{ for(ii=0;ii<Template;ii++) { for(jj=0;jj<Template;jj++) { NCC=NCC+((unsigned char)RectL->imageData[(i+ii)*RectL->width+(j+jj)]-mean1)*((unsigned char)RectR->imageData[(k+ii)*RectR->width+(l+jj)]-mean2); } } NCCtemp=NCC/sqrt(stdv1*stdv2); // check max if(NCCtemp>NCCmax) { NCCmax=NCCtemp; maxk=k; maxl=l; } } } } // check threshold & store point info. if(NCCmax>thereshold) { // check order if(initial==0) { insert(j,i,maxl,maxk); Nmatched++; prel=maxl;
initial=1; predist=sqrt((i-k)*(i-k)+(j-l)*(j-l)); } currentdist=sqrt((i-k)*(i-k)+(j-l)*(j-l)); //if(initial>0 && maxl>prel && currentdist/predist>.9 && currentdist/predist<1.1 ) if(initial>0 && maxl>prel) { insert(j,i,maxl,maxk); Nmatched++; Draw.x=j; Draw.y=i; draw_cross(RectL,Draw,CV_RGB(0,0,255),3); Draw.x=maxl; Draw.y=maxk; draw_cross(RectR,Draw,CV_RGB(0,0,255),3); prel=maxl; } } } } } printf("Total %d points are matched\n",Nmatched); cvNamedWindow( "RectL", 1 ); cvShowImage( "RectL", RectL ); cvNamedWindow( "RectR", 1 ); cvShowImage( "RectR", RectR ); /////////////////////////////////////////////////////////////// // intersect /////////////////////////////////////////////////////////////// FILE *report,*passpts; report=fopen("result.dat","wt"); passpts=fopen("passpts.dat","wt"); CvMat* HPL = cvCreateMat(3,4,CV_64FC1); CvMat* HPR = cvCreateMat(3,4,CV_64FC1); CvMat* pt_l = cvCreateMat(3,1,CV_64FC1); CvMat* pt_r = cvCreateMat(3,1,CV_64FC1); CvMat* ptt = cvCreateMat(3,1,CV_64FC1); CvMat* X = cvCreateMat(4,1,CV_64FC1); cvMatMul(HL,PL,HPL); cvMatMul(HR,PR,HPR); ptr=head->next; for(i=0;i<Nmatched;i++) { cvmSet(pt_l,0,0,ptr->x1); cvmSet(pt_l,1,0,ptr->y1); cvmSet(pt_l,2,0,1); fprintf(passpts,"%f\t%f\t",cvmGet(pt_l,0,0),cvmGet(pt_l,1,0)); cvmSet(pt_r,0,0,ptr->x2); cvmSet(pt_r,1,0,ptr->y2); cvmSet(pt_r,2,0,1); fprintf(passpts,"%f\t%f\n",cvmGet(pt_r,0,0),cvmGet(pt_r,1,0)); LinearTriangulation(HPL,pt_l,HPR,pt_r,X,1); fprintf(report,"%f\t%f\t%f\n",cvmGet(X,0,0),cvmGet(X,1,0),cvmGet(X,2,0)); ptr=ptr->next; } fclose(report); fclose(passpts); cvWaitKey(0); cvReleaseImage(&imgL); cvReleaseImage(&imgR); cvReleaseImage(&CannyL); cvReleaseImage(&CannyR); cvReleaseImage(&RectL); cvReleaseImage(&RectR); cvReleaseMat(&ManualptsL); cvReleaseMat(&ManualptsR); cvReleaseMat(&F); cvReleaseMat(&Ft); cvReleaseMat(&eL); cvReleaseMat(&eR);
cvReleaseMat(&M); cvReleaseMat(&pt1); cvReleaseMat(&pt2); cvReleaseMat(&Tcheck); cvReleaseMat(&check); cvReleaseMat(&check2); cvReleaseMat(&HR); cvReleaseMat(&HL); }
<Included functions> // determine Fundamental Matrix void determineF_8pts(CvMat *F,CvMat *eL,CvMat *eR,CvMat *M, CvMat* CornersL, CvMat* CornersR, int Npts, IplImage *imgL, IplImage *imgR, int Viewflag); void determineF_GSA(CvMat *F,CvMat *eL,CvMat *eR, CvMat *M,CvMat *PL,CvMat *PR,CvMat* CornersL, CvMat* CornersR, int Npts, IplImage *imgL, IplImage *imgR, int Viewflag); // Linear triangulation method void LinearTriangulation(CvMat *PL,CvMat *x,CvMat *PR,CvMat *x2,CvMat *X, int Npts); // Set Camera matrix PL & PR to estimate F void setCamerasbyF(CvMat *F,CvMat *eR,CvMat *PL,CvMat *PR); // Set Jacobian matrix to estimate F using the Golden Standard Algorithm void setJFundamental(CvMat *PR,CvMat *X,CvMat *CornersL, CvMat *CornersR,CvMat *J,int Npts); // draw epipolar lines void DrawEpiLines(CvMat *vector, IplImage *img); // Image Rectification void RectificationH(IplImage* imgL,IplImage* imgR,CvMat *F,CvMat *M,CvMat *eL,CvMat *eR,CvMat *CornersL,CvMat *CornersR,int Npts,CvMat *HL,CvMat *HR,char *filenameL, char *filenameR, int Viewflag); // Bilinear interpolation unsigned char Bilinear(IplImage* img,double x,double y); // construct Normalization matrix for image coordinates double NormalizationMatrixImg(CvMat *Corners1,int N1,CvMat *MT); // estimate geometric distances double ErrorsGDforF(CvMat* PL,CvMat* PR,CvMat* ObjectPoints,CvMat* ImgPointsL,CvMat* ImgPointsR,int N, CvMat* d);
< function determineF_8pts > Input : - F : 3x3 fundamental matrix - eL : epipole in the left image - eR : epipole in the rightt image - M : first 3x3 element of the right camera matrix - CornersL : corresponding left image coordinates - CornersR : corresponding right image coordinates - Npts : number of points - imgL : left image - imgR : right image Output : estimated F,eL,eR,M void determineF_8pts(CvMat *F,CvMat *eL,CvMat *eR, CvMat *M, CvMat* CornersL, CvMat* CornersR, int Npts, IplImage *imgL, IplImage *imgR, int Viewflag){ if(Npts<8) { printf("Minumum 8 correspondences are needed to determine F using this algorithm!\n"); exit; } int i,j; ////////////////////////////////////////////////// // Normalize coordinates ////////////////////////////////////////////////// CvMat* T = cvCreateMat(3,3,CV_64FC1); CvMat* Tp = cvCreateMat(3,3,CV_64FC1); CvMat* Tpt = cvCreateMat(3,3,CV_64FC1); CvMat* nCornersL = cvCreateMat(3,Npts,CV_64FC1); CvMat* nCornersR = cvCreateMat(3,Npts,CV_64FC1); double scale1,scale2; scale1=NormalizationMatrixImg(CornersL,Npts,T); scale2=NormalizationMatrixImg(CornersR,Npts,Tp); cvMatMul(T,CornersL,nCornersL); cvMatMul(Tp,CornersR,nCornersR); cvTranspose(Tp,Tpt); ////////////////////////////////////////////////// // estimate Fn ////////////////////////////////////////////////// CvMat* A = cvCreateMat(Npts,9,CV_64FC1); CvMat* U = cvCreateMat(Npts,Npts,CV_64FC1); CvMat* D = cvCreateMat(Npts,9,CV_64FC1); CvMat* V = cvCreateMat(9,9,CV_64FC1); CvMat* Fn = cvCreateMat(3,3,CV_64FC1); CvMat* Fnc = cvCreateMat(3,3,CV_64FC1); CvMat* Ftemp = cvCreateMat(3,3,CV_64FC1); CvMat* Ft = cvCreateMat(3,3,CV_64FC1); for(j=0;j<Npts;j++) { cvmSet(A,j,0,cvmGet(nCornersR,0,j)*cvmGet(nCornersL,0,j)); cvmSet(A,j,1,cvmGet(nCornersR,0,j)*cvmGet(nCornersL,1,j)); cvmSet(A,j,2,cvmGet(nCornersR,0,j) ); cvmSet(A,j,3,cvmGet(nCornersR,1,j)*cvmGet(nCornersL,0,j)); cvmSet(A,j,4,cvmGet(nCornersR,1,j)*cvmGet(nCornersL,1,j)); cvmSet(A,j,5,cvmGet(nCornersR,1,j) ); cvmSet(A,j,6,cvmGet(nCornersL,0,j) ); cvmSet(A,j,7,cvmGet(nCornersL,1,j) ); cvmSet(A,j,8,1 ); } cvSVD(A, D, U, V, CV_SVD_V_T); cvmSet(Fn,0,0,cvmGet(V,8,0)); cvmSet(Fn,0,1,cvmGet(V,8,1)); cvmSet(Fn,0,2,cvmGet(V,8,2)); cvmSet(Fn,1,0,cvmGet(V,8,3)); cvmSet(Fn,1,1,cvmGet(V,8,4));
cvmSet(Fn,1,2,cvmGet(V,8,5)); cvmSet(Fn,2,0,cvmGet(V,8,6)); cvmSet(Fn,2,1,cvmGet(V,8,7)); cvmSet(Fn,2,2,cvmGet(V,8,8)); ////////////////////////////////////////////////// // Constraint enforcement (Singularity condition) : Fnc ////////////////////////////////////////////////// CvMat* U2 = cvCreateMat(3,3,CV_64FC1); CvMat* D2 = cvCreateMat(3,3,CV_64FC1); CvMat* V2 = cvCreateMat(3,3,CV_64FC1); CvMat* T2 = cvCreateMat(3,3,CV_64FC1); cvSVD(Fn, D2, U2, V2, CV_SVD_V_T); cvmSet(D2,2,2,0); cvMatMul(U2,D2,T2); cvMatMul(T2,V2,Fnc); ////////////////////////////////////////////////// // Denormalize F=Tp'*Fnc*T ////////////////////////////////////////////////// cvMatMul(Tpt,Fnc,Ftemp); cvMatMul(Ftemp,T,F); cvTranspose(F,Ft); // find epipoles cvSVD(F, D2, U2, V2, CV_SVD_V_T); cvmSet(eL,0,0,cvmGet(V2,2,0)/cvmGet(V2,2,2)); cvmSet(eL,1,0,cvmGet(V2,2,1)/cvmGet(V2,2,2)); cvmSet(eL,2,0,cvmGet(V2,2,2)/cvmGet(V2,2,2)); cvmSet(eR,0,0,cvmGet(U2,0,2)/cvmGet(U2,2,2)); cvmSet(eR,1,0,cvmGet(U2,1,2)/cvmGet(U2,2,2)); cvmSet(eR,2,0,cvmGet(U2,2,2)/cvmGet(U2,2,2)); // setup M // setup [e]x CvMat* ex = cvCreateMat(3,3,CV_64FC1); cvmSet(ex,0,0, 0 ); cvmSet(ex,0,1,-cvmGet(eR,2,0)); cvmSet(ex,0,2, cvmGet(eR,1,0)); cvmSet(ex,1,0, cvmGet(eR,1,0)); cvmSet(ex,1,1, 0 ); cvmSet(ex,1,2,-cvmGet(eR,0,0)); cvmSet(ex,2,0,-cvmGet(eR,1,0)); cvmSet(ex,2,1, cvmGet(eR,0,0)); cvmSet(ex,2,2, 0 ); // setup M cvMatMul(ex,F,M); // Drawing if(Viewflag==1) { // display left images CvPoint Draw; CvMat* line = cvCreateMat(3,1,CV_64FC1); CvMat* pt = cvCreateMat(3,1,CV_64FC1); char text[20]; CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 0.8, 0.8, 0, 1); FILE *pointid; pointid=fopen("pointid.txt","rt"); // left image cvPutText(imgL,"Left image",cvPoint(20,20),&font,CV_RGB(255,255,255)); for(i=0;i<Npts;i++) { cvmSet(pt,0,0,cvmGet(CornersR,0,i)); cvmSet(pt,1,0,cvmGet(CornersR,1,i)); cvmSet(pt,2,0,cvmGet(CornersR,2,i)); cvMatMul(Ft,pt,line); DrawEpiLines(line,imgL); fscanf(pointid,"%s",text); Draw.x=cvmGet(CornersL,0,i);
Draw.y=cvmGet(CornersL,1,i); draw_cross(imgL,Draw,CV_RGB(255,255,255),3); cvPutText(imgL,text,Draw,&font,CV_RGB(255,255,255)); } Draw.x=cvmGet(eL,0,0)/cvmGet(eL,2,0); Draw.y=cvmGet(eL,1,0)/cvmGet(eL,2,0); draw_cross(imgL,Draw,CV_RGB(255,0,0),3); cvPutText(imgL,"epipole",Draw,&font,CV_RGB(255,0,0)); cvNamedWindow( "Left", 1 ); cvShowImage( "Left", imgL ); // display right images cvPutText(imgR,"Right image",cvPoint(20,20),&font,CV_RGB(255,255,255)); pointid=fopen("pointid.txt","rt"); for(i=0;i<Npts;i++) { cvmSet(pt,0,0,cvmGet(CornersL,0,i)); cvmSet(pt,1,0,cvmGet(CornersL,1,i)); cvmSet(pt,2,0,cvmGet(CornersL,2,i)); cvMatMul(F,pt,line); DrawEpiLines(line,imgR); fscanf(pointid,"%s",text); Draw.x=cvmGet(CornersR,0,i); Draw.y=cvmGet(CornersR,1,i); draw_cross(imgR,Draw,CV_RGB(255,255,255),3); cvPutText(imgR,text,Draw,&font,CV_RGB(255,255,255)); } Draw.x=cvmGet(eR,0,0)/cvmGet(eR,2,0); Draw.y=cvmGet(eR,1,0)/cvmGet(eR,2,0); draw_cross(imgR,Draw,CV_RGB(255,0,0),3); cvPutText(imgL,"epipole",Draw,&font,CV_RGB(255,0,0)); cvNamedWindow( "Right", 1 ); cvShowImage( "Right", imgR ); cvWaitKey(0); } // release CvMat cvReleaseMat(&nCornersL); cvReleaseMat(&nCornersR); cvReleaseMat(&T); cvReleaseMat(&Tp); cvReleaseMat(&Tpt); cvReleaseMat(&A); cvReleaseMat(&U); cvReleaseMat(&D); cvReleaseMat(&V); cvReleaseMat(&Fn); cvReleaseMat(&Fnc); cvReleaseMat(&Ftemp); cvReleaseMat(&Ft); cvReleaseMat(&U2); cvReleaseMat(&D2); cvReleaseMat(&V2); cvReleaseMat(&T2); } < function determineF_GSA (GSA stands for the Gold Standard Algorithm) > Input : - F : 3x3 fundamental matrix - eL : epipole in the left image - eR : epipole in the rightt image - PL : 3x4 left camera matrix - PR : 3x4 right camera matrix - M : first 3x3 element of the right camera matrix - CornersL : corresponding left image coordinates - CornersR : corresponding right image coordinates - Npts : number of points
- imgL : left image - imgR : right image - Viewflag : if 1 show the results, else do nothing. Output : estimated F,eL,eR,M,PL,PR void determineF_GSA(CvMat *F,CvMat *eL,CvMat *eR, CvMat *M,CvMat *PL,CvMat *PR,CvMat* CornersL, CvMat* CornersR, int Npts, IplImage *imgL, IplImage *imgR, int Viewflag){ int i,j; if(Npts<8) { printf("Minumum 8 correspondences are needed to determine F using this algorithm!\n"); exit; } //////////////////////////////////////////////////////////////////////////// // step 1. Compute initial rank 2 estimate of F using a linear algorithm //////////////////////////////////////////////////////////////////////////// determineF_8pts(F,eL,eR,M,CornersL,CornersR,Npts,imgL,imgR,0); //////////////////////////////////////////////////////////////////////////// // step 2. Compute an initial estimate of the subsidiary variables //////////////////////////////////////////////////////////////////////////// // (a) Choose camera matrices P=[I|0] and P2=[[e']x F | e' ] // , where e' is obtained from F setCamerasbyF(F,eR,PL,PR); // (b) from the correspondence CornersL & CornersR, determine an estimate of X // using the linear triangulation method CvMat* X = cvCreateMat(4,Npts,CV_64FC1); LinearTriangulation(PL,CornersL,PR,CornersR,X,Npts); // (c) the correspondence consistent with F is obtained as estCornersL=PX, estCornersR=P2X CvMat* estCornersL = cvCreateMat(3,Npts,CV_64FC1); CvMat* estCornersR = cvCreateMat(3,Npts,CV_64FC1); cvMatMul(PL,X,estCornersL); cvMatMul(PR,X,estCornersR); //////////////////////////////////////////////////////////////////////////// // step 3. Minimize the cost function (puitative distances) // using the LM algorithm over n*3+12 unknowns //////////////////////////////////////////////////////////////////////////// double threshold=1e-6; double error,error0,error2,delta=100; int Iteration=0; double damping=0.01; // create initial damping factor // setup identity matrix CvMat* I = cvCreateMat(3*Npts+12,3*Npts+12,CV_64FC1); for(i=0;i<3*Npts+12;i++) { for(j=0;j<3*Npts+12;j++) { if(i==j) cvmSet(I,i,j,1); else cvmSet(I,i,j,0); } } // Assign Initial Values CvMat *Param = cvCreateMat(3*Npts+12,1,CV_64FC1); CvMat *Ptemp = cvCreateMat(3*Npts+12,1,CV_64FC1); CvMat *dP = cvCreateMat(3*Npts+12,1,CV_64FC1); cvmSet(Param, 0,0,cvmGet(PR,0,0)); cvmSet(Param, 1,0,cvmGet(PR,0,1)); cvmSet(Param, 2,0,cvmGet(PR,0,2)); cvmSet(Param, 3,0,cvmGet(PR,0,3)); cvmSet(Param, 4,0,cvmGet(PR,1,0)); cvmSet(Param, 5,0,cvmGet(PR,1,1)); cvmSet(Param, 6,0,cvmGet(PR,1,2));
cvmSet(Param, 7,0,cvmGet(PR,1,3)); cvmSet(Param, 8,0,cvmGet(PR,2,0)); cvmSet(Param, 9,0,cvmGet(PR,2,1)); cvmSet(Param,10,0,cvmGet(PR,2,2)); cvmSet(Param,11,0,cvmGet(PR,2,3)); for(i=0;i<Npts;i++) { cvmSet(Param,12+i*3 ,0,cvmGet(X,0,i)); cvmSet(Param,13+i*3 ,0,cvmGet(X,1,i)); cvmSet(Param,14+i*3 ,0,cvmGet(X,2,i)); } // Initial geometric distance CvMat *d = cvCreateMat(Npts,1,CV_64FC1); CvMat *d2 = cvCreateMat(Npts,1,CV_64FC1); error0=ErrorsGDforF(PL,PR,X,CornersL,CornersR,Npts,d); printf("Initial error=%f\n",error0); // Iterate using LM method CvMat* J = cvCreateMat(Npts ,3*Npts+12,CV_64FC1); CvMat* JT = cvCreateMat(3*Npts+12, Npts,CV_64FC1); CvMat* JTd = cvCreateMat(3*Npts+12, 1,CV_64FC1); CvMat* Hessian = cvCreateMat(3*Npts+12,3*Npts+12,CV_64FC1); CvMat* Hessian_lm = cvCreateMat(3*Npts+12,3*Npts+12,CV_64FC1); CvMat* Hessian_lminv = cvCreateMat(3*Npts+12,3*Npts+12,CV_64FC1); CvMat* PRtemp = cvCreateMat(3 ,4 ,CV_64FC1); CvMat* Xtemp = cvCreateMat(4 ,Npts ,CV_64FC1); int updateJ=1; error=error0; for(i=0;i<200;i++) { if(updateJ==1) { // determine Jacobian matrix J setJFundamental(Param,X,CornersL,CornersR,J,Npts); // Hessian Matrix cvTranspose(J,JT); cvMatMul(JT,J,Hessian); } // apply damping factor cvScaleAdd(I,cvScalar(damping),Hessian,Hessian_lm); cvInvert(Hessian_lm,Hessian_lminv); // temporary update P Ptemp=P-inv(H_lm)*(J'*d) cvMatMul(JT,d,JTd); cvMatMul(Hessian_lminv,JTd,dP); cvScaleAdd(dP,cvScalar(-1),Param,Ptemp); delta=0; for(j=0;j<12+3*Npts;j++) delta=delta+pow(cvmGet(dP,j,0),2); delta=sqrt(delta)/(12+3*Npts); // check errors by Ptemp cvmSet(PRtemp,0,0,cvmGet(Ptemp, 0,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,0,1,cvmGet(Ptemp, 1,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,0,2,cvmGet(Ptemp, 2,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,0,3,cvmGet(Ptemp, 3,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,1,0,cvmGet(Ptemp, 4,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,1,1,cvmGet(Ptemp, 5,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,1,2,cvmGet(Ptemp, 6,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,1,3,cvmGet(Ptemp, 7,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,2,0,cvmGet(Ptemp, 8,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,2,1,cvmGet(Ptemp, 9,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,2,2,cvmGet(Ptemp,10,0)/cvmGet(Ptemp,11,0)); cvmSet(PRtemp,2,3,cvmGet(Ptemp,11,0)/cvmGet(Ptemp,11,0)); for(j=0;j<Npts;j++) { cvmSet(Xtemp,0,j,cvmGet(Ptemp,12+3*j,0)); cvmSet(Xtemp,1,j,cvmGet(Ptemp,13+3*j,0)); cvmSet(Xtemp,2,j,cvmGet(Ptemp,14+3*j,0)); cvmSet(Xtemp,3,j, 1 ); } error2=ErrorsGDforF(PL,PRtemp,Xtemp,CornersL,CornersR,Npts,d2); if(error2<error) //decrease damping factor and update {
damping=damping/10; // update parameters cvCopy(Ptemp,Param,0); cvCopy(d2,d,0); error=error2; updateJ=1; printf("iteration %d ] error=%f\n",i+1,error); printf(" delta=%f\n",delta); if(delta<threshold) updateJ=3; } else // increase damping factor and try again { updateJ=0; i=i-1; damping=damping*10; } if(updateJ==3) break; } printf("Number of iteration = %d\n",i+1); printf("delta = %f\n",delta); printf("error=%f\n",error); printf("damping=%f\n",damping); // Update estimated F CvMat* tx = cvCreateMat(3,3,CV_64FC1); CvMat* Ftemp= cvCreateMat(3,3,CV_64FC1); CvMat* Ft = cvCreateMat(3,3,CV_64FC1); // setup [t]x cvmSet(tx,0,0, 0 ); cvmSet(tx,0,1,-cvmGet(Param,11,0)); cvmSet(tx,0,2, cvmGet(Param, 7,0)); cvmSet(tx,1,0, cvmGet(Param,11,0)); cvmSet(tx,1,1, 0 ); cvmSet(tx,1,2,-cvmGet(Param, 3,0)); cvmSet(tx,2,0,-cvmGet(Param, 7,0)); cvmSet(tx,2,1, cvmGet(Param, 3,0)); cvmSet(tx,2,2, 0 ); // setup M cvmSet(M,0,0,cvmGet(Param, 0,0)); cvmSet(M,0,1,cvmGet(Param, 1,0)); cvmSet(M,0,2,cvmGet(Param, 2,0)); cvmSet(M,1,0,cvmGet(Param, 4,0)); cvmSet(M,1,1,cvmGet(Param, 5,0)); cvmSet(M,1,2,cvmGet(Param, 6,0)); cvmSet(M,2,0,cvmGet(Param, 8,0)); cvmSet(M,2,1,cvmGet(Param, 9,0)); cvmSet(M,2,2,cvmGet(Param,10,0)); // F=[t]xM cvMatMul(tx,M,F); cvmSet(F,0,0,cvmGet(F,0,0)/cvmGet(F,2,2)); cvmSet(F,0,1,cvmGet(F,0,1)/cvmGet(F,2,2)); cvmSet(F,0,2,cvmGet(F,0,2)/cvmGet(F,2,2)); cvmSet(F,1,0,cvmGet(F,1,0)/cvmGet(F,2,2)); cvmSet(F,1,1,cvmGet(F,1,1)/cvmGet(F,2,2)); cvmSet(F,1,2,cvmGet(F,1,2)/cvmGet(F,2,2)); cvmSet(F,2,0,cvmGet(F,2,0)/cvmGet(F,2,2)); cvmSet(F,2,1,cvmGet(F,2,1)/cvmGet(F,2,2)); cvmSet(F,2,2,cvmGet(F,2,2)/cvmGet(F,2,2)); cvTranspose(F,Ft); cvmSet(PR,0,0,cvmGet(PRtemp,0,0)/cvmGet(PRtemp,2,3)); cvmSet(PR,0,1,cvmGet(PRtemp,0,1)/cvmGet(PRtemp,2,3)); cvmSet(PR,0,2,cvmGet(PRtemp,0,2)/cvmGet(PRtemp,2,3)); cvmSet(PR,0,3,cvmGet(PRtemp,0,3)/cvmGet(PRtemp,2,3)); cvmSet(PR,1,0,cvmGet(PRtemp,1,0)/cvmGet(PRtemp,2,3)); cvmSet(PR,1,1,cvmGet(PRtemp,1,1)/cvmGet(PRtemp,2,3)); cvmSet(PR,1,2,cvmGet(PRtemp,1,2)/cvmGet(PRtemp,2,3)); cvmSet(PR,1,3,cvmGet(PRtemp,1,3)/cvmGet(PRtemp,2,3)); cvmSet(PR,2,0,cvmGet(PRtemp,2,0)/cvmGet(PRtemp,2,3)); cvmSet(PR,2,1,cvmGet(PRtemp,2,1)/cvmGet(PRtemp,2,3)); cvmSet(PR,2,2,cvmGet(PRtemp,2,2)/cvmGet(PRtemp,2,3)); cvmSet(PR,2,3,cvmGet(PRtemp,2,3)/cvmGet(PRtemp,2,3));
// find epipoles CvMat* U = cvCreateMat(3,3,CV_64FC1); CvMat* D = cvCreateMat(3,3,CV_64FC1); CvMat* V = cvCreateMat(3,3,CV_64FC1); cvSVD(F, D, U, V, CV_SVD_V_T); cvmSet(eL,0,0,cvmGet(V,2,0)/cvmGet(V,2,2)); cvmSet(eL,1,0,cvmGet(V,2,1)/cvmGet(V,2,2)); cvmSet(eL,2,0,cvmGet(V,2,2)/cvmGet(V,2,2)); cvmSet(eR,0,0,cvmGet(U,0,2)/cvmGet(U,2,2)); cvmSet(eR,1,0,cvmGet(U,1,2)/cvmGet(U,2,2)); cvmSet(eR,2,0,cvmGet(U,2,2)/cvmGet(U,2,2)); // Drawing if(Viewflag==1) { // display left images CvPoint Draw; CvMat* line = cvCreateMat(3,1,CV_64FC1); CvMat* pt = cvCreateMat(3,1,CV_64FC1); char text[20]; CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 0.8, 0.8, 0, 1); FILE *pointid; pointid=fopen("pointid.txt","rt"); // left image cvPutText(imgL,"Left image",cvPoint(20,20),&font,CV_RGB(255,255,255)); for(i=0;i<Npts;i++) { cvmSet(pt,0,0,cvmGet(CornersR,0,i)); cvmSet(pt,1,0,cvmGet(CornersR,1,i)); cvmSet(pt,2,0,cvmGet(CornersR,2,i)); cvMatMul(Ft,pt,line); DrawEpiLines(line,imgL); fscanf(pointid,"%s",text); Draw.x=cvmGet(CornersL,0,i); Draw.y=cvmGet(CornersL,1,i); draw_cross(imgL,Draw,CV_RGB(255,0,0),3); cvPutText(imgL,text,Draw,&font,CV_RGB(255,255,255)); } Draw.x=cvmGet(eL,0,0)/cvmGet(eL,2,0); Draw.y=cvmGet(eL,1,0)/cvmGet(eL,2,0); draw_cross(imgL,Draw,CV_RGB(255,0,0),3); cvPutText(imgL,"epipole",Draw,&font,CV_RGB(255,0,0)); cvNamedWindow( "Left(GSA)", 1 ); cvShowImage( "Left(GSA)", imgL ); // display right images cvPutText(imgR,"Right image",cvPoint(20,20),&font,CV_RGB(255,255,255)); pointid=fopen("pointid.txt","rt"); for(i=0;i<Npts;i++) { cvmSet(pt,0,0,cvmGet(CornersL,0,i)); cvmSet(pt,1,0,cvmGet(CornersL,1,i)); cvmSet(pt,2,0,cvmGet(CornersL,2,i)); cvMatMul(F,pt,line); DrawEpiLines(line,imgR); fscanf(pointid,"%s",text); Draw.x=cvmGet(CornersR,0,i); Draw.y=cvmGet(CornersR,1,i); draw_cross(imgR,Draw,CV_RGB(255,0,0),3); cvPutText(imgR,text,Draw,&font,CV_RGB(255,255,255)); } Draw.x=cvmGet(eR,0,0)/cvmGet(eR,2,0); Draw.y=cvmGet(eR,1,0)/cvmGet(eR,2,0); draw_cross(imgR,Draw,CV_RGB(255,0,0),3); cvPutText(imgL,"epipole",Draw,&font,CV_RGB(255,0,0)); cvNamedWindow( "Right(GSA)", 1 ); cvShowImage( "Right(GSA)", imgR ); } }
< function LinearTriangulation > Input : - PL : 3x4 camera matrix for the left image - CornersL : left image coordinates - PR : 3x4 camera matrix for the right image - CornersR : corresponding right image coordinates - X : corresponding object coordinates - Npts : number of points Output : estimated 3D object coordinates void LinearTriangulation(CvMat *PL,CvMat *CornersL,CvMat *PR,CvMat *CornersR,CvMat *X, int Npts){ CvMat *A = cvCreateMat(4,4,CV_64FC1); CvMat *U = cvCreateMat(4,4,CV_64FC1); CvMat *D = cvCreateMat(4,4,CV_64FC1); CvMat *V = cvCreateMat(4,4,CV_64FC1); int i; for(i=0;i<Npts;i++) { cvmSet(A, 0, 0, ( cvmGet(CornersL,0,i)*cvmGet(PL,2,0) - cvmGet(PL,0,0) ) ); cvmSet(A, 0, 1, ( cvmGet(CornersL,0,i)*cvmGet(PL,2,1) - cvmGet(PL,0,1) ) ); cvmSet(A, 0, 2, ( cvmGet(CornersL,0,i)*cvmGet(PL,2,2) - cvmGet(PL,0,2) ) ); cvmSet(A, 0, 3, ( cvmGet(CornersL,0,i)*cvmGet(PL,2,3) - cvmGet(PL,0,3) ) ); cvmSet(A, 1, 0, ( cvmGet(CornersL,1,i)*cvmGet(PL,2,0) - cvmGet(PL,1,0) ) ); cvmSet(A, 1, 1, ( cvmGet(CornersL,1,i)*cvmGet(PL,2,1) - cvmGet(PL,1,1) ) ); cvmSet(A, 1, 2, ( cvmGet(CornersL,1,i)*cvmGet(PL,2,2) - cvmGet(PL,1,2) ) ); cvmSet(A, 1, 3, ( cvmGet(CornersL,1,i)*cvmGet(PL,2,3) - cvmGet(PL,1,3) ) ); cvmSet(A, 2, 0, ( cvmGet(CornersR,0,i)*cvmGet(PR,2,0) - cvmGet(PR,0,0) ) ); cvmSet(A, 2, 1, ( cvmGet(CornersR,0,i)*cvmGet(PR,2,1) - cvmGet(PR,0,1) ) ); cvmSet(A, 2, 2, ( cvmGet(CornersR,0,i)*cvmGet(PR,2,2) - cvmGet(PR,0,2) ) ); cvmSet(A, 2, 3, ( cvmGet(CornersR,0,i)*cvmGet(PR,2,3) - cvmGet(PR,0,3) ) ); cvmSet(A, 3, 0, ( cvmGet(CornersR,1,i)*cvmGet(PR,2,0) - cvmGet(PR,1,0) ) ); cvmSet(A, 3, 1, ( cvmGet(CornersR,1,i)*cvmGet(PR,2,1) - cvmGet(PR,1,1) ) ); cvmSet(A, 3, 2, ( cvmGet(CornersR,1,i)*cvmGet(PR,2,2) - cvmGet(PR,1,2) ) ); cvmSet(A, 3, 3, ( cvmGet(CornersR,1,i)*cvmGet(PR,2,3) - cvmGet(PR,1,3) ) ); cvSVD(A, D, U, V, CV_SVD_V_T); cvmSet(X,0,i,cvmGet(V,3,0)/cvmGet(V,3,3)); cvmSet(X,1,i,cvmGet(V,3,1)/cvmGet(V,3,3)); cvmSet(X,2,i,cvmGet(V,3,2)/cvmGet(V,3,3)); cvmSet(X,3,i,cvmGet(V,3,3)/cvmGet(V,3,3)); } } < function setCamerasbyF > Input : - PL : 3x4 camera matrix for the left image - PR : 3x4 camera matrix for the right image - F : fundamental matrix - eR : epipole in the right image Output : estimated PL,PR void setCamerasbyF(CvMat *F,CvMat *eR,CvMat *PL,CvMat *PR){ CvMat* ex = cvCreateMat(3,3,CV_64FC1); CvMat* exF = cvCreateMat(3,3,CV_64FC1); cvmSet(eR,0,0,cvmGet(eR,0,0)/cvmGet(eR,2,0)); cvmSet(eR,1,0,cvmGet(eR,1,0)/cvmGet(eR,2,0)); cvmSet(eR,2,0,cvmGet(eR,2,0)/cvmGet(eR,2,0)); // setup PL cvmSet(PL,0,0,1); cvmSet(PL,0,1,0); cvmSet(PL,0,2,0); cvmSet(PL,0,3,0); cvmSet(PL,1,0,0); cvmSet(PL,1,1,1); cvmSet(PL,1,2,0); cvmSet(PL,1,3,0);
cvmSet(PL,2,0,0); cvmSet(PL,2,1,0); cvmSet(PL,2,2,1); cvmSet(PL,2,3,0); // [e']x cvmSet(ex,0,0, 0 ); cvmSet(ex,0,1,-cvmGet(eR,2,0)); cvmSet(ex,0,2, cvmGet(eR,1,0)); cvmSet(ex,1,0, cvmGet(eR,2,0)); cvmSet(ex,1,1, 0 ); cvmSet(ex,1,2,-cvmGet(eR,0,0)); cvmSet(ex,2,0,-cvmGet(eR,1,0)); cvmSet(ex,2,1, cvmGet(eR,0,0)); cvmSet(ex,2,2, 0 ); cvMatMul(ex,F,exF); cvmSet(PR,0,0,cvmGet(exF,0,0)); cvmSet(PR,0,1,cvmGet(exF,0,1)); cvmSet(PR,0,2,cvmGet(exF,0,2)); cvmSet(PR,0,3,cvmGet(eR ,0,0)); cvmSet(PR,1,0,cvmGet(exF,1,0)); cvmSet(PR,1,1,cvmGet(exF,1,1)); cvmSet(PR,1,2,cvmGet(exF,1,2)); cvmSet(PR,1,3,cvmGet(eR ,1,0)); cvmSet(PR,2,0,cvmGet(exF,2,0)); cvmSet(PR,2,1,cvmGet(exF,2,1)); cvmSet(PR,2,2,cvmGet(exF,2,2)); cvmSet(PR,2,3,cvmGet(eR ,2,0)); } < function setJFundamental > Setup Jacobian matrix to estimate F using the LM algorithm Input : - Param : 12+3xNpts parameter vector - X : 3D points coordinates - CornersL : corresponding left image coordinate - CornersR : corresponding right image coordinate - J : Jacobian matrix - Npts : number of points Output : estimated J void setJFundamental(CvMat *Param,CvMat *X,CvMat *CornersL, CvMat *CornersR, CvMat *J, int Npts){ int i,j; double uml,vml,umr,vmr; double p11=cvmGet(Param, 0,0); double p12=cvmGet(Param, 1,0); double p13=cvmGet(Param, 2,0); double p14=cvmGet(Param, 3,0); double p21=cvmGet(Param, 4,0); double p22=cvmGet(Param, 5,0); double p23=cvmGet(Param, 6,0); double p24=cvmGet(Param, 7,0); double p31=cvmGet(Param, 8,0); double p32=cvmGet(Param, 9,0); double p33=cvmGet(Param,10,0); double p34=cvmGet(Param,11,0); double Xobj,Yobj,Zobj; double t0; // initialize J for(i=0;i<Npts;i++) { for(j=0;j<12+3*Npts;j++) cvmSet(J,i,j,0); } // setup J for(i=0;i<Npts;i++) {
uml=cvmGet(CornersL,0,i); vml=cvmGet(CornersL,1,i); umr=cvmGet(CornersR,0,i); vmr=cvmGet(CornersR,1,i); t0 = -2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*Xobj/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 0,t0); t0 = -2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*Yobj/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 1,t0); t0 = -2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*Zobj/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 2,t0); t0 = -2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 3,t0); t0 = -2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*Xobj/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 4,t0); t0 = -2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*Yobj/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 5,t0); t0 = -2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*Zobj/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 6,t0); t0 = -2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))/(p31*Xobj+p32*Yobj+p33*Zobj+p34); cvmSet(J,i, 7,t0); t0 = 2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p11*Xobj+p12*Yobj+p13*Zobj+p14)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*Xobj+2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p21*Xobj+p22*Yobj+p23*Zobj+p24)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*Xobj; cvmSet(J,i, 8,t0); t0 = 2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p11*Xobj+p12*Yobj+p13*Zobj+p14)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*Yobj+2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p21*Xobj+p22*Yobj+p23*Zobj+p24)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*Yobj; cvmSet(J,i, 9,t0); t0 = 2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p11*Xobj+p12*Yobj+p13*Zobj+p14)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*Zobj+2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p21*Xobj+p22*Yobj+p23*Zobj+p24)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*Zobj; cvmSet(J,i,10,t0); t0 = 2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p11*Xobj+p12*Yobj+p13*Zobj+p14)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)+2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(p21*Xobj+p22*Yobj+p23*Zobj+p24)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0); cvmSet(J,i,11,t0); t0 = -2.0*(uml-Xobj/Zobj)/Zobj+2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(-p11/(p31*Xobj+p32*Yobj+p33*Zobj+p34)+(p11*Xobj+p12*Yobj+p13*Zobj+p14)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*p31)+2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(-p21/(p31*Xobj+p32*Yobj+p33*Zobj+p34)+(p21*Xobj+p22*Yobj+p23*Zobj+p24)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*p31); cvmSet(J,i,12+3*i,t0); t0 = -2.0*(vml-Yobj/Zobj)/Zobj+2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(-p12/(p31*Xobj+p32*Yobj+p33*Zobj+p34)+(p11*Xobj+p12*Yobj+p13*Zobj+p14)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*p32)+2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(-p22/(p31*Xobj+p32*Yobj+p33*Zobj+p34)+(p21*Xobj+p22*Yobj+p23*Zobj+p24)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*p32); cvmSet(J,i,13+3*i,t0); t0 = 2.0*(uml-Xobj/Zobj)*Xobj/(Zobj*Zobj)+2.0*(vml-Yobj/Zobj)*Yobj/(Zobj*Zobj)+2.0*(umr-(p11*Xobj+p12*Yobj+p13*Zobj+p14)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(-p13/(p31*Xobj+p32*Yobj+p33*Zobj+p34)+(p11*Xobj+p12*Yobj+p13*Zobj+p14)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*p33)+2.0*(vmr-(p21*Xobj+p22*Yobj+p23*Zobj+p24)/(p31*Xobj+p32*Yobj+p33*Zobj+p34))*(-p23/(p31*Xobj+p32*Yobj+p33*Zobj+p34)+(p21*Xobj+p22*Yobj+p23*Zobj+p24)/pow(p31*Xobj+p32*Yobj+p33*Zobj+p34,2.0)*p33); cvmSet(J,i,14+3*i,t0);
} } < function DrawEpiLines > Input : - vector : line vector - img : image void DrawEpiLines(CvMat *vector, IplImage *img){ int height = img->height; int width = img->width; int flag1=0; int flag2=0; double x,y; CvPoint pt1,pt2; // four corner points CvMat *UL = cvCreateMat(3,1,CV_64FC1); cvmSet(UL,0,0,0); cvmSet(UL,1,0,0); cvmSet(UL,2,0,1); CvMat *UR = cvCreateMat(3,1,CV_64FC1); cvmSet(UR,0,0,width-1); cvmSet(UR,1,0,0); cvmSet(UR,2,0,1); CvMat *LL = cvCreateMat(3,1,CV_64FC1); cvmSet(LL,0,0,0); cvmSet(LL,1,0,height-1); cvmSet(LL,2,0,1); CvMat *LR = cvCreateMat(3,1,CV_64FC1); cvmSet(LR,0,0,width-1); cvmSet(LR,1,0,height-1); cvmSet(LR,2,0,1); // check boundaries CvMat * TOP = cvCreateMat(3,1,CV_64FC1); cvCrossProduct(UL,UR,TOP); CvMat * BOTTOM = cvCreateMat(3,1,CV_64FC1); cvCrossProduct(LL,LR,BOTTOM); CvMat * LEFT = cvCreateMat(3,1,CV_64FC1); cvCrossProduct(UL,LL,LEFT); CvMat * RIGHT = cvCreateMat(3,1,CV_64FC1); cvCrossProduct(UR,LR,RIGHT); CvMat * temp = cvCreateMat(3,1,CV_64FC1); cvCrossProduct(vector,TOP,temp); x=cvmGet(temp,0,0)/cvmGet(temp,2,0); y=cvmGet(temp,1,0)/cvmGet(temp,2,0); if((int)(x+.5)>=0 && (int)(x+.5)<=width-1 && (int)(y+.5)>=0 && (int)(y+.5)<=height-1) { pt1.x=(int)(x+.5); pt1.y=(int)(y+.5); flag1=1; } cvCrossProduct(vector,BOTTOM,temp); x=cvmGet(temp,0,0)/cvmGet(temp,2,0); y=cvmGet(temp,1,0)/cvmGet(temp,2,0); if((int)(x+.5)>=0 && (int)(x+.5)<=width-1 && (int)(y+.5)>=0 && (int)(y+.5)<=height-1) { if(flag1==1) { pt2.x=(int)(x+.5); pt2.y=(int)(y+.5); flag2=1; } else { pt1.x=(int)(x+.5);
pt1.y=(int)(y+.5); flag1=1; } } cvCrossProduct(vector,LEFT,temp); x=cvmGet(temp,0,0)/cvmGet(temp,2,0); y=cvmGet(temp,1,0)/cvmGet(temp,2,0); if((int)(x+.5)>=0 && (int)(x+.5)<=width-1 && (int)(y+.5)>=0 && (int)(y+.5)<=height-1) { if(flag1==0) { pt1.x=(int)(x+.5); pt1.y=(int)(y+.5); flag1=1; } else if(flag1==1 && flag2==0) { pt2.x=(int)(x+.5); pt2.y=(int)(y+.5); flag2=1; } else { // do nothing } } cvCrossProduct(vector,RIGHT,temp); x=cvmGet(temp,0,0)/cvmGet(temp,2,0); y=cvmGet(temp,1,0)/cvmGet(temp,2,0); if((int)(x+.5)>=0 && (int)(x+.5)<=width-1 && (int)(y+.5)>=0 && (int)(y+.5)<=height-1) { if(flag1==1 && flag2==0) { pt2.x=(int)(x+.5); pt2.y=(int)(y+.5); flag2=1; } else { // do nothing } } cvLine( img, pt1, pt2, CV_RGB(0,255,0), 1, 8, 0 ); } < function RectificationH > Input : - imgL : left image - imgR : right image - F : Fundamental matrix - M : M matrix - eL : left epipole - eR : right epipole - CornersL : left image coordinate - CornersR : corresponding right image coordinate - Npts : number of points - HL : homography rectifying the left image - HR : homography rectifying the right image - filenameL : filename for the left rectified imate - filenameR : filename for the right rectified imate - Viewflag : if 1, show the results, else do nothing
void RectificationH(IplImage* imgL,IplImage* imgR,CvMat *F,CvMat *M,CvMat *eL,CvMat *eR,CvMat *CornersL,CvMat *CornersR,int Npts,CvMat *HL,CvMat *HR,char *filenameL, char *filenameR, int Viewflag){ int i,j; double PI=3.14159265; ///////////////////////////////////////////////////////////////// // step 1. determine translation matrix ///////////////////////////////////////////////////////////////// // xo is the center of image CvMat* xo = cvCreateMat(3,1,CV_64FC1); CvMat* T = cvCreateMat(3,3,CV_64FC1); //cvmSet(xo,0,0, imgL->width/2); //cvmSet(xo,1,0, imgL->height/2); cvmSet(xo,0,0, 0); cvmSet(xo,1,0, 0); cvmSet(xo,2,0, 1); for(i=0;i<3;i++) { for(j=0;j<3;j++) { if(i==j) cvmSet(T,i,j,1); else cvmSet(T,i,j,0); } } cvmSet(T,0,2,-cvmGet(xo,0,0)); cvmSet(T,1,2,-cvmGet(xo,1,0)); ///////////////////////////////////////////////////////////////// // step 2. determine rotation matrix ///////////////////////////////////////////////////////////////// double theta; int index; CvMat* eRnorm1 = cvCreateMat(3,1,CV_64FC1); CvMat* neweR = cvCreateMat(3,1,CV_64FC1); CvMat* diff = cvCreateMat(5,1,CV_64FC1); CvMat* R = cvCreateMat(3,3,CV_64FC1); double normeR=sqrt(pow(cvmGet(eR,0,0),2)+pow(cvmGet(eR,1,0),2)+pow(cvmGet(eR,2,0),2)); cvmSet(eRnorm1,0,0,cvmGet(eR,0,0)/normeR); cvmSet(eRnorm1,1,0,cvmGet(eR,1,0)/normeR); cvmSet(eRnorm1,2,0,cvmGet(eR,2,0)/normeR); if (fabs(cvmGet(eRnorm1,2,0)) < 1e-20) { theta = 0; if( fabs(cvmGet(eRnorm1,0,0)) > fabs(cvmGet(eRnorm1,1,0)) ) { cvmSet(neweR,0,0,1); cvmSet(neweR,1,0,0); cvmSet(neweR,2,0,0); } else { cvmSet(neweR,0,0,0); cvmSet(neweR,1,0,1); cvmSet(neweR,2,0,0); } } else { theta=atan2(cvmGet(eRnorm1,1,0)/cvmGet(eRnorm1,2,0)-cvmGet(xo,1,0),cvmGet(eRnorm1,0,0)/cvmGet(eRnorm1,2,0)-cvmGet(xo,0,0)); cvmSet(diff,0,0, 0-theta); cvmSet(diff,1,0, -PI-theta); cvmSet(diff,2,0, PI-theta); cvmSet(diff,3,0,-PI/2-theta); cvmSet(diff,4,0, PI/2-theta); double min=1e20; for(i=0;i<5;i++) {
if(fabs(cvmGet(diff,i,0))<min){ min=fabs(cvmGet(diff,i,0)); index=i; } } if(index<3) { cvmSet(neweR,0,0,1); cvmSet(neweR,1,0,0); cvmSet(neweR,2,0,0); } else { cvmSet(neweR,0,0,0); cvmSet(neweR,1,0,1); cvmSet(neweR,2,0,0); } } theta=cvmGet(diff,index,0); cvmSet(R,0,0, cos(theta)); cvmSet(R,0,1,-sin(theta)); cvmSet(R,0,2, 0); cvmSet(R,1,0, sin(theta)); cvmSet(R,1,1, cos(theta)); cvmSet(R,1,2, 0); cvmSet(R,2,0, 0); cvmSet(R,2,1, 0); cvmSet(R,2,2, 1); ///////////////////////////////////////////////////////////////// // step 3. send epipole in the right image to the point at infinity ///////////////////////////////////////////////////////////////// CvMat* ReR = cvCreateMat(3,1,CV_64FC1); CvMat* temp = cvCreateMat(3,3,CV_64FC1); CvMat* G = cvCreateMat(3,3,CV_64FC1); for(i=0;i<3;i++) { for(j=0;j<3;j++) { if(i==j) { cvmSet(G,i,j,1); } else { cvmSet(G,i,j,0); } } } if(cvmGet(neweR,0,0)==1) { cvMatMul(R,eR,ReR); cvmSet(G,2,0,-cvmGet(ReR,2,0)/cvmGet(ReR,0,0)); } else { cvMatMul(R,eR,ReR); cvmSet(G,2,1,-cvmGet(ReR,2,0)/cvmGet(ReR,0,0)); } cvMatMul(R,T,temp); cvMatMul(G,temp,HR); ///////////////////////////////////////////////////////////////// // step 4. estimate a Homography for the left image using least squares ///////////////////////////////////////////////////////////////// CvMat* Ho = cvCreateMat(3,3,CV_64FC1); CvMat* xhat = cvCreateMat(3,Npts,CV_64FC1); CvMat* xhatp = cvCreateMat(3,Npts,CV_64FC1); CvMat* A = cvCreateMat(3,3,CV_64FC1); CvMat* b = cvCreateMat(3,1,CV_64FC1);
CvMat* aest = cvCreateMat(3,1,CV_64FC1); CvMat* Ainv = cvCreateMat(3,3,CV_64FC1); CvMat* Ha = cvCreateMat(3,3,CV_64FC1); cvMatMul(HR,M,Ho); cvmSet(Ho,0,0,cvmGet(Ho,0,0)/cvmGet(Ho,2,2)); cvmSet(Ho,0,1,cvmGet(Ho,0,1)/cvmGet(Ho,2,2)); cvmSet(Ho,0,2,cvmGet(Ho,0,2)/cvmGet(Ho,2,2)); cvmSet(Ho,1,0,cvmGet(Ho,1,0)/cvmGet(Ho,2,2)); cvmSet(Ho,1,1,cvmGet(Ho,1,1)/cvmGet(Ho,2,2)); cvmSet(Ho,1,2,cvmGet(Ho,1,2)/cvmGet(Ho,2,2)); cvmSet(Ho,2,0,cvmGet(Ho,2,0)/cvmGet(Ho,2,2)); cvmSet(Ho,2,1,cvmGet(Ho,2,1)/cvmGet(Ho,2,2)); cvmSet(Ho,2,2,cvmGet(Ho,2,2)/cvmGet(Ho,2,2)); // minimize d(Ha*xhat,xhatp) cvMatMul(Ho,CornersL,xhat); cvMatMul(HR,CornersR,xhatp); cvmSet(A,0,0,0); cvmSet(A,0,1,0); cvmSet(A,0,2,0); cvmSet(A,1,0,0); cvmSet(A,1,1,0); cvmSet(A,1,2,0); cvmSet(A,2,0,0); cvmSet(A,2,1,0); cvmSet(A,2,2,0); cvmSet(b,0,0,0); cvmSet(b,1,0,0); cvmSet(b,2,0,0); double xx,yy,xxp; for(i=0;i<Npts;i++) { xx=cvmGet(xhat,0,i)/cvmGet(xhat,2,i); yy=cvmGet(xhat,1,i)/cvmGet(xhat,2,i); xxp=cvmGet(xhatp,0,i)/cvmGet(xhatp,2,i); cvmSet(A,0,0,cvmGet(A,0,0)+xx*xx ); cvmSet(A,0,1,cvmGet(A,0,1)+xx*yy ); cvmSet(A,0,2,cvmGet(A,0,2)+xx ); cvmSet(A,1,0,cvmGet(A,1,0)+xx*yy ); cvmSet(A,1,1,cvmGet(A,1,1)+yy*yy ); cvmSet(A,1,2,cvmGet(A,1,2)+yy ); cvmSet(A,2,0,cvmGet(A,2,0)+xx ); cvmSet(A,2,1,cvmGet(A,2,1)+yy ); cvmSet(A,2,2,cvmGet(A,2,2)+1 ); cvmSet(b,0,0,cvmGet(b,0,0)+xx*xxp ); cvmSet(b,1,0,cvmGet(b,1,0)+yy*xxp ); cvmSet(b,2,0,cvmGet(b,2,0)+xxp ); } cvInvert(A,Ainv); cvMatMul(Ainv,b,aest); if(cvmGet(neweR,0,0)==1) { cvmSet(Ha,0,0,cvmGet(aest,0,0)); cvmSet(Ha,0,1,cvmGet(aest,1,0)); cvmSet(Ha,0,2,cvmGet(aest,2,0)); cvmSet(Ha,1,0,0); cvmSet(Ha,1,1,1); cvmSet(Ha,1,2,0); cvmSet(Ha,2,0,0); cvmSet(Ha,2,1,0); cvmSet(Ha,2,2,1); } else { cvmSet(Ha,0,0,1); cvmSet(Ha,0,1,0); cvmSet(Ha,0,2,0); cvmSet(Ha,1,0,cvmGet(aest,0,0)); cvmSet(Ha,1,1,cvmGet(aest,1,0));
cvmSet(Ha,1,2,cvmGet(aest,2,0)); cvmSet(Ha,2,0,0); cvmSet(Ha,2,1,0); cvmSet(Ha,2,2,1); } cvMatMul(Ha,Ho,HL); cvmSet(HL,0,0,cvmGet(HL,0,0)/cvmGet(HL,2,2)); cvmSet(HL,0,1,cvmGet(HL,0,1)/cvmGet(HL,2,2)); cvmSet(HL,0,2,cvmGet(HL,0,2)/cvmGet(HL,2,2)); cvmSet(HL,1,0,cvmGet(HL,1,0)/cvmGet(HL,2,2)); cvmSet(HL,1,1,cvmGet(HL,1,1)/cvmGet(HL,2,2)); cvmSet(HL,1,2,cvmGet(HL,1,2)/cvmGet(HL,2,2)); cvmSet(HL,2,0,cvmGet(HL,2,0)/cvmGet(HL,2,2)); cvmSet(HL,2,1,cvmGet(HL,2,1)/cvmGet(HL,2,2)); cvmSet(HL,2,2,cvmGet(HL,2,2)/cvmGet(HL,2,2)); ///////////////////////////////////////////////////////////////// // step 5. Resampling images ///////////////////////////////////////////////////////////////// CvMat* Corners = cvCreateMat(3,4,CV_64FC1); CvMat* Lboundary= cvCreateMat(3,4,CV_64FC1); CvMat* Rboundary = cvCreateMat(3,4,CV_64FC1); CvMat* pt = cvCreateMat(3,1,CV_64FC1); CvMat* pt2 = cvCreateMat(3,1,CV_64FC1); CvMat* HLinv = cvCreateMat(3,3,CV_64FC1); CvMat* HRinv = cvCreateMat(3,3,CV_64FC1); // boundary check and prepare common array for the stereo image pair cvmSet(Corners,0,0,0); cvmSet(Corners,1,0,0); cvmSet(Corners,2,0,1); cvmSet(Corners,0,1,imgL->width-1 ); cvmSet(Corners,1,1,0); cvmSet(Corners,2,1,1); cvmSet(Corners,0,2,0); cvmSet(Corners,1,2,imgL->height-1); cvmSet(Corners,2,2,1); cvmSet(Corners,0,3,imgL->width-1 ); cvmSet(Corners,1,3,imgL->height-1); cvmSet(Corners,2,3,1); cvMatMul(HL,Corners,Lboundary); cvMatMul(HR,Corners,Rboundary); // find common boundaries double minx=1e10; double maxx=-1e10; double miny=1e10; double maxy=-1e10; for(i=0;i<4;i++) { if(cvmGet(Lboundary,0,i)/cvmGet(Lboundary,2,i)<minx) minx=cvmGet(Lboundary,0,i)/cvmGet(Lboundary,2,i); if(cvmGet(Lboundary,0,i)/cvmGet(Lboundary,2,i)>maxx) maxx=cvmGet(Lboundary,0,i)/cvmGet(Lboundary,2,i); if(cvmGet(Lboundary,1,i)/cvmGet(Lboundary,2,i)<miny) miny=cvmGet(Lboundary,1,i)/cvmGet(Lboundary,2,i); if(cvmGet(Lboundary,1,i)/cvmGet(Lboundary,2,i)>maxy) maxy=cvmGet(Lboundary,1,i)/cvmGet(Lboundary,2,i); if(cvmGet(Rboundary,0,i)/cvmGet(Rboundary,2,i)<minx) minx=cvmGet(Rboundary,0,i)/cvmGet(Rboundary,2,i); if(cvmGet(Rboundary,0,i)/cvmGet(Rboundary,2,i)>maxx) maxx=cvmGet(Rboundary,0,i)/cvmGet(Rboundary,2,i); if(cvmGet(Rboundary,1,i)/cvmGet(Rboundary,2,i)<miny) miny=cvmGet(Rboundary,1,i)/cvmGet(Rboundary,2,i); if(cvmGet(Rboundary,1,i)/cvmGet(Rboundary,2,i)>maxy) maxy=cvmGet(Rboundary,1,i)/cvmGet(Rboundary,2,i); } // assign equivalent pixel size double pix=((maxx-minx)/imgL->width+(maxy-miny)/imgL->height)/2; printf("pix=%f\n",pix); // ISS building //pix=1.2; // Corridors pix=2.5; int width =(int)(maxx-minx)/pix+.5; int height=(int)(maxy-miny)/pix+.5; double x,y; IplImage* dstL = cvCreateImage( cvSize(width,height), 8, 1 ); IplImage* dstR = cvCreateImage( cvSize(width,height), 8, 1 );
// define similarity transformatiom H2 from Hx to Rectified image // no rotations! only translation and scale CvMat* H2 = cvCreateMat(3,3,CV_64FC1); CvMat* Htemp = cvCreateMat(3,3,CV_64FC1); cvmSet(H2,0,0,1/pix); cvmSet(H2,0,1,0); cvmSet(H2,0,2,-((int)(minx+.5))/pix); cvmSet(H2,1,0,0); cvmSet(H2,1,1,1/pix); cvmSet(H2,1,2,-((int)(miny+.5))/pix); cvmSet(H2,2,0,0); cvmSet(H2,2,1,0); cvmSet(H2,2,2,1); cvCopy(HL,Htemp,0); cvMatMul(H2,Htemp,HL); cvmSet(HL,0,0,cvmGet(HL,0,0)/cvmGet(HL,2,2)); cvmSet(HL,0,1,cvmGet(HL,0,1)/cvmGet(HL,2,2)); cvmSet(HL,0,2,cvmGet(HL,0,2)/cvmGet(HL,2,2)); cvmSet(HL,1,0,cvmGet(HL,1,0)/cvmGet(HL,2,2)); cvmSet(HL,1,1,cvmGet(HL,1,1)/cvmGet(HL,2,2)); cvmSet(HL,1,2,cvmGet(HL,1,2)/cvmGet(HL,2,2)); cvmSet(HL,2,0,cvmGet(HL,2,0)/cvmGet(HL,2,2)); cvmSet(HL,2,1,cvmGet(HL,2,1)/cvmGet(HL,2,2)); cvmSet(HL,2,2,cvmGet(HL,2,2)/cvmGet(HL,2,2)); cvCopy(HR,Htemp,0); cvMatMul(H2,Htemp,HR); // resampling images cvInvert(HL,HLinv); cvInvert(HR,HRinv); for(i=0;i<height;i++) { for(j=0;j<width;j++) { cvmSet(pt,0,0,(double)j); cvmSet(pt,1,0,(double)i); cvmSet(pt,2,0,(double)1); cvMatMul(HLinv,pt,pt2); x=cvmGet(pt2,0,0)/cvmGet(pt2,2,0); y=cvmGet(pt2,1,0)/cvmGet(pt2,2,0); if(x>=0 && x<imgL->width && y>=0 && y<imgL->height) { dstL->imageData[i*dstL->width+j]=Bilinear(imgL,x,y); } else { dstL->imageData[i*dstL->width+j]=255; } cvMatMul(HRinv,pt,pt2); x=cvmGet(pt2,0,0)/cvmGet(pt2,2,0); y=cvmGet(pt2,1,0)/cvmGet(pt2,2,0); if(x>=0 && x<imgR->width && y>=0 && y<imgR->height) { dstR->imageData[i*dstR->width+j]=Bilinear(imgR,x,y); } else { dstR->imageData[i*dstR->width+j]=255; } } } cvSaveImage(filenameL, dstL); cvSaveImage(filenameR, dstR); cvReleaseImage(&dstL); cvReleaseImage(&dstR); // display epipolar lines // Right images IplImage* RectR = cvLoadImage(filenameR,1); CvMat* line = cvCreateMat(3,1,CV_64FC1); CvMat* line2 = cvCreateMat(3,1,CV_64FC1); CvMat* HRinvt = cvCreateMat(3,3,CV_64FC1);
CvMat* HManualptsR = cvCreateMat(3,Npts,CV_64FC1); cvTranspose(HRinv,HRinvt); CvPoint Draw; cvMatMul(HR,CornersR,HManualptsR); for(i=0;i<Npts;i++) { cvmSet(pt,0,0,cvmGet(CornersL,0,i)); cvmSet(pt,1,0,cvmGet(CornersL,1,i)); cvmSet(pt,2,0,cvmGet(CornersL,2,i)); cvMatMul(F,pt,line); cvmSet(line,0,0,cvmGet(line,0,0)/cvmGet(line,2,0)); cvmSet(line,1,0,cvmGet(line,1,0)/cvmGet(line,2,0)); cvmSet(line,2,0,cvmGet(line,2,0)/cvmGet(line,2,0)); cvMatMul(HRinvt,line,line2); cvmSet(line2,0,0,cvmGet(line2,0,0)/cvmGet(line2,2,0)); cvmSet(line2,1,0,cvmGet(line2,1,0)/cvmGet(line2,2,0)); cvmSet(line2,2,0,cvmGet(line2,2,0)/cvmGet(line2,2,0)); DrawEpiLines(line2,RectR); Draw.x=(int)cvmGet(HManualptsR,0,i)/cvmGet(HManualptsR,2,i); Draw.y=(int)cvmGet(HManualptsR,1,i)/cvmGet(HManualptsR,2,i); draw_cross(RectR,Draw,CV_RGB(0,255,0),3); } // Left images IplImage* RectL = cvLoadImage(filenameL,1); CvMat* HLinvt = cvCreateMat(3,3,CV_64FC1); CvMat* HManualptsL = cvCreateMat(3,Npts,CV_64FC1); CvMat* Ft = cvCreateMat(3,3,CV_64FC1); cvTranspose(F,Ft); cvTranspose(HLinv,HLinvt); cvMatMul(HL,CornersL,HManualptsL); for(i=0;i<Npts;i++) { cvmSet(pt,0,0,cvmGet(CornersR,0,i)); cvmSet(pt,1,0,cvmGet(CornersR,1,i)); cvmSet(pt,2,0,cvmGet(CornersR,2,i)); cvMatMul(Ft,pt,line); cvMatMul(HLinvt,line,line2); DrawEpiLines(line2,RectL); Draw.x=(int)cvmGet(HManualptsL,0,i)/cvmGet(HManualptsL,2,i); Draw.y=(int)cvmGet(HManualptsL,1,i)/cvmGet(HManualptsL,2,i); draw_cross(RectL,Draw,CV_RGB(0,255,0),3); } if(Viewflag==1) { cvNamedWindow( filenameL, 1 ); cvShowImage( filenameL, RectL ); cvNamedWindow( filenameR, 1 ); cvShowImage( filenameR, RectR ); cvSaveImage("RectL_epilines.jpg",RectL); cvSaveImage("RectR_epilines.jpg",RectR); } cvReleaseImage(&RectL); cvReleaseImage(&RectR); cvReleaseMat(&xo); cvReleaseMat(&T); cvReleaseMat(&eRnorm1); cvReleaseMat(&neweR); cvReleaseMat(&diff); cvReleaseMat(&R); cvReleaseMat(&ReR); cvReleaseMat(&temp); cvReleaseMat(&G); cvReleaseMat(&Ho); cvReleaseMat(&xhat); cvReleaseMat(&xhatp); cvReleaseMat(&A); cvReleaseMat(&b); cvReleaseMat(&aest); cvReleaseMat(&Ainv); cvReleaseMat(&Ha);
cvReleaseMat(&Corners); cvReleaseMat(&Lboundary); cvReleaseMat(&Rboundary); cvReleaseMat(&pt); cvReleaseMat(&pt2); cvReleaseMat(&HLinv); cvReleaseMat(&HRinv); cvReleaseMat(&line); cvReleaseMat(&line2); cvReleaseMat(&HRinvt); cvReleaseMat(&HManualptsR); cvReleaseMat(&HLinvt); cvReleaseMat(&HManualptsL); cvReleaseMat(&Ft); } < function Bilinear > Bilinear interpolation of DN value Input : - img : image - x,y : image coordinates Output : interpolated DN unsigned char Bilinear(IplImage* img,double x,double y){ unsigned char DN; if(fabs(x)<1 || fabs(x-img->width+1)<1 || fabs(y)<1 || fabs(y-img->height+1)<1) { int xx,yy; xx=(int)x+.5; yy=(int)y+.5; DN=(unsigned char)img->imageData[yy*img->width+xx]; } else { unsigned char DN1,DN2,DN3,DN4; double DN12,DN34; int x1,x2,y1,y2; x1=(int)x; y1=(int)y; x2=x1+1; y2=y1+1; DN1=(unsigned char)img->imageData[y1*img->width+x1]; DN2=(unsigned char)img->imageData[y1*img->width+x2]; DN3=(unsigned char)img->imageData[y2*img->width+x1]; DN4=(unsigned char)img->imageData[y2*img->width+x2]; DN12=(x-(double)x1)*(double)DN2+((double)x2-x)*(double)DN1; DN34=(x-(double)x1)*(double)DN3+((double)x2-x)*(double)DN4; DN=(unsigned char)((y-y1)*DN34+(y2-y)*DN12+.5); } return DN; } < function NormalizationMatrixImg > Input : - Corners : image coordinates - N : number of points - MT : matrix for the normalization Output : normalization matrix MT double NormalizationMatrixImg(CvMat *Corners1,int N,CvMat *MT){ int i; double scale,Cx,Cy,AvgDist; Cx=0;Cy=0;AvgDist=0; for(i=0;i<N;i++) {
Cx=Cx+cvmGet(Corners1,0,i); Cy=Cy+cvmGet(Corners1,1,i); } Cx=Cx/N; Cy=Cy/N; for(i=0;i<N;i++) AvgDist=AvgDist+sqrt(pow(cvmGet(Corners1,0,i)-Cx,2)+pow(cvmGet(Corners1,1,i)-Cy,2)); AvgDist=AvgDist/N; scale=sqrt(2)/AvgDist; cvmSet(MT,0,0,scale); cvmSet(MT,0,1,0); cvmSet(MT,0,2,-scale*Cx); cvmSet(MT,1,0,0); cvmSet(MT,1,1,scale); cvmSet(MT,1,2,-scale*Cy); cvmSet(MT,2,0,0); cvmSet(MT,2,1,0); cvmSet(MT,2,2,1); return scale; } < function ErrorsGDforF > Input : - PL : 3x4 camera matrix for the left image - PR : 3x4 camera matrix for the right image - ObjectPoints : object coordinates - ImgPointsL : corresponding left image coordinates - ImgPointsR : corresponding right image coordinates - N : number of points - d : error vector in pixel Output : error magnitude double ErrorsGDforF(CvMat* PL,CvMat* PR,CvMat* ObjectPoints,CvMat* ImgPointsL,CvMat* ImgPointsR,int N, CvMat* d){ CvMat* x_left = cvCreateMat(3, N,CV_64FC1); CvMat* x_right = cvCreateMat(3, N,CV_64FC1); CvMat* d_left = cvCreateMat(N*2,1,CV_64FC1); CvMat* d_right = cvCreateMat(N*2,1,CV_64FC1); cvMatMul(PL,ObjectPoints,x_left); cvMatMul(PR,ObjectPoints,x_right); int i; for(i=0;i<N;i++) { cvmSet(d_left ,2*i ,0,cvmGet(ImgPointsL,0,i)/cvmGet(ImgPointsL,2,i)-cvmGet(x_left ,0,i)/cvmGet(x_left ,2,i)); cvmSet(d_left ,2*i+1,0,cvmGet(ImgPointsL,0,i)/cvmGet(ImgPointsL,2,i)-cvmGet(x_left ,0,i)/cvmGet(x_left ,2,i)); cvmSet(d_right,2*i ,0,cvmGet(ImgPointsR,0,i)/cvmGet(ImgPointsR,2,i)-cvmGet(x_right,0,i)/cvmGet(x_right,2,i)); cvmSet(d_right,2*i+1,0,cvmGet(ImgPointsR,0,i)/cvmGet(ImgPointsR,2,i)-cvmGet(x_right,0,i)/cvmGet(x_right,2,i)); } for(i=0;i<N;i++) { cvmSet(d,i ,0, pow(cvmGet(d_left,2*i,0),2)+pow(cvmGet(d_left,2*i+1,0),2)+pow(cvmGet(d_right,2*i,0),2)+pow(cvmGet(d_right,2*i+1,0),2)); } double RMSE=0; for(i=0;i<N;i++) RMSE=RMSE+cvmGet(d,i,0); return RMSE; }