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