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
77
78
79
80
81
82
| //8+ algorithm for fondamental matrix estimation
//points[i]=(x_i, x^'_i) the matched points by SIFT
Mat Fond(3,3,CV_32FC2);
Mat A(8,9,CV_32FC2);
Mat T1(3,3,CV_32FC2),T2(3,3,CV_32FC2);
//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=Tx1+keypoints[i].pt.x;
Ty1=Ty1+keypoints[i].pt.y;
Tx2=Tx2+keypoints2[i].pt.x;
Ty2=Ty2+keypoints2[i].pt.y;
}
Tx1/=8;
Ty1/=8;
Tx2/=8;
Ty2/=8;
//the scaling parameters computing
for(int i=0;i<8;i++)
{
S1+=sqrt((keypoints[i].pt.x-Tx1)*(keypoints[i].pt.x-Tx1)+(keypoints[i].pt.y-Ty1)*(keypoints[i].pt.y-Ty1));
S2+=sqrt((keypoints2[i].pt.x-Tx2)*(keypoints2[i].pt.x-Tx2)+(keypoints2[i].pt.y-Ty2)*(keypoints2[i].pt.y-Ty2));
}
S1/=8;
S2/=8;
S1=sqrt(double(2))/S1;
S2=sqrt(double(2))/S2;
//preparing the transofrmation matrices T1 et T2'
T1.at<double>(0*3,0)=S1; T1.at<double>(0*3,1)=0; T1.at<double>(0*3,2)=-S1*Tx1;
T1.at<double>(1*3,0)=0; T1.at<double>(1*3,1)=S1; T1.at<double>(1*3,2)=-S1*Ty1;
T1.at<double>(2*3,0)=0; T1.at<double>(2*3,1)=0; T1.at<double>(2*3,2)=1;
T2.at<double>(0*3,0)=S2; T2.at<double>(0*3,1)=0; T2.at<double>(0*3,2)=0;
T2.at<double>(1*3,0)=0; T2.at<double>(1*3,1)=S2; T2.at<double>(1*3,2)=0;
T2.at<double>(2*3,0)=-S2*Tx2; T2.at<double>(2*3,1)=-S2*Ty2; T2.at<double>(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.at<double>(i,0)=(S2*(keypoints2[i].pt.x-Tx2))*(S1*(keypoints[i].pt.x-Tx1));
A.at<double>(i,1)=(S2*(keypoints2[i].pt.x-Tx2))*(S1*(keypoints[i].pt.y-Ty1));
A.at<double>(i,2)=S2*(keypoints2[i].pt.x-Tx2);
A.at<double>(i,3)=(S2*(keypoints2[i].pt.y-Ty2))*(S1*(keypoints[i].pt.x-Tx1));
A.at<double>(i,4)=(S2*(keypoints2[i].pt.y-Ty2))*(S1*(keypoints[i].pt.y-Ty1));
A.at<double>(i,5)=S2*(keypoints2[i].pt.y-Ty2);
A.at<double>(i,6)=S1*(keypoints[i].pt.x-Tx1);
A.at<double>(i,7)=S1*(keypoints[i].pt.y-Ty1);
A.at<double>(i,8)=1;
}
//F estimation matrix with SVD decomposition
Mat s,U,Vt;
//A.svda(U,s,Vt); // A=U.S.Vt with diag(S)=s (all SVD)
SVD::compute(A, s, U, Vt,0);
//F est la dernière ligne de Vt
Fond.at<double>(0,0)=Vt.at<double>(8*9,0); Fond.at<double>(0*3,1)=Vt.at<double>(8*9,1); Fond.at<double>(0*3,2)=Vt.at<double>(8*9,2);
Fond.at<double>(1*3,0)=Vt.at<double>(8*9,3); Fond.at<double>(1*3+1)=Vt.at<double>(8*9,4); Fond.at<double>(1*3,2)=Vt.at<double>(8*9,5);
Fond.at<double>(1*3,0)=Vt.at<double>(8*9,6); Fond.at<double>(1*3,1)=Vt.at<double>(8*9,7); Fond.at<double>(1*3,2)=Vt.at<double>(8*9,8);
//enforce the matrix Fond to have rank 2
//Fond.svda(U,s,Vt);
SVD::compute(Fond, s, U, Vt,0);
Mat_<double> S(3,3);
S.at<double>(0)=s.at<double>(0); S.at<double>(1)=0; S.at<double>(2)=0;
S.at<double>(3)=0; S.at<double>(4)=s.at<double>(1); S.at<double>(5)=0;
S.at<double>(6)=0; S.at<double>(7)=0; S.at<double>(8)=0;
Fond=U*S*Vt;
//denormalize the fondamentale matrix
Fond=T2*Fond*T1; |
