[37] | 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 2 | %% Script file: projectiveMassageDino.m |
---|
| 3 | %% |
---|
| 4 | %% Demonstrate 3D affine and Euclidean reconstructions from corresponding |
---|
| 5 | %% points in perspective projection. |
---|
| 6 | %% |
---|
| 7 | %% The perspective factorization algorithm is due to Mahamud, Hebert, |
---|
| 8 | %% Omori, and Ponce, CVPR, 2001. I replaced the generalized eigenvalue |
---|
| 9 | %% problem with a few optimization steps along the gradient. |
---|
| 10 | %% |
---|
| 11 | %% Note, the self calibration follows the algorithm of Polleyfeys, Koch, and van Gool, |
---|
| 12 | %% IJCV, 1998, except we search for a nearby rank 3 Q matrix. |
---|
| 13 | %% |
---|
| 14 | %% TTD: |
---|
| 15 | %% - The self-calibration method is quite sensitive to rescaling. |
---|
| 16 | %% Some thoughts on how to do this are warranted. |
---|
| 17 | %% - We should be solving a CONSTRAINED (and normalized) least squares |
---|
| 18 | %% problem for Q, where the constraint is that Q has rank 3. See literature? |
---|
| 19 | %% - Turning the self-calibration off is now a problem. Scaling issue? |
---|
| 20 | %% |
---|
| 21 | %% ADJ, Nov. 03. |
---|
| 22 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 23 | |
---|
| 24 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 25 | %%% Initialization |
---|
| 26 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 27 | close all; |
---|
| 28 | clear |
---|
| 29 | global TRUE FALSE; |
---|
| 30 | global matlabVisRoot; |
---|
| 31 | |
---|
| 32 | TRUE = 1==1; |
---|
| 33 | FALSE = ~TRUE; |
---|
| 34 | |
---|
| 35 | if length(matlabVisRoot) == 0 |
---|
| 36 | dir = pwd; |
---|
| 37 | cd '/h/51/jepson/pub/matlab'; % CHANGE THIS |
---|
| 38 | %cd 'C:/work/Matlab' |
---|
| 39 | startup; |
---|
| 40 | cd(dir); |
---|
| 41 | end |
---|
| 42 | reconRoot = [matlabVisRoot '/utvisToolbox/tutorials']; |
---|
| 43 | |
---|
| 44 | addpath([reconRoot '/3dRecon/utils']); |
---|
| 45 | addpath([reconRoot '/3dRecon/projective']); |
---|
| 46 | |
---|
| 47 | % Random number generator seed: |
---|
| 48 | seed = round(sum(1000*clock)); |
---|
| 49 | rand('state', seed); |
---|
| 50 | seed0 = seed; |
---|
| 51 | % We also need to start randn. Use a seedn generated from seed: |
---|
| 52 | seedn = round(rand(1,1) * 1.0e+6); |
---|
| 53 | randn('state', seedn); |
---|
| 54 | |
---|
| 55 | %% Control Flags |
---|
| 56 | DEBUG = FALSE; %% TRUE => Extra output |
---|
| 57 | HOMOG_DEMO = TRUE; %% Show effect of simple homography on 3D data points |
---|
| 58 | NUM_RESCALE = TRUE; %% Use Hartley's rescaling of image data for stability |
---|
| 59 | SELF_CALIBRATE = TRUE; %% Use only self-calibration constraints in metric |
---|
| 60 | %% estimation. (FALSE => broken, for some reason) |
---|
| 61 | UNDO_RESCALE = FALSE & NUM_RESCALE; %% TRUE => Use original scaling of image data |
---|
| 62 | %% during metric upgrade. (Not good.) |
---|
| 63 | |
---|
| 64 | %% Parameters |
---|
| 65 | sigma = 1.0; %% Std Dev of noise (in pixels) in point locations |
---|
| 66 | tol = 1.0e-9; %% Convergence tolerance for z-estimation in projective fit |
---|
| 67 | nIm = 10; %% Number of data images to use (try 3-10). |
---|
| 68 | %% nIm = 2 is enough for projective reconstruction, but not |
---|
| 69 | %% for metric reconstruction algorithm. |
---|
| 70 | |
---|
| 71 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 72 | %%% Read Dino data set |
---|
| 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 74 | [v f] = getHalfDino; |
---|
| 75 | |
---|
| 76 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 77 | %%% Place Dino in a fixed 3D pose in world coordinate frame and display |
---|
| 78 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 79 | %%% Set canonical 3D pose |
---|
| 80 | R0 = [1 0 0; 0 0 1; 0 -1 0]; %% Rotate and reflect dino (concave away). |
---|
| 81 | mn0 = [0 0 0]'; %% Place Dino's mean on Z axis. |
---|
| 82 | P0 = R0 * v' + repmat(mn0, 1, size(v,1)); |
---|
| 83 | if size(P0,1)==3 |
---|
| 84 | P0 = [P0; ones(1,size(P0,2))]; |
---|
| 85 | end |
---|
| 86 | fprintf(2,'Depth statistics...'); |
---|
| 87 | imStats(P0(3,:)); |
---|
| 88 | nPts = size(P0,2); |
---|
| 89 | |
---|
| 90 | %%%%% Surface plot of data. |
---|
| 91 | figure(3); clf; |
---|
| 92 | for k = 1:length(f) |
---|
| 93 | vf = P0(:, f{k}); |
---|
| 94 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 95 | end |
---|
| 96 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 97 | axis vis3d; axis square; axis equal; |
---|
| 98 | title('Dino Model'); |
---|
| 99 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 100 | fprintf(2,'Rotate this figure.\n'); |
---|
| 101 | fprintf(2,'Press any key to continue...'); |
---|
| 102 | pause; fprintf(2,'ok\n'); |
---|
| 103 | |
---|
| 104 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 105 | %% Create some camera locations |
---|
| 106 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 107 | %% Columns of dMat give X,Y,Z locations of camera in world coordinate frame. |
---|
| 108 | dMat = [ 0 0 0 50 -70 -50 10 80 2.5 10 |
---|
| 109 | 0 50 -100 -20 10 0 -50 -20 10 -10 |
---|
| 110 | -150 -140 -145 -160 -145 -155 -150 -160 -140 -145]; |
---|
| 111 | %% Focal lengths for each of these 10 cameras. |
---|
| 112 | fMat = [100 125 150 125 125 120 120 120 120 120]'; |
---|
| 113 | %% Camera rotation will be chosen to fixate Dino's center. |
---|
| 114 | |
---|
| 115 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 116 | %% Grab nIm Scaled - Orthographic images (nIm <= length(fMat) == 10) |
---|
| 117 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 118 | imPts = []; |
---|
| 119 | zPts = []; |
---|
| 120 | M_GT = []; |
---|
| 121 | for k = 1:nIm |
---|
| 122 | |
---|
| 123 | d = dMat(:,k); % Camera center |
---|
| 124 | f1 = fMat(k); % Focal length |
---|
| 125 | |
---|
| 126 | %% Choose rotation to fixate mn0, say |
---|
| 127 | %% That is solve: R * (mn0 - d) = [0 0 1]'; |
---|
| 128 | R = eye(3); |
---|
| 129 | R(:,3) = (mn0 - d)/norm(mn0 - d); |
---|
| 130 | R(:,2) = R(:,2) - (R(:,3)' * R(:,2)) * R(:,3); |
---|
| 131 | R(:,2) = R(:,2)/norm(R(:,2)); |
---|
| 132 | R(:,1) = R(:,1) - R(:,2:3) * (R(:,2:3)' * R(:,1)); |
---|
| 133 | R(:,1) = R(:,1)/norm(R(:,1)); |
---|
| 134 | R = R'; |
---|
| 135 | if DEBUG |
---|
| 136 | fprintf(2,'Sanity check:\n Should be [0 0 Center_Depth]: %f %f %f\n', ... |
---|
| 137 | (R * (mn0 - d))); |
---|
| 138 | end |
---|
| 139 | |
---|
| 140 | %% Build camera matrix K and image formation matrix M. |
---|
| 141 | K = diag([f1, f1, 1]); |
---|
| 142 | M = K * [R -R*d]; |
---|
| 143 | if size(M_GT,1)>0 |
---|
| 144 | M_GT = [M_GT; M]; |
---|
| 145 | else |
---|
| 146 | M_GT = M; |
---|
| 147 | end |
---|
| 148 | |
---|
| 149 | %% Compute the projected image locations |
---|
| 150 | p = M * P0; |
---|
| 151 | |
---|
| 152 | %% Concatenate ground truth depth values over multiple frames. |
---|
| 153 | if size(zPts,1)>0 |
---|
| 154 | zPts = cat(3, zPts, p(3,:)); |
---|
| 155 | else |
---|
| 156 | zPts = p(3,:); |
---|
| 157 | end |
---|
| 158 | |
---|
| 159 | %% Concatenate noisy image points over multiple frames. |
---|
| 160 | p = p ./ repmat(p(3,:),3,1); |
---|
| 161 | p(1:2,:) = p(1:2,:) + sigma * randn(2,nPts); %% Add noise |
---|
| 162 | if size(imPts,1)>0 |
---|
| 163 | imPts = cat(3, imPts, p(1:2,:)); |
---|
| 164 | else |
---|
| 165 | imPts = p(1:2,:); |
---|
| 166 | end |
---|
| 167 | |
---|
| 168 | %% Show image |
---|
| 169 | figure(10); clf; |
---|
| 170 | h = plot(p(1,:), p(2,:), '.b'); |
---|
| 171 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 172 | axis([-150 150 -150 150]); |
---|
| 173 | title(sprintf('Image %d',k)); |
---|
| 174 | fprintf(2,'Press any key to continue...'); |
---|
| 175 | pause; |
---|
| 176 | fprintf(2,'ok\n'); |
---|
| 177 | |
---|
| 178 | end %% Forming images. |
---|
| 179 | |
---|
| 180 | |
---|
| 181 | %% Reorder the matrices zPts and imPts in the form nIm by nPts |
---|
| 182 | if size(zPts,1) == 1 & size(zPts,2) == nPts & size(zPts,3) == nIm |
---|
| 183 | zPts = permute(zPts, [3 2 1]); |
---|
| 184 | zPts = reshape(zPts, nIm, nPts); |
---|
| 185 | end |
---|
| 186 | if size(imPts,1) == 2 |
---|
| 187 | imPts = cat(1, imPts, ones(1, nPts,nIm)); |
---|
| 188 | end |
---|
| 189 | if size(imPts,2) == nPts & size(imPts,3) == nIm |
---|
| 190 | imPts = permute(imPts,[1, 3, 2]); |
---|
| 191 | end |
---|
| 192 | %% Set homogeneous 3rd coord in imPts |
---|
| 193 | imPts(3,:,:) = 1; |
---|
| 194 | imPtsSave = imPts; %% Save the image points in case we mess up... |
---|
| 195 | |
---|
| 196 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 197 | %% END of data generation |
---|
| 198 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 199 | |
---|
| 200 | |
---|
| 201 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 202 | %% Rescale image data (attempting to obtain more numerical stability). |
---|
| 203 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 204 | Knum = repmat(eye(3), [1 1 nIm]); |
---|
| 205 | if NUM_RESCALE |
---|
| 206 | %%% Rescale for numerical stability |
---|
| 207 | mn = sum(imPts(1:2,:,:),3)/nPts; |
---|
| 208 | %mns = reshape(mn, [2 nIm 1]); |
---|
| 209 | var = sum(sum((imPts(1:2,:,:)-repmat(mn, [1 1 nPts])).^2,3)/nPts, 1); |
---|
| 210 | %% Scale image points so that sum of variances of x and y = 2. |
---|
| 211 | scl = sqrt(2./var(:)); |
---|
| 212 | %% Sanity: varScl = var .* reshape(scl.^2, [1 nIm]); % Should be all 2's |
---|
| 213 | |
---|
| 214 | %% Scale so x and y variance is roughly 1, translate so image mean (x,y) is zero. |
---|
| 215 | Knum(1:2,3,:) = -reshape(mn, [2, 1, nIm]); |
---|
| 216 | Knum(1:2,:,:) = Knum(1:2,:,:).*repmat(reshape(scl, [1 1 nIm]), [2, 3,1]); |
---|
| 217 | for kIm = 1:nIm |
---|
| 218 | imPts(:,kIm, :) = reshape(Knum(:,:,kIm),3,3) * reshape(imPts(:,kIm, :),3,nPts); |
---|
| 219 | end |
---|
| 220 | %% Sanity check |
---|
| 221 | % sum(imPts(1:2,:,:),3)/nPts % Should be [0 0]' |
---|
| 222 | % sum(sum(imPts(1:2,:,:).^2,3)/nPts,1) % Should be 2. |
---|
| 223 | end |
---|
| 224 | |
---|
| 225 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 226 | %% Do Projective Reconstruction |
---|
| 227 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 228 | %%% Initial guess for projective depths |
---|
| 229 | z = ones( nIm, nPts); |
---|
| 230 | z = reshape(z, [1 nIm nPts]); |
---|
| 231 | |
---|
| 232 | %%% Do projective factorization iterations |
---|
| 233 | for its=1:50 |
---|
| 234 | |
---|
| 235 | %% Data matrix |
---|
| 236 | D = reshape(imPts .* repmat(z, [3, 1, 1]), 3 * nIm, nPts); |
---|
| 237 | W = sqrt(sum(D .* D, 1)).^-1; |
---|
| 238 | D = D .* repmat(W, 3*nIm, 1); |
---|
| 239 | |
---|
| 240 | %% Factor data matrix |
---|
| 241 | if size(D,2)<=size(D,1) |
---|
| 242 | [U S V] = svd(D,0); |
---|
| 243 | else |
---|
| 244 | [V S U] = svd(D',0); |
---|
| 245 | end |
---|
| 246 | S = diag(S); |
---|
| 247 | fprintf(2, 'it %d, sv(5) %e\n', its, S(5)); |
---|
| 248 | if its == 1 |
---|
| 249 | figure(1); clf; |
---|
| 250 | hand1 = plot(S, '-*g', 'MarkerSize', 14, 'LineWidth', 2); |
---|
| 251 | set(gca,'FontSize', 14); |
---|
| 252 | title('Data Matrix Singular Values'); |
---|
| 253 | xlabel('Singular Value Index'); |
---|
| 254 | ylabel('Sigular Value'); |
---|
| 255 | Sstart = S; |
---|
| 256 | else |
---|
| 257 | figure(1); clf; |
---|
| 258 | hand1=plot(Sstart, '-*g', 'MarkerSize', 14, 'LineWidth', 2); |
---|
| 259 | hold on; |
---|
| 260 | hand2=plot(S, '-*r', 'MarkerSize', 14, 'LineWidth', 2); |
---|
| 261 | set(gca,'FontSize', 14); |
---|
| 262 | title('Singular Values of Data Matrix'); |
---|
| 263 | xlabel('Singular Value Index'); |
---|
| 264 | ylabel('Singular Value'); |
---|
| 265 | legend([hand1 hand2], 'Iteration 1', sprintf('Iteration %d', its)); |
---|
| 266 | end |
---|
| 267 | pause(0.1); |
---|
| 268 | |
---|
| 269 | %% Extract estimates for camera matrices, Mfac, and projective points, Pfac |
---|
| 270 | Mfac = U(:,1:4); |
---|
| 271 | Sfac = S(1:4); |
---|
| 272 | Pfac = V(:,1:4)'; |
---|
| 273 | |
---|
| 274 | %% Given Mfac, Sfac and Pfac, re-estimate projective depths z. |
---|
| 275 | zEst = zeros(size(z)); |
---|
| 276 | for j=1:nPts |
---|
| 277 | C = imPts(:,:,j); |
---|
| 278 | Ckron = zeros(3*nIm, nIm); |
---|
| 279 | for k = 1:nIm |
---|
| 280 | ei = zeros(nIm,1); |
---|
| 281 | ei(k) = 1; |
---|
| 282 | Ckron(:,k) = kron(ei, C(:,k)); |
---|
| 283 | end |
---|
| 284 | |
---|
| 285 | %% Minimize || Ckron * z - Mfac * Pfac(:,j) ||^2 subject to ||Ckron * z|| = 1 |
---|
| 286 | Cz = Ckron * z(1, :, j)'; |
---|
| 287 | b = Mfac * diag(Sfac) * Pfac(:,j); |
---|
| 288 | scl = norm(Cz).^-1; |
---|
| 289 | f0 = sum((scl * Cz - b).^2); |
---|
| 290 | |
---|
| 291 | if f0 > tol %% Objective function is large enough, try to decrease it. |
---|
| 292 | |
---|
| 293 | %%Require || Ckron z || = 1 = || diag(sCk) * vCk' * z_k || |
---|
| 294 | %% Set y = diag(sCk) * vCk' z_k, then ||y||_2 = 1. |
---|
| 295 | [uCk sCk vCk] = svd(Ckron, 0); sCk=diag(sCk); |
---|
| 296 | if DEBUG |
---|
| 297 | if sCk(1)/sCk(end) > 1000 |
---|
| 298 | fprintf(2,' Cond number Ckron(%d): %e\n',j, sCk(1)/sCk(end)); |
---|
| 299 | end |
---|
| 300 | end |
---|
| 301 | scl = sCk.^-1; |
---|
| 302 | |
---|
| 303 | %% Minimize ||Pk * y - Mfac * diag(Sfac) * Pfac(:,j)|| with ||y||=1 |
---|
| 304 | %% where Pk is given by: |
---|
| 305 | Pk = Ckron * (vCk .* repmat(scl', nIm, 1)); |
---|
| 306 | |
---|
| 307 | %% Current guess is y0 = diag(sCk) * vCk' z_k. |
---|
| 308 | y0 = sCk.* (vCk' * z(1,:,j)'); |
---|
| 309 | y0 = y0/norm(y0); |
---|
| 310 | |
---|
| 311 | %% Simplify ||Pk * y - Mfac* diag(Sfac) * Pfac(:,j)|| to ||diag(sPk) * x - b ||, |
---|
| 312 | %% subject to ||x|| = 1, where Pk = uPk * diag(sPk) * vPk', and vPk' * y = x |
---|
| 313 | [uPk sPk vPk] = svd(Pk, 0); sPk=diag(sPk); |
---|
| 314 | if DEBUG |
---|
| 315 | if sPk(1)/sPk(end) > 1000 |
---|
| 316 | fprintf(2,' Cond number Pk(%d): %e\n',j, sPk(1)/sPk(end)); |
---|
| 317 | end |
---|
| 318 | end |
---|
| 319 | b = uPk'*(Mfac * diag(Sfac) * Pfac(:,j)); |
---|
| 320 | x0 = vPk' * y0; |
---|
| 321 | |
---|
| 322 | %% Minimize ||diag(sPk) * x - b|| with ||x||=1, initial guess x0. |
---|
| 323 | %% Compute the negative gradient direction |
---|
| 324 | delta = sPk .* (b - sPk .* x0); |
---|
| 325 | delta = delta - x0 * (x0' * delta); %% delta must be perp to x0. |
---|
| 326 | nDelta = norm(delta); |
---|
| 327 | delta = delta / max(nDelta, 1); |
---|
| 328 | |
---|
| 329 | %% Try a step in the negative gradient direction of length alpha1 |
---|
| 330 | alpha = 0; |
---|
| 331 | alpha1 = 1; |
---|
| 332 | cnt = 0; |
---|
| 333 | %% Shrink alpha1 until objective function decreases. |
---|
| 334 | while alpha1 * nDelta > 10e-6 |
---|
| 335 | x1 = x0 + alpha1 * delta; |
---|
| 336 | x1 = x1/norm(x1); |
---|
| 337 | f1 = sum((sPk .* x1 - b).^2); |
---|
| 338 | cnt = cnt+1; |
---|
| 339 | if f1 < f0 |
---|
| 340 | alpha = alpha1; |
---|
| 341 | break; |
---|
| 342 | end |
---|
| 343 | alpha1 = alpha1/2; |
---|
| 344 | end |
---|
| 345 | %% Here alpha is either 0, or the objective function f1 is smaller |
---|
| 346 | %% at this alpha and we can update the projective depths. |
---|
| 347 | |
---|
| 348 | %% Compute projective depths for this alpha |
---|
| 349 | x = x0 + alpha * delta; |
---|
| 350 | x = x/norm(x); |
---|
| 351 | y = vPk * x; |
---|
| 352 | zEst(1,:,j) = vCk * (scl .* y); |
---|
| 353 | |
---|
| 354 | else %% Objective function small enough already. Keep projective depths. |
---|
| 355 | zEst(1,:,j) = z(1,:,j); |
---|
| 356 | end |
---|
| 357 | |
---|
| 358 | end %% End of z-estimation loop |
---|
| 359 | |
---|
| 360 | z = zEst; |
---|
| 361 | |
---|
| 362 | end %% End of projective factorization loop. |
---|
| 363 | % Reshape projective depths from (1,nIm,nPts) to (nIm, nPts). |
---|
| 364 | z = reshape(z, nIm, nPts); |
---|
| 365 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 366 | %% End of Projective Reconstruction |
---|
| 367 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 368 | |
---|
| 369 | |
---|
| 370 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 371 | %%%% Check out projective reconstruction. |
---|
| 372 | %%%% Does it approximate the ground truth data up to a 3D Homography? |
---|
| 373 | %%%% The problem here is that we cannot expect to simply display projective |
---|
| 374 | %%%% reconstructions. We first need to get the homography roughly right. |
---|
| 375 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 376 | Pest = Pfac; |
---|
| 377 | Mest = Mfac * diag(Sfac(1:4)); |
---|
| 378 | MASSAGED_MEST = FALSE; |
---|
| 379 | |
---|
| 380 | %% Fit a homography from the estimated reconstruction, Pest, to the |
---|
| 381 | %% ground truth data P0. |
---|
| 382 | [Hmassaged Sa] = linEstH3D(P0, Pest); |
---|
| 383 | |
---|
| 384 | Pmassaged = Hmassaged * Pest; |
---|
| 385 | XYZmassaged = Pmassaged./repmat(Pmassaged(4,:),4,1); |
---|
| 386 | |
---|
| 387 | %%% Show 3D coords for transformed reconstruction. |
---|
| 388 | %%% NOTE: The homography has be chosen using the ground truth. |
---|
| 389 | %%% This just illustrates the consistency of the projective |
---|
| 390 | %%% reconstruction with the original data. |
---|
| 391 | figure(1); clf; |
---|
| 392 | for k = 1:3 |
---|
| 393 | subplot(1,3,k) |
---|
| 394 | hand1=plot(XYZmassaged(k,:), 'b', 'LineWidth', 2); |
---|
| 395 | set(gca,'FontSize', 14); |
---|
| 396 | hold on; |
---|
| 397 | hand2=plot(P0(k,:), 'r'); |
---|
| 398 | if k == 2 |
---|
| 399 | title('Recovered Projective Model (b), Ground Truth (r)'); |
---|
| 400 | end |
---|
| 401 | xlabel('Point index'); |
---|
| 402 | ylabel(sprintf('X(%d)\n',k)); |
---|
| 403 | end |
---|
| 404 | fprintf(2,'Press any key to continue...'); |
---|
| 405 | pause; |
---|
| 406 | fprintf(2,'ok\n'); |
---|
| 407 | |
---|
| 408 | %% Summarize errors. |
---|
| 409 | fprintf(2,'RMS Error: %f %f %f\n', sqrt(sum((P0(1:3,:) - XYZmassaged(1:3,:)).^2,2)/nPts)); |
---|
| 410 | mn0 = sum(P0(1:3,:),2)/nPts; |
---|
| 411 | fprintf(2,'Data Point RMS variation: %f %f %f\n', sqrt(sum((P0(1:3,:) - repmat(mn0,1,nPts)).^2,2)/nPts)); |
---|
| 412 | |
---|
| 413 | %%%%% Surface plot of transformed projective reconstruction. |
---|
| 414 | figure(3); clf; |
---|
| 415 | for k = 1:length(f) |
---|
| 416 | vf = XYZmassaged(:,f{k}); |
---|
| 417 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 418 | end |
---|
| 419 | set(gca,'YDir', 'reverse'); |
---|
| 420 | axis vis3d; axis square; axis equal; |
---|
| 421 | title('Projective Reconstruction (see NOTE)'); |
---|
| 422 | fprintf(2,'Rotate this figure.\n'); |
---|
| 423 | fprintf(2,'Press any key to continue...'); |
---|
| 424 | pause; |
---|
| 425 | fprintf(2,'ok\n'); |
---|
| 426 | |
---|
| 427 | %%%%% Surface plot of ground truth data |
---|
| 428 | figure(10); clf; |
---|
| 429 | for k = 1:length(f) |
---|
| 430 | vf = P0(:,f{k}); |
---|
| 431 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 432 | end |
---|
| 433 | set(gca,'YDir', 'reverse'); |
---|
| 434 | axis vis3d; axis square; axis equal; |
---|
| 435 | title('Ground Truth Data'); |
---|
| 436 | fprintf(2,'Rotate this figure.\n'); |
---|
| 437 | fprintf(2,'Press any key to continue...'); |
---|
| 438 | pause; |
---|
| 439 | fprintf(2,'ok\n'); |
---|
| 440 | |
---|
| 441 | %%%%% Show effect of various (non-affine) homographies |
---|
| 442 | if HOMOG_DEMO |
---|
| 443 | for k = 1:10 |
---|
| 444 | H = eye(4); |
---|
| 445 | H(4,1:3) = 0.02*(rand(1,3)-0.5); |
---|
| 446 | |
---|
| 447 | Ph = H * Pmassaged; |
---|
| 448 | |
---|
| 449 | %%%%% Surface plot of data |
---|
| 450 | figure(4); clf; |
---|
| 451 | for k = 1:length(f) |
---|
| 452 | vf = Ph(1:3,f{k})./repmat(Ph(4,f{k}),3,1); |
---|
| 453 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 454 | end |
---|
| 455 | set(gca, 'FontSize', 14); |
---|
| 456 | axis vis3d; axis square; axis equal; |
---|
| 457 | title('Equivalent Projective Reconstruction'); |
---|
| 458 | fprintf(2,'Press any key to continue...'); |
---|
| 459 | pause; fprintf(2,'ok\n'); |
---|
| 460 | end |
---|
| 461 | clear H Ph; |
---|
| 462 | end |
---|
| 463 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 464 | %% Finished checking out projective reconstruction. |
---|
| 465 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 466 | |
---|
| 467 | |
---|
| 468 | |
---|
| 469 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 470 | %% Metric Upgrade (and an attempt at self-calibration). |
---|
| 471 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 472 | if ~MASSAGED_MEST %% Only do the following permutation of the Mest array once. |
---|
| 473 | if UNDO_RESCALE |
---|
| 474 | %% Undo the image transformation used for numerical stability, Knum |
---|
| 475 | %% Not recommended. |
---|
| 476 | Mest = permute(reshape(Mest, [3, nIm, 4]), [1 3 2]); |
---|
| 477 | for kIm = 1:nIm |
---|
| 478 | Mest(:,:, kIm) = inv(reshape(Knum(:,:,kIm),3,3)) * reshape(Mest(:,:,kIm), 3, 4); |
---|
| 479 | Knum(:,:,kIm) = eye(3,3); |
---|
| 480 | end |
---|
| 481 | Mest = permute(Mest, [1 3 2]); |
---|
| 482 | Mest = reshape(Mest, [3*nIm, 4]); |
---|
| 483 | end |
---|
| 484 | Mest = reshape(Mest, [3, nIm, 4]); |
---|
| 485 | Mest = permute(Mest, [1 3 2]); |
---|
| 486 | MASSAGED_MEST = TRUE; |
---|
| 487 | end |
---|
| 488 | |
---|
| 489 | %%% Compute constraints on Qinf, the absolute dual quadric. |
---|
| 490 | %%% Know Mest(i,:,k) * Qinf * Mest(j,:,k)' = element i,j of K(:,:,k) * K(:,:,k)' |
---|
| 491 | %%% K(:,:,k) the camera to image coordinate 3 x 3 matrix Knum * diag([f(k), f(k),1]) |
---|
| 492 | Acum = []; |
---|
| 493 | bcum = []; |
---|
| 494 | for iIm = 1:nIm |
---|
| 495 | |
---|
| 496 | %% Compute constraints on Qinf from current image formation |
---|
| 497 | %% matrix Mest(:,:,iIm). |
---|
| 498 | A = zeros(6,10); |
---|
| 499 | b = zeros(6,1); |
---|
| 500 | nc = 0; |
---|
| 501 | for i=1:3 |
---|
| 502 | for j= i:3 |
---|
| 503 | nc = nc+1; |
---|
| 504 | q = reshape(Mest(i,:,iIm),4,1) * reshape(Mest(j,:, iIm), 1,4); |
---|
| 505 | q = q + q' - diag(diag(q)); |
---|
| 506 | A(nc,:) = [q(1,1:4) q(2,2:4) q(3,3:4) q(4,4)]; |
---|
| 507 | end |
---|
| 508 | end |
---|
| 509 | KK = reshape(Knum(:,:,iIm),3,3) * diag([fMat(iIm) fMat(iIm) 1]); |
---|
| 510 | KK = KK * KK'; |
---|
| 511 | %% For (i,j)=(3,3) currently 6th row in A and b |
---|
| 512 | b = [KK(1, 1:3) KK(2, 2:3) KK(3,3)]'; |
---|
| 513 | |
---|
| 514 | if SELF_CALIBRATE |
---|
| 515 | %% For (i,j)=(1,1) and (2,2), 1st and 4th elements in A and b, get |
---|
| 516 | %% combined constraint. This cancels the dependence on the focal length. |
---|
| 517 | A(4,:) = A(4,:) - A(1,:); |
---|
| 518 | b(4) = b(4) - b(1); |
---|
| 519 | %% Delete 1st row of both A and b to avoid extra constraint on (1,1) element. |
---|
| 520 | A = A(2:end, :); |
---|
| 521 | b = b(2:end); |
---|
| 522 | end |
---|
| 523 | |
---|
| 524 | %% Concatenate constraints on Qinf across all images. |
---|
| 525 | if size(Acum,1)>0 |
---|
| 526 | Acum = [Acum; A]; |
---|
| 527 | bcum = [bcum; b]; |
---|
| 528 | else |
---|
| 529 | Acum = A; |
---|
| 530 | bcum = b; |
---|
| 531 | end |
---|
| 532 | |
---|
| 533 | end |
---|
| 534 | |
---|
| 535 | %%% Solve A q = b for q and hence for Qinf. |
---|
| 536 | [uA sA vA] = svd(Acum,0); sA = diag(sA); |
---|
| 537 | % log10(sA(1:10)+eps)' |
---|
| 538 | %% Should be rank 10 |
---|
| 539 | if sA(10)/sA(1) < sqrt(eps) |
---|
| 540 | %% Oh, oh. Trouble!!!! |
---|
| 541 | fprintf(2,'Warning: System for Qinf nearly singular.\n'); |
---|
| 542 | fprintf(2,'Press any key to continue...'); |
---|
| 543 | pause; |
---|
| 544 | %% Solve A q = b, ignore singular element |
---|
| 545 | q = vA * (diag([sA(1:9).^-1 ; 0]) * (uA' * bcum)); |
---|
| 546 | q0 = q; |
---|
| 547 | fprintf(2,'ok\n'); |
---|
| 548 | else |
---|
| 549 | %% Solve A q = b |
---|
| 550 | q = vA * (diag([sA.^-1]) * (uA' * bcum)); |
---|
| 551 | q0 = q; |
---|
| 552 | end |
---|
| 553 | if DEBUG |
---|
| 554 | fprintf(2, 'Residuals in solving for Qinf...'); |
---|
| 555 | imStats(Acum * q - bcum); |
---|
| 556 | end |
---|
| 557 | |
---|
| 558 | %% Search along the least constrained direction for Q for a rank 3 Q matrix. |
---|
| 559 | CHATTY = TRUE; |
---|
| 560 | foundMetric = FALSE; |
---|
| 561 | foundZero = FALSE; |
---|
| 562 | minEQ = nan; |
---|
| 563 | if CHATTY |
---|
| 564 | fprintf(2,'Eigenvalues of estimated Qinf at each value of itNull:\n'); |
---|
| 565 | end |
---|
| 566 | for itNull = -50:1:50 |
---|
| 567 | q = q0 + itNull * vA(:,10); |
---|
| 568 | |
---|
| 569 | %% Unpack q into symmetric 4x4 matrix |
---|
| 570 | Qinf = [q(1) q(2) q(3) q(4); |
---|
| 571 | q(2) q(5) q(6) q(7); |
---|
| 572 | q(3) q(6) q(8) q(9); |
---|
| 573 | q(4) q(7) q(9) q(10)]; |
---|
| 574 | |
---|
| 575 | %% Check out eigenvalues of estimated Qinf |
---|
| 576 | [uQ sQ vQ] = svd(Qinf); sQ = diag(sQ); |
---|
| 577 | sgn = diag(uQ' * vQ)'; |
---|
| 578 | if CHATTY |
---|
| 579 | fprintf(2,'%f: ', itNull); |
---|
| 580 | fprintf(2,' %e',sQ(:).*sgn(:)); |
---|
| 581 | fprintf(2,'\n'); |
---|
| 582 | end |
---|
| 583 | |
---|
| 584 | if all(sgn(1:3)>0) |
---|
| 585 | if isnan(minEQ) |
---|
| 586 | minEQ = sgn(4); |
---|
| 587 | elseif minEQ * sgn(4) < 0 & ~foundZero |
---|
| 588 | if CHATTY |
---|
| 589 | fprintf(2, 'Detected zero crossing, itNull = %f\n',itNull); |
---|
| 590 | end |
---|
| 591 | foundMetric = TRUE; |
---|
| 592 | minEQ = sgn(3); |
---|
| 593 | itNull0 = itNull; |
---|
| 594 | foundZero = TRUE; |
---|
| 595 | end |
---|
| 596 | end |
---|
| 597 | end |
---|
| 598 | |
---|
| 599 | if ~foundMetric |
---|
| 600 | fprintf(2, 'Failed to do self-calibration\n'); |
---|
| 601 | else |
---|
| 602 | % Use itNull0 |
---|
| 603 | q = q0 + itNull0 * vA(:,10); |
---|
| 604 | |
---|
| 605 | %% Unpack q into symmetric 4x4 matrix |
---|
| 606 | Qinf = [q(1) q(2) q(3) q(4); |
---|
| 607 | q(2) q(5) q(6) q(7); |
---|
| 608 | q(3) q(6) q(8) q(9); |
---|
| 609 | q(4) q(7) q(9) q(10)]; |
---|
| 610 | |
---|
| 611 | %% Check out eigenvalues of estimated Qinf |
---|
| 612 | [uQ sQ vQ] = svd(Qinf); sQ = diag(sQ); |
---|
| 613 | sgn = diag(uQ' * vQ)'; |
---|
| 614 | %% Find closest non-negative definite rank 3 matrix to estimated Qinf |
---|
| 615 | kSv = 4; %% Set smallest sv to set to zero |
---|
| 616 | sQinf = sQ; |
---|
| 617 | sQinf(kSv) = 0; |
---|
| 618 | Qinf = uQ * diag(sQinf) * uQ'; %% Reconstructed rank 3 Qinf. |
---|
| 619 | |
---|
| 620 | %%% Estimate homography for metric upgrade from this Qinf. |
---|
| 621 | sH = zeros(4,1); |
---|
| 622 | uH = zeros(4,4); |
---|
| 623 | n = 1; |
---|
| 624 | for k=1:4 |
---|
| 625 | if k ~= kSv |
---|
| 626 | sH(n) = sQ(k); |
---|
| 627 | uH(:,n) = uQ(:,k); |
---|
| 628 | n= n+1; |
---|
| 629 | end |
---|
| 630 | end |
---|
| 631 | uH(:,4) = uQ(:,kSv); |
---|
| 632 | sH(4) = 1; |
---|
| 633 | sH = sH .^ -0.5; |
---|
| 634 | H = (uH * diag(sH))'; |
---|
| 635 | Hinv = uH' * diag(sH.^-1); |
---|
| 636 | |
---|
| 637 | if DEBUG |
---|
| 638 | fprintf(2, 'Sanity check; Transformed Qinf should be apprx diag([1 1 1 0])\n'); |
---|
| 639 | H * Qinf * H' |
---|
| 640 | end |
---|
| 641 | |
---|
| 642 | %%%% Given this estimated homography H, apply it the the projective |
---|
| 643 | %reconstruction. |
---|
| 644 | Hm2 = H; |
---|
| 645 | Pm2 = H * Pest; |
---|
| 646 | Pm2 = Pm2 ./ repmat(Pm2(4,:),4,1); |
---|
| 647 | |
---|
| 648 | %%%%% Surface plot of metric reconstruction. |
---|
| 649 | figure(3); clf; |
---|
| 650 | for k = 1:length(f) |
---|
| 651 | vf = Pm2(:,f{k}); |
---|
| 652 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 653 | end |
---|
| 654 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 655 | axis vis3d; axis square; axis equal; |
---|
| 656 | title('Euclidean Reconstruction'); |
---|
| 657 | fprintf(2,' Rotate this reconstruction.\n'); |
---|
| 658 | fprintf(2,... |
---|
| 659 | ' Note: The colour map is NOT supposed to be the same, just the shape.\n'); |
---|
| 660 | |
---|
| 661 | pause; |
---|
| 662 | |
---|
| 663 | |
---|
| 664 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 665 | %%%% Check out metric reconstruction. |
---|
| 666 | %%%% Does it approximate the ground truth data up to a 3D Homography? |
---|
| 667 | %%%% The problem here is that we cannot expect to simply display projective |
---|
| 668 | %%%% reconstructions. We first need to get the homography roughly right. |
---|
| 669 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 670 | |
---|
| 671 | %% Fit a rigid transform from the estimated reconstruction, Pest, to the |
---|
| 672 | %% ground truth data P0. |
---|
| 673 | |
---|
| 674 | [Haff] = linEstAff3D(P0, Pm2); |
---|
| 675 | |
---|
| 676 | %% FInd a nearby rigid transform; |
---|
| 677 | R0 = Haff(1:3,1:3); |
---|
| 678 | [Ur Sr Vr] = svd(R0); Sr = diag(Sr); |
---|
| 679 | Sr = sum(Sr)/3 * eye(3); |
---|
| 680 | R = Ur * Sr * Vr'; |
---|
| 681 | Haff(1:3,:) = [R, R * inv(R0)* Haff(1:3,4)]; |
---|
| 682 | |
---|
| 683 | Pmassaged = Haff* Pm2; |
---|
| 684 | XYZmassaged = Pmassaged./repmat(Pmassaged(4,:),4,1); |
---|
| 685 | |
---|
| 686 | %%% Show 3D coords for transformed reconstruction. |
---|
| 687 | %%% NOTE: The homography has be chosen using the ground truth. |
---|
| 688 | %%% This just illustrates the consistency of the projective |
---|
| 689 | %%% reconstruction with the original data. |
---|
| 690 | figure(1); clf; |
---|
| 691 | for k = 1:3 |
---|
| 692 | subplot(1,3,k) |
---|
| 693 | hand1=plot(XYZmassaged(k,:), 'b', 'LineWidth', 2); |
---|
| 694 | set(gca,'FontSize', 14); |
---|
| 695 | hold on; |
---|
| 696 | hand2=plot(P0(k,:), 'r'); |
---|
| 697 | if k == 2 |
---|
| 698 | title('Recovered Euclidean Model (b), Ground Truth (r)'); |
---|
| 699 | end |
---|
| 700 | xlabel('Point index'); |
---|
| 701 | ylabel(sprintf('X(%d)\n',k)); |
---|
| 702 | end |
---|
| 703 | fprintf(2,'Press any key to continue...'); |
---|
| 704 | pause; |
---|
| 705 | fprintf(2,'ok\n'); |
---|
| 706 | |
---|
| 707 | %% Summarize errors. |
---|
| 708 | fprintf(2,'RMS Error: %f %f %f\n', sqrt(sum((P0(1:3,:) - XYZmassaged(1:3,:)).^2,2)/nPts)); |
---|
| 709 | mn0 = sum(P0(1:3,:),2)/nPts; |
---|
| 710 | fprintf(2,'Data Point RMS variation: %f %f %f\n', sqrt(sum((P0(1:3,:) - repmat(mn0,1,nPts)).^2,2)/nPts)); |
---|
| 711 | |
---|
| 712 | %%%% Surface plot of metric reconstruction. |
---|
| 713 | figure(3); clf; |
---|
| 714 | for k = 1:length(f) |
---|
| 715 | vf = XYZmassaged(:,f{k}); |
---|
| 716 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 717 | end |
---|
| 718 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 719 | axis vis3d; axis square; axis equal; |
---|
| 720 | title('Euclidean Reconstruction'); |
---|
| 721 | fprintf(2,' Rotate this reconstruction.\n'); |
---|
| 722 | end |
---|
| 723 | |
---|
| 724 | |
---|
| 725 | %% Summary: For nIm perspective images, nIm >=3, we have demonstrated: |
---|
| 726 | %% - Euclidean scene reconstruction from 3 or more orthographic images. |
---|
| 727 | %% - The reconstruction of the viewing directions. |
---|
| 728 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 729 | %%% End: projectiveMassageDino.m |
---|
| 730 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 731 | |
---|