1 | %% File: grappleFmatrix |
---|
2 | %% A4 2003 handout code |
---|
3 | %% Uses RANSAC to estimate F matrix from corresponding points. |
---|
4 | %% |
---|
5 | %% ADJ, Nov. 03 |
---|
6 | |
---|
7 | clear |
---|
8 | close all |
---|
9 | FALSE = 1 == 0; |
---|
10 | TRUE = ~FALSE; |
---|
11 | global matlabVisRoot |
---|
12 | |
---|
13 | %% We need to ensure the path is set for the iseToolbox. |
---|
14 | if length(matlabVisRoot)==0 |
---|
15 | dir = pwd; |
---|
16 | cd /h/51/jepson/pub/matlab %% CHANGE THIS |
---|
17 | startup; |
---|
18 | cd(dir); |
---|
19 | end |
---|
20 | |
---|
21 | reconRoot = [matlabVisRoot '/utvisToolbox/tutorials/3dRecon']; |
---|
22 | addpath([reconRoot '/data/wadham']); |
---|
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 try. |
---|
35 | |
---|
36 | %% Wadham left image: use wadham/001.jpg |
---|
37 | imPath = 'data/wadham/'; fnameLeft = '001'; |
---|
38 | im = imread([imPath fnameLeft],'jpg'); |
---|
39 | im = double(im(:,:,2)); |
---|
40 | imDwn = blurDn(im,1)/2; |
---|
41 | imLeft = imDwn; |
---|
42 | |
---|
43 | %% Read correspondence data |
---|
44 | load data/wadham/corrPnts5 |
---|
45 | %% Wadham right image data/wadham002-5.jpg use for corrPnts2-5 respectively |
---|
46 | fnameRight = '005'; |
---|
47 | im = imread([imPath fnameRight],'jpg'); |
---|
48 | im = double(im(:,:,2)); |
---|
49 | imDwn = blurDn(im,1)/2; |
---|
50 | imRight = imDwn; |
---|
51 | |
---|
52 | clear imPts; |
---|
53 | imPts = cat(3, im_pos1', im_pos2'); |
---|
54 | nPts = size(imPts,2); |
---|
55 | if size(imPts,1) == 2 |
---|
56 | imPts = cat(1, imPts, ones(1,nPts,2)); |
---|
57 | end |
---|
58 | |
---|
59 | %% RANSAC for F |
---|
60 | seeds = {}; |
---|
61 | sigma = 2.0; rho = 2; |
---|
62 | for kTrial = 1: nTrial |
---|
63 | %% Test out F matrix on a random sample of 8 points |
---|
64 | idTest = randperm(nPts); |
---|
65 | nTest = min(8, nPts); |
---|
66 | idTest = idTest(1:nTest); |
---|
67 | |
---|
68 | %% Solve for F matrix on the random sample |
---|
69 | [F Sa Sf] = linEstF(imPts(:,idTest,1), imPts(:,idTest,2),1); |
---|
70 | |
---|
71 | %% Compute perpendicular error of all points to epipolar lines |
---|
72 | perpErrL = zeros(1,nPts); |
---|
73 | for k = 1:nPts |
---|
74 | lk = imPts(:,k,2)' * F'; |
---|
75 | perpErrL(k) = (lk* imPts(:,k,1))/norm(lk(1:2)); |
---|
76 | end |
---|
77 | |
---|
78 | %% Detect inliers |
---|
79 | idInlier = abs(perpErrL) < rho*sigma; |
---|
80 | |
---|
81 | %% Count inliers |
---|
82 | nInlier = sum(idInlier); |
---|
83 | if nInlier > 20 |
---|
84 | %% Store sets of sampled points with at least 20 inliers |
---|
85 | seed.id = idTest; |
---|
86 | seed.idInlier = idInlier; |
---|
87 | seed.nInlier = nInlier; |
---|
88 | seed.F = F; |
---|
89 | |
---|
90 | kSeed = length(seeds)+1 |
---|
91 | seeds{kSeed} = seed; |
---|
92 | end |
---|
93 | end |
---|
94 | %% Done RANSAC trials |
---|
95 | |
---|
96 | %% Extract best solution |
---|
97 | nInliers = zeros(1, length(seeds)); |
---|
98 | for ks = 1:length(seeds) |
---|
99 | nInliers(ks) = seeds{ks}.nInlier; |
---|
100 | end |
---|
101 | [nM ks] = max(nInliers); |
---|
102 | nInliers(ks) |
---|
103 | |
---|
104 | %% Refine estimate of F using all inliers. |
---|
105 | F = seeds{ks}.F; |
---|
106 | idInlier = seeds{ks}.idInlier; |
---|
107 | |
---|
108 | idInlierOld = idInlier; |
---|
109 | sum(idInlier) |
---|
110 | %% Do at most 10 iterations attempting to entrain as many points as possible. |
---|
111 | for kIt = 1: 10 |
---|
112 | %% Fit F using all current inliers |
---|
113 | [F Sa Sf] = linEstF(imPts(:,idInlier,1), imPts(:,idInlier,2),1); |
---|
114 | |
---|
115 | %% Compute perpendicular error to epipolar lines |
---|
116 | perpErrL = zeros(1,nPts); |
---|
117 | for k = 1:nPts |
---|
118 | lk = imPts(:,k,2)' * F'; |
---|
119 | perpErrL(k) = (lk* imPts(:,k,1))/norm(lk(1:2)); |
---|
120 | end |
---|
121 | idInlier = abs(perpErrL) < rho*sigma; |
---|
122 | nInlier = sum(idInlier) |
---|
123 | |
---|
124 | %% If we have the same set of inliers as the previous iteration then stop. |
---|
125 | if all(idInlier == idInlierOld) |
---|
126 | break; |
---|
127 | end |
---|
128 | idInlierOld = idInlier; |
---|
129 | end |
---|
130 | |
---|
131 | %%%%%%%%%%%%%%%%%%%%% Plot results |
---|
132 | nTest = 64; %% Number of epipolar lines to plot |
---|
133 | nCol = 16; %% Number of different colours to use. |
---|
134 | col = hsv(nCol); %% Colour map. |
---|
135 | |
---|
136 | %% Random sample the lines to plot |
---|
137 | idLines = randperm(nPts); |
---|
138 | idLines = idLines(1:nTest); |
---|
139 | |
---|
140 | %% Show left image |
---|
141 | SUPERIMPOSE = TRUE; |
---|
142 | hFig = figure(2); |
---|
143 | clf; |
---|
144 | if SUPERIMPOSE |
---|
145 | image(imLeft); |
---|
146 | colormap(gray(256)); |
---|
147 | end |
---|
148 | resizeImageFig(hFig, size(imDwn), 1); hold on; |
---|
149 | set(get(hFig,'CurrentAxes'),'Ydir','reverse'); |
---|
150 | hold on; |
---|
151 | % Plot all interest point locations as blue .'s |
---|
152 | plot(imPts(1,:,1), imPts(2,:,1), '.b'); |
---|
153 | set(gca,'YDir', 'reverse'); |
---|
154 | ax = axis; |
---|
155 | cropBox = [ax(1) ax(3) ax(2) ax(4)]; |
---|
156 | title('Epipolar Lines in Left Image'); |
---|
157 | for kl = 1:length(idLines) |
---|
158 | % Plot interest point location corresponding to epipolar line as a "o" |
---|
159 | % in the same colour as the epipolar line. |
---|
160 | k = idLines(kl); |
---|
161 | plot(imPts(1,k,1), imPts(2,k,1), 'o', 'Color', col(mod(k,nCol)+1,:)); |
---|
162 | % Plot epipolar line. |
---|
163 | lk = imPts(:,k,2)' * F'; |
---|
164 | epk = cropLineInBox(lk(1:2), lk(3), cropBox); |
---|
165 | set(line(epk(:,1), epk(:,2)), 'Color', col(mod(k,nCol)+1,:)); |
---|
166 | end |
---|
167 | |
---|
168 | |
---|
169 | %% Show right image |
---|
170 | SUPERIMPOSE = TRUE; |
---|
171 | hFig = figure(3); |
---|
172 | clf; |
---|
173 | if SUPERIMPOSE |
---|
174 | image(imRight); |
---|
175 | colormap(gray(256)); |
---|
176 | end |
---|
177 | resizeImageFig(hFig, size(imDwn), 1); hold on; |
---|
178 | set(get(hFig,'CurrentAxes'),'Ydir','reverse'); |
---|
179 | hold on; |
---|
180 | % Plot all interest point locations as blue .'s |
---|
181 | plot(imPts(1,:,2), imPts(2,:,2), '.b'); |
---|
182 | set(gca,'YDir', 'reverse'); |
---|
183 | ax = axis; |
---|
184 | cropBox = [ax(1) ax(3) ax(2) ax(4)]; |
---|
185 | title('Epipolar Lines in Left Image'); |
---|
186 | perpErrR = []; |
---|
187 | for kl = 1:length(idLines) |
---|
188 | % Plot interest point location corresponding to epipolar line as a "o" |
---|
189 | % in the same colour as the epipolar line. |
---|
190 | k = idLines(kl); |
---|
191 | plot(imPts(1,k,2), imPts(2,k,2), 'o', 'Color', col(mod(k,nCol)+1,:)); |
---|
192 | % Plot epipolar line. |
---|
193 | lk = imPts(:,k,1)' * F; |
---|
194 | epk = cropLineInBox(lk(1:2), lk(3), cropBox); |
---|
195 | set(line(epk(:,1), epk(:,2)), 'Color', col(mod(k,nCol)+1,:)); |
---|
196 | perpErrR = [perpErrR (lk * imPts(:,k,2)/norm(lk(1:2)))]; |
---|
197 | end |
---|
198 | |
---|
199 | %% Compute perpendicular distance to epipolar lines in left and right images. |
---|
200 | perpErrL = []; |
---|
201 | for k = 1:nPts |
---|
202 | lk = imPts(:,k,2)' * F'; |
---|
203 | perpErrL = [perpErrL (lk * imPts(:,k,1))/norm(lk(1:2))]; |
---|
204 | end |
---|
205 | perpErrR = []; |
---|
206 | for k = 1:nPts |
---|
207 | lk = imPts(:,k,1)' * F; |
---|
208 | perpErrR = [perpErrR (lk * imPts(:,k,2)/norm(lk(1:2)))]; |
---|
209 | end |
---|
210 | |
---|
211 | %% Plot a histogram of the perpendicular distances |
---|
212 | err = [perpErrL perpErrR]; |
---|
213 | err = min(err, 10); |
---|
214 | err = max(err, -10); |
---|
215 | [n b] = histo(err, 64); |
---|
216 | figure(4); clf; |
---|
217 | plot(b,n); |
---|
218 | title('Distance to epipolar line'); |
---|
219 | xlabel('Error in pixels'); |
---|
220 | ylabel('Frequency'); |
---|
221 | |
---|
222 | %% Count inliers |
---|
223 | idL = abs(perpErrL)< rho*sigma; |
---|
224 | idR = abs(perpErrR) < rho*sigma; |
---|
225 | idInlier = idL & idR; |
---|
226 | sum(idInlier) |
---|
227 | sum(idInlier)/nPts |
---|
228 | |
---|
229 | %% save 'Fcorr' F Sa Sf idInlier nInliers |
---|