[37] | 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 |
---|