1 | % By Philip Torr 2002
|
---|
2 | % copyright Microsoft Corp.
|
---|
3 | %generates a set of point matches + their F matrix!@
|
---|
4 |
|
---|
5 | %we need to make sure that the object isnt too affine
|
---|
6 |
|
---|
7 | %default values
|
---|
8 |
|
---|
9 | foc = 256.0;
|
---|
10 | no_matches = 100;
|
---|
11 | noise_multiplier = 1;
|
---|
12 | translation_mult = foc * 2;
|
---|
13 | translation_adder = 0;
|
---|
14 |
|
---|
15 | %max number of degrees to rotate
|
---|
16 | rotation_multplier = 2;
|
---|
17 | min_Z = 1;
|
---|
18 | Z_RAN = 10;
|
---|
19 |
|
---|
20 |
|
---|
21 | m3 = 256.0;
|
---|
22 | C = eye(3);
|
---|
23 | C(1,3) = 0;
|
---|
24 | C(2,3) = 0;
|
---|
25 | C(3,3) = 1/foc;
|
---|
26 |
|
---|
27 | R = eye(3);
|
---|
28 | t = rand(3,1);
|
---|
29 | t = t * (translation_mult *norm(t)) +translation_adder;
|
---|
30 | T = [0 -t(3) t(2); t(3) 0 -t(1); -t(2) t(1) 0];
|
---|
31 | T = T/norm(T);
|
---|
32 |
|
---|
33 | theta = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
34 | n = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
35 | p = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
36 |
|
---|
37 | R(1,1) = (1 - cos(p)) * cos(n)* ( cos(n) * cos(theta) + sin(theta) * sin(n) ) + cos(p)* cos(theta);
|
---|
38 | R(1,2) = (1 - cos(p))* cos(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) - cos(p) *sin(theta);
|
---|
39 | R(1,3) = sin(n) *sin(p);
|
---|
40 | R(2,1) = (1 - cos(p)) *sin(n) *( cos(n) *cos(theta) + sin(theta)* sin(n) ) + cos(p)* sin(theta);
|
---|
41 | R(2,2) = (1 - cos(p)) *sin(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) + cos(p)* cos(theta);
|
---|
42 | R(2,3) = -cos(n) * sin(p);
|
---|
43 | R(3,1) = -sin(p) * ( sin(n) * cos(theta) - sin(theta) * cos(n));
|
---|
44 | R(3,2) = sin(p) * ( cos(n) * cos(theta) + sin(theta) * sin(n));
|
---|
45 | R(3,3) = cos(p);
|
---|
46 |
|
---|
47 | %R * R'
|
---|
48 | %R' * R
|
---|
49 |
|
---|
50 | perfect_matches = rand(no_matches,4);
|
---|
51 | noisy_matches = rand(no_matches,4);
|
---|
52 | m1 = rand(3,no_matches);
|
---|
53 | m2 = rand(3,no_matches);
|
---|
54 |
|
---|
55 | x1 = rand(no_matches,1);
|
---|
56 | x2 = rand(no_matches,1);
|
---|
57 | y1 = rand(no_matches,1);
|
---|
58 | y2 = rand(no_matches,1);
|
---|
59 | u1 = rand(no_matches,1);
|
---|
60 | v1 = rand(no_matches,1);
|
---|
61 | X = rand(3,no_matches);
|
---|
62 |
|
---|
63 | %noisy data:----
|
---|
64 | nx1 = rand(no_matches,1);
|
---|
65 | nx2 = rand(no_matches,1);
|
---|
66 | ny1 = rand(no_matches,1);
|
---|
67 | ny2 = rand(no_matches,1);
|
---|
68 | nu1 = rand(no_matches,1);
|
---|
69 | nv1 = rand(no_matches,1);
|
---|
70 |
|
---|
71 | for(i = 1:no_matches)
|
---|
72 | X(3,i) = min_Z * foc + Z_RAN * foc * X(3,i);
|
---|
73 | X(1,i) = (X(1,i) * 512 -256 ) *X(3,i)/foc;
|
---|
74 | X(2,i) = (X(2,i) * 512 -256 ) * X(3,i)/foc ;
|
---|
75 | end
|
---|
76 |
|
---|
77 |
|
---|
78 | m1 = C *X;
|
---|
79 | m2 = R *X;
|
---|
80 | m2 = m2 + t * ones(1,no_matches);
|
---|
81 | m2 = C * m2;
|
---|
82 | % m2 = C * ( R *X + t);
|
---|
83 |
|
---|
84 | %for(i = 1:no_matches)
|
---|
85 | x1 = (m1(1,:)./m1(3,:))';
|
---|
86 | y1 = (m1(2,:)./m1(3,:))';
|
---|
87 | x2 = (m2(1,:)./m2(3,:))';
|
---|
88 | y2 = (m2(2,:)./m2(3,:))';
|
---|
89 |
|
---|
90 |
|
---|
91 | perfect_matches(:,1) = x1(:);
|
---|
92 | perfect_matches(:,2) = y1(:);
|
---|
93 | perfect_matches(:,3) = x2(:);
|
---|
94 | perfect_matches(:,4) = y2(:);
|
---|
95 |
|
---|
96 | u1(:) = x2(:) - x1(:);
|
---|
97 | v1(:) = y2(:) - y1(:);
|
---|
98 |
|
---|
99 |
|
---|
100 |
|
---|
101 | noisy_matches(:,1) = x1(:) + randn(no_matches,1) *noise_multiplier;
|
---|
102 | noisy_matches(:,2) = y1(:) + randn(no_matches,1) *noise_multiplier;
|
---|
103 | noisy_matches(:,3) = x2(:) + randn(no_matches,1) *noise_multiplier;
|
---|
104 | noisy_matches(:,4) = y2(:) + randn(no_matches,1) *noise_multiplier;
|
---|
105 |
|
---|
106 | nx1 = noisy_matches(:,1);
|
---|
107 | ny1 = noisy_matches(:,2);
|
---|
108 | nx2 = noisy_matches(:,3);
|
---|
109 | ny2 = noisy_matches(:,4);
|
---|
110 | nu1 = nx2(:) - nx1(:);
|
---|
111 | nv1 = ny2(:) - ny1(:);
|
---|
112 |
|
---|
113 |
|
---|
114 | %m2(i)' * F * m1(i)
|
---|
115 |
|
---|
116 | F = inv(C') * T * R * inv(C);
|
---|
117 |
|
---|
118 | %this is the wrong answer for the algebraic residual!! m1(i)' * F * m2(i)
|
---|
119 |
|
---|
120 | %note here m3 = 1, so need to adjust this to check the results...
|
---|
121 | MM = [1 0 0; 0 1 0; 0 0 1/m3];
|
---|
122 | MMC = MM * inv(C);
|
---|
123 |
|
---|
124 | %F2 = MM * F * MM;
|
---|
125 | F2 = MMC * T * R * MMC;
|
---|
126 | true_F = F2 / norm(F2);
|
---|
127 | %
|
---|
128 | % %i think this is the right one
|
---|
129 | % f_true = [F2(1) F2(2,:) F2(3,:)];
|
---|
130 | % f_true = f_true/norm(f_true);
|
---|
131 | %
|
---|
132 | %
|
---|
133 | % %wrong one
|
---|
134 | % fff = [F(:,1)' F(:,2)' F(:,3)']
|
---|
135 | % fff = fff/norm(fff);
|
---|
136 |
|
---|
137 | %e = errf2(fff,x1,y1,x2,y2, no_matches, m3);
|
---|
138 |
|
---|
139 | tf = true_F;
|
---|
140 | %tf = F;
|
---|
141 | 0.5 * (trace(tf * tf'))^2 - trace((tf * tf')^2)
|
---|
142 |
|
---|
143 | tf * (tf' * tf) - 0.5 .* (trace(tf * tf') * tf)
|
---|
144 |
|
---|
145 | svd(true_F)
|
---|
146 | eig(true_F) |
---|