1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
| C++
//8+ algorithm for fondamental matrix estimation
//points[i]=(x_i, x^'_i) the matched points by SIFT
Matrix<double> Fond(3,3);
Matrix<double> A(8,9);
Matrix<double> T1(3,3),T2(3,3);
//First step : Normalisation
double S1=0,S2=0, Tx1=0, Tx2=0, Ty1=0, Ty2=0;
//calculate the translation terms as the centroide of the 8-set points become the origin
for(int i=0;i<8;i++)
{
Tx1+=points(i)->at(0);
Ty1+=points(i)->at(1);
Tx2+=points(i)->at(2);
Ty2+=points(i)->at(3);
}
Tx1/=8;
Ty1/=8;
Tx2/=8;
Ty2/=8;
//the scaling parameters computing
for(int i=0;i<8;i++)
{
S1+=sqrt((points(i)->at(0)-Tx1)*(points(i)->at(0)-Tx1)+(points(i)->at(1)-Ty1)*(points(i)->at(1)-Ty1));
S2+=sqrt((points(i)->at(2)-Tx2)*(points(i)->at(2)-Tx2)+(points(i)->at(3)-Ty2)*(points(i)->at(3)-Ty2));
}
S1/=8;
S2/=8;
S1=sqrt(double(2))/S1;
S2=sqrt(double(2))/S2;
//preparing the transofrmation matrices T1 et T2'
T1[0*3+0]=S1; T1[0*3+1]=0; T1[0*3+2]=-S1*Tx1;
T1[1*3+0]=0; T1[1*3+1]=S1; T1[1*3+2]=-S1*Ty1;
T1[2*3+0]=0; T1[2*3+1]=0; T1[2*3+2]=1;
T2[0*3+0]=S2; T2[0*3+1]=0; T2[0*3+2]=0;
T2[1*3+0]=0; T2[1*3+1]=S2; T2[1*3+2]=0;
T2[2*3+0]=-S2*Tx2; T2[2*3+1]=-S2*Ty2; T2[2*3+2]=1;
//Linear system construction
for(int i=0;i<8;i++)
{//fill the A'i° ligne with the relation P2t*F*P1=0;
A[i*9] =(S2*(points(i)->at(2)-Tx2))*(S1*(points(i)->at(0)-Tx1));
A[i*9+1]=(S2*(points(i)->at(2)-Tx2))*(S1*(points(i)->at(1)-Ty1));
A[i*9+2]=S2*(points(i)->at(2)-Tx2);
A[i*9+3]=(S2*(points(i)->at(3)-Ty2))*(S1*(points(i)->at(0)-Tx1));
A[i*9+4]=(S2*(points(i)->at(3)-Ty2))*(S1*(points(i)->at(1)-Ty1));
A[i*9+5]=S2*(points(i)->at(3)-Ty2);
A[i*9+6]=S1*(points(i)->at(0)-Tx1);
A[i*9+7]=S1*(points(i)->at(1)-Ty1);
A[i*9+8]=1;
}
//F estimation matrix with SVD decomposition
Vector<double> s;
Matrix<double> U,Vt;
A.svda(U,s,Vt); // A=U.S.Vt with diag(S)=s (all SVD)
//F est la dernière ligne de Vt
Fond[0*3+0]=Vt[8*9+0]; Fond[0*3+1]=Vt[8*9+1]; Fond[0*3+2]=Vt[8*9+2];
Fond[1*3+0]=Vt[8*9+3]; Fond[1*3+1]=Vt[8*9+4]; Fond[1*3+2]=Vt[8*9+5];
Fond[1*3+0]=Vt[8*9+6]; Fond[1*3+1]=Vt[8*9+7]; Fond[1*3+2]=Vt[8*9+8];
//enforce the matrix Fond to have rank 2
Fond.svda(U,s,Vt);
Matrix<double> S(3,3);
S[0]=s[0]; S[1]=0; S[2]=0;
S[3]=0; S[4]=s[1]; S[5]=0;
S[6]=0; S[7]=0; S[8]=0;
Fond=U*S*Vt;
//denormalize the fondamentale matrix
Fond=T2*Fond*T1; |
Partager