1 | % By Philip Torr 2002
|
---|
2 | % copyright Microsoft Corp.
|
---|
3 | %generates a set of point matches + their F matrix!@
|
---|
4 | %torr_gen_2view_matches.m
|
---|
5 | %we need to make sure that the object isnt too affine
|
---|
6 | function [true_F,x1,y1,x2,y2,nx1,ny1,nx2,ny2,true_C,true_R,true_TX, true_E, true_X, true_t] = ...
|
---|
7 | torr_gen_2view_matches(foc, no_matches, noise_sigma, translation_mult, translation_adder, ...
|
---|
8 | rotation_multplier, min_Z,Z_RAN,m3, other_parameters)
|
---|
9 |
|
---|
10 | if nargin == 0
|
---|
11 | %default values
|
---|
12 | m3 = 256.0; %third homogeneous coordinate
|
---|
13 |
|
---|
14 | %focal length
|
---|
15 | foc = m3 *3;
|
---|
16 | %number of matches desired
|
---|
17 | no_matches = 100;
|
---|
18 | % noise added to each point
|
---|
19 | noise_sigma = 1;
|
---|
20 |
|
---|
21 | %translation between translation_adder and translation_mult + translation_adder, uniformly distributed.
|
---|
22 | translation_mult = foc * 2;
|
---|
23 | translation_adder = 0;
|
---|
24 |
|
---|
25 | %max number of degrees to rotate
|
---|
26 | rotation_min = 5; %min no of degrees to rotate unless pure_translation == true
|
---|
27 | rotation_multplier = 8;
|
---|
28 |
|
---|
29 | %number of focal lengths nearest and furthest points are from camera one, all other points uniformly
|
---|
30 | % distributed between these two values
|
---|
31 | min_Z = 3;
|
---|
32 | Z_RAN = 10;
|
---|
33 | end
|
---|
34 |
|
---|
35 |
|
---|
36 | %not yet implmented
|
---|
37 | if nargin < 10
|
---|
38 | pure_translation = 0;
|
---|
39 | pure_rotation = 0;
|
---|
40 | orthographic = 0;
|
---|
41 | else
|
---|
42 | pure_translation = other_parameters(1);
|
---|
43 | pure_rotation = other_parameters(2);
|
---|
44 | orthographic = other_parameters(3);
|
---|
45 | end
|
---|
46 |
|
---|
47 |
|
---|
48 | C = eye(3);
|
---|
49 | C(1,3) = 0;
|
---|
50 | C(2,3) = 0;
|
---|
51 | C(3,3) = 1/foc;
|
---|
52 |
|
---|
53 | R = eye(3);
|
---|
54 | t = rand(3,1);
|
---|
55 | t = t * (translation_mult *norm(t)) +translation_adder;
|
---|
56 | true_t = t;
|
---|
57 | T = [0 -t(3) t(2); t(3) 0 -t(1); -t(2) t(1) 0];
|
---|
58 |
|
---|
59 | %
|
---|
60 | % rot_axis = rand(3,1);
|
---|
61 | % rot_axis = rot_axis/norm(rot_axis);
|
---|
62 | % rot_angle = 1/360 * 2 * pi * rand * rotation_multplier + rotation_min;
|
---|
63 | % %Rogregues
|
---|
64 | % II = [1 0 0; 0 1 0; 0 0 1];
|
---|
65 | % AX = torr_skew_sym(rot_axis);
|
---|
66 | % R = cos(rot_angle) * II +sin(rot_angle) * AX + (1 - cos(rot_angle)) * rot_axis * rot_axis';
|
---|
67 |
|
---|
68 |
|
---|
69 | theta = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
70 | n = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
71 | p = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
72 |
|
---|
73 | R(1,1) = (1 - cos(p)) * cos(n)* ( cos(n) * cos(theta) + sin(theta) * sin(n) ) + cos(p)* cos(theta);
|
---|
74 | R(1,2) = (1 - cos(p))* cos(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) - cos(p) *sin(theta);
|
---|
75 | R(1,3) = sin(n) *sin(p);
|
---|
76 | R(2,1) = (1 - cos(p)) *sin(n) *( cos(n) *cos(theta) + sin(theta)* sin(n) ) + cos(p)* sin(theta);
|
---|
77 | R(2,2) = (1 - cos(p)) *sin(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) + cos(p)* cos(theta);
|
---|
78 | R(2,3) = -cos(n) * sin(p);
|
---|
79 | R(3,1) = -sin(p) * ( sin(n) * cos(theta) - sin(theta) * cos(n));
|
---|
80 | R(3,2) = sin(p) * ( cos(n) * cos(theta) + sin(theta) * sin(n));
|
---|
81 | R(3,3) = cos(p);
|
---|
82 |
|
---|
83 | %R * R'
|
---|
84 | %R' * R
|
---|
85 |
|
---|
86 | perfect_matches = rand(no_matches,4);
|
---|
87 | noisy_matches = rand(no_matches,4);
|
---|
88 | m1 = rand(3,no_matches);
|
---|
89 | m2 = rand(3,no_matches);
|
---|
90 |
|
---|
91 | x1 = rand(no_matches,1);
|
---|
92 | x2 = rand(no_matches,1);
|
---|
93 | y1 = rand(no_matches,1);
|
---|
94 | y2 = rand(no_matches,1);
|
---|
95 |
|
---|
96 | X = rand(3,no_matches);
|
---|
97 |
|
---|
98 | %noisy data:----
|
---|
99 | nx1 = rand(no_matches,1);
|
---|
100 | nx2 = rand(no_matches,1);
|
---|
101 | ny1 = rand(no_matches,1);
|
---|
102 | ny2 = rand(no_matches,1);
|
---|
103 |
|
---|
104 | for(i = 1:no_matches)
|
---|
105 | X(3,i) = min_Z * foc + Z_RAN * foc * X(3,i);
|
---|
106 | X(1,i) = (X(1,i) * 512 -256 ) *X(3,i)/foc;
|
---|
107 | X(2,i) = (X(2,i) * 512 -256 ) * X(3,i)/foc;
|
---|
108 | end
|
---|
109 |
|
---|
110 |
|
---|
111 | m1 = C *X;
|
---|
112 | m2 = R *X;
|
---|
113 | m2 = m2 + t * ones(1,no_matches);
|
---|
114 | m2 = C * m2;
|
---|
115 | % m2 = C * ( R *X + t);
|
---|
116 |
|
---|
117 | %for(i = 1:no_matches)
|
---|
118 | x1 = (m1(1,:)./m1(3,:))';
|
---|
119 | y1 = (m1(2,:)./m1(3,:))';
|
---|
120 | x2 = (m2(1,:)./m2(3,:))';
|
---|
121 | y2 = (m2(2,:)./m2(3,:))';
|
---|
122 |
|
---|
123 |
|
---|
124 | perfect_matches(:,1) = x1(:);
|
---|
125 | perfect_matches(:,2) = y1(:);
|
---|
126 | perfect_matches(:,3) = x2(:);
|
---|
127 | perfect_matches(:,4) = y2(:);
|
---|
128 |
|
---|
129 |
|
---|
130 |
|
---|
131 |
|
---|
132 | % noisy_matches(:,1) = x1(:) + randn(no_matches,1) *noise_multiplier;
|
---|
133 | % noisy_matches(:,2) = y1(:) + randn(no_matches,1) *noise_multiplier;
|
---|
134 | % noisy_matches(:,3) = x2(:) + randn(no_matches,1) *noise_multiplier;
|
---|
135 | % noisy_matches(:,4) = y2(:) + randn(no_matches,1) *noise_multiplier;
|
---|
136 |
|
---|
137 | noisy_matches(:,1) = x1 + normrnd(0, noise_sigma, no_matches,1);
|
---|
138 | noisy_matches(:,2) = y1 + normrnd(0, noise_sigma, no_matches,1);
|
---|
139 | noisy_matches(:,3) = x2 + normrnd(0, noise_sigma, no_matches,1);
|
---|
140 | noisy_matches(:,4) = y2 + normrnd(0, noise_sigma, no_matches,1);
|
---|
141 |
|
---|
142 |
|
---|
143 | nx1 = noisy_matches(:,1);
|
---|
144 | ny1 = noisy_matches(:,2);
|
---|
145 | nx2 = noisy_matches(:,3);
|
---|
146 | ny2 = noisy_matches(:,4);
|
---|
147 |
|
---|
148 |
|
---|
149 |
|
---|
150 | %m2(i)' * F * m1(i)
|
---|
151 |
|
---|
152 | F = inv(C') * T * R * inv(C);
|
---|
153 |
|
---|
154 | %this is the wrong answer for the algebraic residual!! m1(i)' * F * m2(i)
|
---|
155 |
|
---|
156 | %note here m3 = 1, so need to adjust this to check the results...
|
---|
157 | MM = [1 0 0; 0 1 0; 0 0 1/m3];
|
---|
158 |
|
---|
159 |
|
---|
160 | F2 = MM * F * MM;
|
---|
161 | true_F = F2 / norm(F2);
|
---|
162 |
|
---|
163 | %return also the true calibration matrix, note we set the 3rd coordinate to m3 so
|
---|
164 | % we need to divide the focal length by this in the camera matrixc
|
---|
165 | true_C = C;
|
---|
166 | true_C(3,3) = true_C(3,3) * m3;
|
---|
167 | true_R = R;
|
---|
168 | true_TX = T;
|
---|
169 | true_E = T * R;
|
---|
170 | true_X = X;
|
---|
171 |
|
---|
172 | %torr_display_matches(perfect_matches);
|
---|
173 | %
|
---|
174 | % %i think this is the right one
|
---|
175 | % f_true = [F2(1) F2(2,:) F2(3,:)];
|
---|
176 | % f_true = f_true/norm(f_true);
|
---|
177 | %
|
---|
178 | %
|
---|
179 | % %wrong one
|
---|
180 | % fff = [F(:,1)' F(:,2)' F(:,3)']
|
---|
181 | % fff = fff/norm(fff);
|
---|
182 |
|
---|
183 | %e = errf2(fff,x1,y1,x2,y2, no_matches, m3); |
---|