1 | % By Philip Torr 2002
|
---|
2 | % copyright Microsoft Corp.
|
---|
3 | %this is a script to test the self calibration stuff
|
---|
4 | %torr_test_calib_sc.m
|
---|
5 | %main()
|
---|
6 | %profile on
|
---|
7 | clear all;
|
---|
8 | m3 = 256;
|
---|
9 | method = 'mapsac';
|
---|
10 | method = 'linear';
|
---|
11 |
|
---|
12 |
|
---|
13 | % % do we want the same result each time
|
---|
14 | % randn('state',0)
|
---|
15 | % rand('state',0)
|
---|
16 | no_test = 1;
|
---|
17 | for(i = 1:no_test)
|
---|
18 |
|
---|
19 | % [true_F,x1,y1,x2,y2,nx1,ny1,nx2,ny2] = ...
|
---|
20 | % torr_gen_2view_matches(foc, no_matches, noise_sigma, translation_mult, translation_adder, ...
|
---|
21 | % rotation_multplier, min_Z,Z_RAN,m3);
|
---|
22 | [true_F,x1,y1,x2,y2,nx1,ny1,nx2,ny2,true_C,true_R,true_TX, true_E] = torr_gen_2view_matches;
|
---|
23 |
|
---|
24 | no_matches = length(nx1);
|
---|
25 |
|
---|
26 | %if we set this to one then the result should be the same as the groundtruth...
|
---|
27 | no_noise = 1;
|
---|
28 | if (no_noise)
|
---|
29 | disp('just using noise free points');
|
---|
30 | nx1 = x1;
|
---|
31 | nx2 = x2;
|
---|
32 | ny1 = y1;
|
---|
33 | ny2 = y2;
|
---|
34 | end
|
---|
35 |
|
---|
36 |
|
---|
37 |
|
---|
38 | matches = [nx1,ny1,nx2,ny2];
|
---|
39 | perfect_matches = [x1,y1,x2,y2];
|
---|
40 | set_rank2 = 1;
|
---|
41 |
|
---|
42 |
|
---|
43 | %first estimate F
|
---|
44 | [f, e1, n_inliers,inlier_index,nF] = torr_estimateF( matches, m3, [], method, set_rank2);
|
---|
45 |
|
---|
46 | %now guess the camera calibration matrix
|
---|
47 | CC = diag(ones(3,1),0);
|
---|
48 | CC(3,3) = 1;
|
---|
49 |
|
---|
50 | %next self calibrate for focal length
|
---|
51 | [focal_length, nE,CCC] = torr_self_calib_f(nF,CC);
|
---|
52 |
|
---|
53 | %now we have an Essential matrix we can establish the camera frame...
|
---|
54 | [P1,P2,R,t,srot_axis,rot_angle,g] = torr_linear_EtoPX(nE,matches,CCC,m3);
|
---|
55 |
|
---|
56 | %next convert the 6 parameters of g to a fundamental matrix
|
---|
57 | f2 = torr_g2F(g,CCC);
|
---|
58 | disp('error before non-linear minimization')
|
---|
59 | e = torr_errf2(f2, nx1,ny1,nx2,ny2, length(nx1), m3);
|
---|
60 | norm(e)
|
---|
61 |
|
---|
62 | [g,f] = torr_nonlinG(g ,nx1,ny1,nx2,ny2, no_matches, m3, CCC)
|
---|
63 |
|
---|
64 |
|
---|
65 | disp('error after')
|
---|
66 | e2 = torr_errf2(f, nx1,ny1,nx2,ny2, length(nx1), m3);
|
---|
67 | norm(e2)
|
---|
68 |
|
---|
69 |
|
---|
70 | %the question now arises: how good is the fit? compare to groundtruth
|
---|
71 | true_rot_axis = [true_R(3,2)-true_R(2,3), true_R(1,3) - true_R(3,1), true_R(2,1) - true_R(1,2)]';
|
---|
72 | true_rot_axis = true_rot_axis /norm(true_rot_axis);
|
---|
73 | true_rot_angle = acos( (trace(true_R)-1)/2);
|
---|
74 |
|
---|
75 | true_t(1) = -true_TX(2,3);
|
---|
76 | true_t(2) = true_TX(1,3);
|
---|
77 | true_t(3) = -true_TX(1,2);
|
---|
78 | true_t = true_t/norm(true_t);
|
---|
79 |
|
---|
80 |
|
---|
81 | disp('true camera parameters')
|
---|
82 | true_t
|
---|
83 | true_rot_axis
|
---|
84 | true_rot_angle
|
---|
85 | true_C
|
---|
86 |
|
---|
87 |
|
---|
88 | rot_axis = torr_sphere2unit([g(2) g(3)]);
|
---|
89 | tt = torr_sphere2unit([g(5) g(6)]);
|
---|
90 | rot_angle = g(4);
|
---|
91 |
|
---|
92 | CCC(3,3) = 1/g(1);
|
---|
93 |
|
---|
94 | disp('estimated camera parameters')
|
---|
95 | tt
|
---|
96 | rot_axis
|
---|
97 | rot_angle
|
---|
98 | CCC
|
---|
99 | end
|
---|