1 | %% File: dinoTestF |
---|
2 | %% A4 2003 handout code |
---|
3 | %% Estimate F matrix from synthetic corresponding points. |
---|
4 | %% |
---|
5 | %% ADJ, Nov. 03 |
---|
6 | |
---|
7 | clear |
---|
8 | close all |
---|
9 | FALSE = 1 == 0; |
---|
10 | TRUE = ~FALSE; |
---|
11 | |
---|
12 | global matlabVisRoot |
---|
13 | |
---|
14 | %% We need to ensure the path is set for the iseToolbox. |
---|
15 | if length(matlabVisRoot)==0 |
---|
16 | dir = pwd; |
---|
17 | cd /h/51/jepson/pub/matlab %% CHANGE THIS to your startup directory |
---|
18 | startup; |
---|
19 | cd(dir); |
---|
20 | end |
---|
21 | |
---|
22 | reconRoot = [matlabVisRoot '/utvisToolbox/tutorials/3dRecon']; |
---|
23 | addpath([reconRoot '/utils']); |
---|
24 | |
---|
25 | |
---|
26 | % Random number generator seed: |
---|
27 | seed = round(sum(1000*clock)); |
---|
28 | rand('state', seed); |
---|
29 | seed0 = seed; |
---|
30 | % We also need to start randn. Use a seedn generated from seed: |
---|
31 | seedn = round(rand(1,1) * 1.0e+6); |
---|
32 | randn('state', seedn); |
---|
33 | |
---|
34 | nTrial = 10; %% Number of ransac trials to use |
---|
35 | |
---|
36 | |
---|
37 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
38 | %% Set up cameras |
---|
39 | %% The cameras are automatically rotated by projectDino to fixate |
---|
40 | %% on the mean of the 3D points. We do not always want to allow |
---|
41 | %% the cameras to move in this fashion (eg. when we change sclZ). |
---|
42 | %% So we will compute the rotations for the left and right cameras |
---|
43 | %% once and for all, and then use these. |
---|
44 | f = 100; % focal length |
---|
45 | dLeft = [-50, 0, -150]'; % Center of projection for left camera |
---|
46 | dRight = [50, 0, -150]'; % Center of projection for right camera |
---|
47 | %% Compute camera rotations to fixate on Dino's center. |
---|
48 | [pLeft polys MintLeft MextLeft] = projectDino(f, dLeft, [], 1.0); |
---|
49 | Rleft = MextLeft(:, 1:3); |
---|
50 | [pRight polys MintRight MextRight] = projectDino(f, dRight, [], 1.0); |
---|
51 | Rright = MextRight(:, 1:3); |
---|
52 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
53 | |
---|
54 | %% Generate data... |
---|
55 | sclZ = 1.0; |
---|
56 | %% Dino left image data |
---|
57 | [pLeft polys MintLeft MextLeft] = projectDino(f, dLeft, Rleft, sclZ); |
---|
58 | |
---|
59 | %% Dino right image data |
---|
60 | [pRight polys MintRight MextRight] = projectDino(f, dRight, Rright, sclZ); |
---|
61 | |
---|
62 | |
---|
63 | %% Show left and right images |
---|
64 | hFig = figure(1); clf; |
---|
65 | plot(pLeft(1,:), pLeft(2,:), '.b'); |
---|
66 | axis xy; axis equal; |
---|
67 | xlabel('X'); ylabel('Y'); |
---|
68 | axis([-150 150 -100 100]); |
---|
69 | title('Left image of Dino'); |
---|
70 | pause(0.1); |
---|
71 | |
---|
72 | hFig = figure(2); clf; |
---|
73 | plot(pRight(1,:), pRight(2,:), '.b'); |
---|
74 | axis xy; axis equal; |
---|
75 | axis([-150 150 -100 100]); |
---|
76 | xlabel('X'); ylabel('Y'); |
---|
77 | title('Right image of Dino'); |
---|
78 | pause(0.1); |
---|
79 | |
---|
80 | |
---|
81 | %% Build correspondence data |
---|
82 | clear imPts; |
---|
83 | imPts = cat(3, pLeft, pRight); |
---|
84 | nPts = size(imPts,2); |
---|
85 | if size(imPts,1) == 2 |
---|
86 | imPts = cat(1, imPts, ones(1,nPts,2)); |
---|
87 | end |
---|
88 | |
---|
89 | |
---|
90 | |
---|
91 | %% RANSAC for F |
---|
92 | seeds = {}; |
---|
93 | sigma = 2.0; rho = 2; |
---|
94 | for kTrial = 1: nTrial |
---|
95 | %% Test out F matrix on a random sample of 8 points |
---|
96 | idTest = randperm(nPts); |
---|
97 | nTest = min(8, nPts); |
---|
98 | idTest = idTest(1:nTest); |
---|
99 | |
---|
100 | %% Solve for F matrix on the random sample |
---|
101 | [F Sa Sf] = linEstF(imPts(:,idTest,1), imPts(:,idTest,2),1); |
---|
102 | |
---|
103 | %% Compute perpendicular error of all points to epipolar lines |
---|
104 | perpErrL = zeros(1,nPts); |
---|
105 | for k = 1:nPts |
---|
106 | lk = imPts(:,k,2)' * F'; |
---|
107 | perpErrL(k) = (lk* imPts(:,k,1))/norm(lk(1:2)); |
---|
108 | end |
---|
109 | |
---|
110 | %% Detect inliers |
---|
111 | idInlier = abs(perpErrL) < rho*sigma; |
---|
112 | |
---|
113 | %% Count inliers |
---|
114 | nInlier = sum(idInlier); |
---|
115 | if nInlier > 20 |
---|
116 | %% Store sets of sampled points with at least 20 inliers |
---|
117 | seed.id = idTest; |
---|
118 | seed.idInlier = idInlier; |
---|
119 | seed.nInlier = nInlier; |
---|
120 | seed.F = F; |
---|
121 | |
---|
122 | kSeed = length(seeds)+1 |
---|
123 | seeds{kSeed} = seed; |
---|
124 | end |
---|
125 | end |
---|
126 | %% Done RANSAC trials |
---|
127 | |
---|
128 | %% Extract best solution |
---|
129 | nInliers = zeros(1, length(seeds)); |
---|
130 | for ks = 1:length(seeds) |
---|
131 | nInliers(ks) = seeds{ks}.nInlier; |
---|
132 | end |
---|
133 | [nM ks] = max(nInliers); |
---|
134 | nInliers(ks) |
---|
135 | |
---|
136 | %% Refine estimate of F using all inliers. |
---|
137 | F = seeds{ks}.F; |
---|
138 | idInlier = seeds{ks}.idInlier; |
---|
139 | |
---|
140 | idInlierOld = idInlier; |
---|
141 | sum(idInlier) |
---|
142 | %% Do at most 10 iterations attempting to entrain as many points as possible. |
---|
143 | for kIt = 1: 10 |
---|
144 | %% Fit F using all current inliers |
---|
145 | [F Sa Sf] = linEstF(imPts(:,idInlier,1), imPts(:,idInlier,2),1); |
---|
146 | |
---|
147 | %% Compute perpendicular error to epipolar lines |
---|
148 | perpErrL = zeros(1,nPts); |
---|
149 | for k = 1:nPts |
---|
150 | lk = imPts(:,k,2)' * F'; |
---|
151 | perpErrL(k) = (lk* imPts(:,k,1))/norm(lk(1:2)); |
---|
152 | end |
---|
153 | idInlier = abs(perpErrL) < rho*sigma; |
---|
154 | nInlier = sum(idInlier) |
---|
155 | |
---|
156 | %% If we have the same set of inliers as the previous iteration then stop. |
---|
157 | if all(idInlier == idInlierOld) |
---|
158 | break; |
---|
159 | end |
---|
160 | idInlierOld = idInlier; |
---|
161 | end |
---|
162 | |
---|
163 | %%%%%%%%%%%%%%%%%%%%% Plot results |
---|
164 | nTest = 64; %% Number of epipolar lines to plot |
---|
165 | nCol = 16; %% Number of different colours to use. |
---|
166 | col = hsv(nCol); %% Colour map. |
---|
167 | |
---|
168 | %% Random sample the lines to plot |
---|
169 | idLines = randperm(nPts); |
---|
170 | idLines = idLines(1:nTest); |
---|
171 | |
---|
172 | %% Show left image |
---|
173 | hFig = figure(1); |
---|
174 | clf; hold on; |
---|
175 | % Plot all interest point locations as blue .'s |
---|
176 | plot(imPts(1,:,1), imPts(2,:,1), '.b'); |
---|
177 | axis([-150 150 -100 100]); axis xy; axis equal; |
---|
178 | ax = axis; |
---|
179 | cropBox = [ax(1) ax(3) ax(2) ax(4)]; |
---|
180 | title('Epipolar Lines in Left Image'); |
---|
181 | for kl = 1:length(idLines) |
---|
182 | % Plot interest point location corresponding to epipolar line as a "o" |
---|
183 | % in the same colour as the epipolar line. |
---|
184 | k = idLines(kl); |
---|
185 | plot(imPts(1,k,1), imPts(2,k,1), 'o', 'Color', col(mod(k,nCol)+1,:)); |
---|
186 | % Plot epipolar line. |
---|
187 | lk = imPts(:,k,2)' * F'; |
---|
188 | epk = cropLineInBox(lk(1:2), lk(3), cropBox); |
---|
189 | set(line(epk(:,1), epk(:,2)), 'Color', col(mod(k,nCol)+1,:)); |
---|
190 | end |
---|
191 | |
---|
192 | %% Show right image |
---|
193 | hFig = figure(2); |
---|
194 | clf; hold on; |
---|
195 | % Plot all interest point locations as blue .'s |
---|
196 | plot(imPts(1,:,2), imPts(2,:,2), '.b'); |
---|
197 | axis([-150 150 -100 100]); axis xy; axis equal; |
---|
198 | ax = axis; |
---|
199 | cropBox = [ax(1) ax(3) ax(2) ax(4)]; |
---|
200 | title('Epipolar Lines in Right Image'); |
---|
201 | perpErrR = []; |
---|
202 | for kl = 1:length(idLines) |
---|
203 | % Plot interest point location corresponding to epipolar line as a "o" |
---|
204 | % in the same colour as the epipolar line. |
---|
205 | k = idLines(kl); |
---|
206 | plot(imPts(1,k,2), imPts(2,k,2), 'o', 'Color', col(mod(k,nCol)+1,:)); |
---|
207 | % Plot epipolar line. |
---|
208 | lk = imPts(:,k,1)' * F; |
---|
209 | epk = cropLineInBox(lk(1:2), lk(3), cropBox); |
---|
210 | set(line(epk(:,1), epk(:,2)), 'Color', col(mod(k,nCol)+1,:)); |
---|
211 | perpErrR = [perpErrR (lk * imPts(:,k,2)/norm(lk(1:2)))]; |
---|
212 | end |
---|
213 | |
---|
214 | %% Compute perpendicular distance to epipolar lines in left and right images. |
---|
215 | perpErrL = []; |
---|
216 | for k = 1:nPts |
---|
217 | lk = imPts(:,k,2)' * F'; |
---|
218 | perpErrL = [perpErrL (lk * imPts(:,k,1))/norm(lk(1:2))]; |
---|
219 | end |
---|
220 | perpErrR = []; |
---|
221 | for k = 1:nPts |
---|
222 | lk = imPts(:,k,1)' * F; |
---|
223 | perpErrR = [perpErrR (lk * imPts(:,k,2)/norm(lk(1:2)))]; |
---|
224 | end |
---|
225 | |
---|
226 | %% Plot a histogram of the perpendicular distances |
---|
227 | err = [perpErrL perpErrR]; |
---|
228 | err = min(err, 10); |
---|
229 | err = max(err, -10); |
---|
230 | [n b] = histo(err, 64); |
---|
231 | figure(4); clf; |
---|
232 | plot(b,n); |
---|
233 | title('Distance to epipolar line'); |
---|
234 | xlabel('Error in pixels'); |
---|
235 | ylabel('Frequency'); |
---|
236 | |
---|
237 | %% Count inliers |
---|
238 | idL = abs(perpErrL)< rho*sigma; |
---|
239 | idR = abs(perpErrR) < rho*sigma; |
---|
240 | idInlier = idL & idR; |
---|
241 | sum(idInlier) |
---|
242 | sum(idInlier)/nPts |
---|
243 | |
---|
244 | %% save 'Fcorr' F Sa Sf idInlier nInliers |
---|