[37] | 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 2 | %% Script file: orthoMassageDino.m |
---|
| 3 | %% |
---|
| 4 | %% Demonstrate 3D affine and Euclidean reconstructions from corresponding points in |
---|
| 5 | %% orthographic projection. |
---|
| 6 | %% |
---|
| 7 | %% ADJ, Nov. 03. |
---|
| 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 9 | |
---|
| 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 11 | %%% Initialization |
---|
| 12 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 13 | close all; |
---|
| 14 | clear |
---|
| 15 | global TRUE FALSE; |
---|
| 16 | global matlabVisRoot; |
---|
| 17 | |
---|
| 18 | TRUE = 1==1; |
---|
| 19 | FALSE = ~TRUE; |
---|
| 20 | |
---|
| 21 | if length(matlabVisRoot) == 0 |
---|
| 22 | dir = pwd; |
---|
| 23 | cd '/h/51/jepson/pub/matlab'; %% CHANGE THIS |
---|
| 24 | %cd 'C:/work/Matlab' |
---|
| 25 | startup; |
---|
| 26 | cd(dir); |
---|
| 27 | end |
---|
| 28 | reconRoot = [matlabVisRoot '/utvisToolbox/tutorials']; |
---|
| 29 | |
---|
| 30 | addpath([reconRoot '/3dRecon/utils']); |
---|
| 31 | addpath([reconRoot '/3dRecon/orthographic']); |
---|
| 32 | |
---|
| 33 | % Random number generator seed: |
---|
| 34 | seed = round(sum(1000*clock)); |
---|
| 35 | rand('state', seed); |
---|
| 36 | seed0 = seed; |
---|
| 37 | % We also need to start randn. Use a seedn generated from seed: |
---|
| 38 | seedn = round(rand(1,1) * 1.0e+6); |
---|
| 39 | randn('state', seedn); |
---|
| 40 | |
---|
| 41 | %% Parameters |
---|
| 42 | DEBUG = FALSE; |
---|
| 43 | sigma = 2.0; %% Std Dev of noise (in pixels) in point locations |
---|
| 44 | nIm = 3; %% Number of data images to use (try 2-10). |
---|
| 45 | %% nIm = 2 is enough for Affine reconstruction, but not |
---|
| 46 | %% for Euclidean reconstruction. |
---|
| 47 | NeckerReversal = FALSE; %% Flip reconstruction in depth (the sign is ambiguous). |
---|
| 48 | |
---|
| 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 50 | %%% Read Dino data set |
---|
| 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 52 | [v f] = getHalfDino; |
---|
| 53 | |
---|
| 54 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 55 | %%% Place Dino in a fixed 3D pose in world coordinate frame and display |
---|
| 56 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 57 | %%% Set canonical 3D pose |
---|
| 58 | R0 = [1 0 0; 0 0 1; 0 -1 0]; %% Rotate and reflect dino (concave away). |
---|
| 59 | mn0 = [0 0 0]'; %% Place Dino's mean on Z axis. |
---|
| 60 | P0 = R0 * v' + repmat(mn0, 1, size(v,1)); |
---|
| 61 | if size(P0,1)==3 |
---|
| 62 | P0 = [P0; ones(1,size(P0,2))]; |
---|
| 63 | end |
---|
| 64 | fprintf(2,'Depth statistics...'); |
---|
| 65 | imStats(P0(3,:)); |
---|
| 66 | nPts = size(P0,2); |
---|
| 67 | |
---|
| 68 | %%%%% Surface plot of data. |
---|
| 69 | figure(3); clf; |
---|
| 70 | for k = 1:length(f) |
---|
| 71 | vf = P0(:, f{k}); |
---|
| 72 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 73 | end |
---|
| 74 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 75 | axis vis3d; axis square; axis equal; |
---|
| 76 | title('Dino Model'); |
---|
| 77 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 78 | fprintf(2,'Rotate this figure.\n'); |
---|
| 79 | fprintf(2,'Press any key to continue...'); |
---|
| 80 | pause; fprintf(2,'ok\n'); |
---|
| 81 | |
---|
| 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 83 | %% Create some camera locations |
---|
| 84 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 85 | %% Columns of dMat give X,Y,Z locations of camera in world coordinate frame. |
---|
| 86 | dMat = [50 -50 0 50 -40 -50 10 20 2.5 10 |
---|
| 87 | 0 0 -100 -20 10 0 -50 -20 10 -10 |
---|
| 88 | -150 -150 -145 -160 -145 155 -150 -160 -140 -145]; |
---|
| 89 | %% Focal lengths for each of these 10 cameras. |
---|
| 90 | fMat = [1.25 1.25 0.75 1.25 1.25 1.20 0.8 1.0 1.20 1.0]; |
---|
| 91 | %% Camera rotation will be chosen to fixate Dino's center. |
---|
| 92 | |
---|
| 93 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 94 | %% Grab nIm Scaled - Orthographic images (nIm <= length(fMat) == 10) |
---|
| 95 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 96 | imPts = []; |
---|
| 97 | zPts = []; |
---|
| 98 | M_GT = []; |
---|
| 99 | for k = 1:nIm |
---|
| 100 | |
---|
| 101 | d = dMat(:,k); % Camera center |
---|
| 102 | f1 = fMat(k); % Focal length |
---|
| 103 | |
---|
| 104 | %% Choose rotation to fixate mn0, say |
---|
| 105 | %% That is solve: R * (mn0 - d) = [0 0 1]'; |
---|
| 106 | R = eye(3); |
---|
| 107 | R(:,3) = (mn0 - d)/norm(mn0 - d); |
---|
| 108 | R(:,2) = R(:,2) - (R(:,3)' * R(:,2)) * R(:,3); |
---|
| 109 | R(:,2) = R(:,2)/norm(R(:,2)); |
---|
| 110 | R(:,1) = R(:,1) - R(:,2:3) * (R(:,2:3)' * R(:,1)); |
---|
| 111 | R(:,1) = R(:,1)/norm(R(:,1)); |
---|
| 112 | R = R'; |
---|
| 113 | if DEBUG |
---|
| 114 | fprintf(2,'Sanity check:\n Should be [0 0 Center_Depth]: %f %f %f\n', ... |
---|
| 115 | (R * (mn0 - d))); |
---|
| 116 | end |
---|
| 117 | |
---|
| 118 | %% Build camera matrix K and image formation matrix M. |
---|
| 119 | K = diag([f1, f1, 0]); %% Scaled-Orthographic |
---|
| 120 | M = K * [R -R*d]; |
---|
| 121 | if size(M_GT,1)>0 |
---|
| 122 | M_GT = [M_GT; M(1:2, 1:3)]; |
---|
| 123 | else |
---|
| 124 | M_GT = M(1:2, 1:3); |
---|
| 125 | end |
---|
| 126 | |
---|
| 127 | %% Compute the orthographic image locations |
---|
| 128 | p = M * P0; |
---|
| 129 | |
---|
| 130 | %% Add imaging noise. |
---|
| 131 | p(1:2,:) = p(1:2,:) + sigma * randn(2,nPts); |
---|
| 132 | |
---|
| 133 | %% Concatenate image points over multiple frames. |
---|
| 134 | if size(imPts,1)>0 |
---|
| 135 | imPts = cat(3, imPts, p(1:2,:)); |
---|
| 136 | else |
---|
| 137 | imPts = p(1:2,:); |
---|
| 138 | end |
---|
| 139 | |
---|
| 140 | %% Show current noisy image |
---|
| 141 | figure(10); clf; |
---|
| 142 | imStats(p(1,:)); |
---|
| 143 | imStats(p(2,:)); |
---|
| 144 | h = plot(p(1,:), p(2,:), '.b'); |
---|
| 145 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 146 | axis([-200 200 -200 200]); |
---|
| 147 | title(sprintf('Image %d',k)); |
---|
| 148 | fprintf(2,'Press any key to continue...'); |
---|
| 149 | pause; fprintf(2,'ok\n'); |
---|
| 150 | |
---|
| 151 | end %% Forming images. |
---|
| 152 | |
---|
| 153 | %% Reorder the matrices imPts in the form nIm by nPts |
---|
| 154 | if size(imPts,2) == nPts & size(imPts,3) == nIm |
---|
| 155 | imPts = permute(imPts,[1, 3, 2]); |
---|
| 156 | end |
---|
| 157 | imPtsSave = imPts; %% Save the image points in case we mess up... |
---|
| 158 | |
---|
| 159 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 160 | %% END of data generation |
---|
| 161 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 162 | |
---|
| 163 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 164 | %% Do Orthographic Factorization: Recover Affine Shape |
---|
| 165 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 166 | imPts = imPtsSave; |
---|
| 167 | |
---|
| 168 | %% Do factorization |
---|
| 169 | D = reshape(imPts, 2 * nIm, nPts); |
---|
| 170 | if size(D,2)<=size(D,1) |
---|
| 171 | [U S V] = svd(D,0); S = diag(S); |
---|
| 172 | else |
---|
| 173 | [V S U] = svd(D',0); S = diag(S); |
---|
| 174 | end |
---|
| 175 | |
---|
| 176 | %% Print size of 4th singular value relative to first. Is exactly zero without |
---|
| 177 | %% noise and round-off errors. |
---|
| 178 | fprintf(2, 'sv(4)/sv(1) %e\n', S(4)/S(1)); |
---|
| 179 | |
---|
| 180 | %% Plot singular values. Data matrix should be essentially rank 3, if |
---|
| 181 | %% imaging noise is small enough. |
---|
| 182 | figure(1); clf; plot(S, '-*', 'MarkerSize', 14, 'LineWidth', 2); |
---|
| 183 | set(gca, 'FontSize', 14); |
---|
| 184 | title('Singular Values of Data Matrix'); |
---|
| 185 | xlabel('Singular Value Index'); |
---|
| 186 | ylabel('Singlur Value'); |
---|
| 187 | pause(0.1); |
---|
| 188 | |
---|
| 189 | %% Extract estimates for affine shape. |
---|
| 190 | Mest = U(:,1:3) * diag(S(1:3)); |
---|
| 191 | Pest = V(:,1:3)'; |
---|
| 192 | |
---|
| 193 | %% Show estimate for affine shape. |
---|
| 194 | figure(1); clf; |
---|
| 195 | showWire(Pest', f); |
---|
| 196 | set(gca, 'FontSize', 14); |
---|
| 197 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 198 | title('Affine Estimation of Dino'); |
---|
| 199 | pause(0.1); |
---|
| 200 | |
---|
| 201 | %%%%% Surface plot of data. |
---|
| 202 | figure(3); clf; |
---|
| 203 | for k = 1:length(f) |
---|
| 204 | vf = Pest(:, f{k}); |
---|
| 205 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 206 | end |
---|
| 207 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 208 | axis vis3d; axis square; axis equal; |
---|
| 209 | title('Affine Estimation of Dino'); |
---|
| 210 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 211 | fprintf(2,'Rotate this figure.\n'); |
---|
| 212 | fprintf(2,'Press any key to continue...'); |
---|
| 213 | pause; fprintf(2,'ok\n'); |
---|
| 214 | |
---|
| 215 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 216 | %% Try Euclidean Reconstruction. |
---|
| 217 | %% For nIm == 2 we will get the bas relief ambiguity (rotation versus depth). |
---|
| 218 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 219 | |
---|
| 220 | %% Square pixels: |
---|
| 221 | %% Seek a matrix Q such that the rows of MestQ = Mest *Q for image k have the same |
---|
| 222 | %% length. That is norm(MestQ(2*k-1,:)) = norm(MestQ(2*k, :)) for k = 1..nIm. |
---|
| 223 | %% We also require these rows to be orthogonal, i.e. |
---|
| 224 | %% MestQ(2*k-1,:) * MestQ(2*k, :) = 0, for k = 1..nIm. |
---|
| 225 | %% These constraints ensure the pixels are square in each image. |
---|
| 226 | %% Overall scale factor: |
---|
| 227 | %% Finally to resolve the overall scale (which is ambiguous), we will |
---|
| 228 | %% set the norm of the rows of MestQ for the first image to 1. That is, |
---|
| 229 | %% norm(MestQ(1,:)) = norm(MestQ(2,:)) = 1. |
---|
| 230 | |
---|
| 231 | %% These constraints are encoded in terms of the values of particular |
---|
| 232 | %% rows and columns in the matrix: |
---|
| 233 | %% (Mest * Q) (Mest * Q)') = Mest * Q * Q' * Mest' |
---|
| 234 | %% We Set QQT = Q * Q'. It turns out we need to solve a linear system |
---|
| 235 | %% for the elements of QQT. Note QQT is a symmetric 3x3 matrix, so we |
---|
| 236 | %% only need to recover 6 coefficients. These will be stored in the |
---|
| 237 | %% q2, where: |
---|
| 238 | %% QQT = [q2(1) q2(2) q2(3); q2(2) q2(4) q2(5); q2(3) q2(5) q2(6)] |
---|
| 239 | %% We build the linear system for q2 next, in the form: C * q2 = b; |
---|
| 240 | |
---|
| 241 | C = zeros(2*(nIm-1)+3, 6); |
---|
| 242 | b = zeros(2*(nIm-1)+3,1); |
---|
| 243 | kC = 0; % The constraint counter. |
---|
| 244 | for kIm = 1:nIm |
---|
| 245 | kM = 2*(kIm-1); |
---|
| 246 | %% Get rows of Mest for image kIm. |
---|
| 247 | Mk = Mest(kM+(1:2),:); |
---|
| 248 | %% Build the constraints on the length of the rows of Mk. |
---|
| 249 | if kIm == 1 |
---|
| 250 | %% For the first image, require the length of both rows is 1. |
---|
| 251 | %% This sets the overall scale factor. |
---|
| 252 | for r = 1:2 |
---|
| 253 | outer = Mk(r,:)' * Mk(r,:); |
---|
| 254 | outer = outer + outer' - diag(diag(outer)); |
---|
| 255 | C(kC+r,:) = [outer(1,1:3) outer(2,2:3) outer(3,3)]; |
---|
| 256 | b(kC+r) = 1; |
---|
| 257 | end |
---|
| 258 | kC = kC+2; |
---|
| 259 | else %% For images 2 through nIm, require the lengths of both rows of |
---|
| 260 | %% Mk are equal. This allows the scale of each image to be different. |
---|
| 261 | for r = 1:2 |
---|
| 262 | outer = Mk(r,:)' * Mk(r,:); |
---|
| 263 | outer = outer + outer' - diag(diag(outer)); |
---|
| 264 | if r==1 |
---|
| 265 | C(kC+1,:) = [outer(1,1:3) outer(2,2:3) outer(3,3)]; |
---|
| 266 | else |
---|
| 267 | C(kC+1,:) = C(kC+1,:) - [outer(1,1:3) outer(2,2:3) outer(3,3)]; |
---|
| 268 | end |
---|
| 269 | end |
---|
| 270 | b(kC+1) = 0; |
---|
| 271 | kC = kC+1; |
---|
| 272 | end |
---|
| 273 | %% Make sure the two rows of Mk are orthogonal. |
---|
| 274 | outer = Mk(1,:)' * Mk(2,:); |
---|
| 275 | outer = outer + outer' - diag(diag(outer)); |
---|
| 276 | C(kC+1,:) = [outer(1,1:3) outer(2,2:3) outer(3,3)]; |
---|
| 277 | b(kC+1) = 0; |
---|
| 278 | kC = kC+1; |
---|
| 279 | end |
---|
| 280 | |
---|
| 281 | |
---|
| 282 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 283 | %% Solve for Q. |
---|
| 284 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 285 | if nIm>2 |
---|
| 286 | |
---|
| 287 | %% If the camera locations are not (nearly) coplanar, we should be able to solve uniquely |
---|
| 288 | %% for Q. |
---|
| 289 | [Uc Sc Vc] = svd(C,0); Sc = diag(Sc); |
---|
| 290 | log10(Sc) %% Is C singular? It will be if the cameras are coplanar |
---|
| 291 | %% (and there is no noise). |
---|
| 292 | tmp = Uc' * b; |
---|
| 293 | tmp = tmp./Sc; |
---|
| 294 | q2 = Vc * tmp; |
---|
| 295 | log10(abs(C*q2 - b)) %% Did we solve the linear system? |
---|
| 296 | |
---|
| 297 | %% Build the symmetric matrix QQT def= Q * Q' from q2. |
---|
| 298 | QQT = [q2(1:3)'; q2(2) q2(4:5)'; q2(3) q2(5) q2(6)]; |
---|
| 299 | |
---|
| 300 | %% Factor QQT into Q and Q', by first finding the SVD. |
---|
| 301 | [Uq Sq Vq] = svd(QQT,0); Sq = diag(Sq) |
---|
| 302 | sgn = diag(Vq' * Uq) %% These MUST all be positive, otherwise we get a |
---|
| 303 | %% complex-valued factor. |
---|
| 304 | |
---|
| 305 | %% Panic if the signs don't work out. |
---|
| 306 | if ~all(sgn>0) |
---|
| 307 | error('Panic, QQT not postive definite'); |
---|
| 308 | else |
---|
| 309 | %% Get the factors Q and Q' from the SVD of QQT. |
---|
| 310 | Q = Uq * diag([(Sq).^0.5]) * Vq'; |
---|
| 311 | if NeckerReversal %% Depth reversal |
---|
| 312 | Q = -Q; %% Note the sign of Q is ambiguous. Changing the sign |
---|
| 313 | end %% of Q effectively reverses the reconstruction in depth. |
---|
| 314 | MestQ = Mest * Q; |
---|
| 315 | sqrt(sum((MestQ).^2,2)) %% This should be constant, in pairs. The |
---|
| 316 | %% constants should be the scale factors in fMat. |
---|
| 317 | PestQ = inv(Q) * Pest; |
---|
| 318 | |
---|
| 319 | %% Show the Euclidean reconstruction (shape is known up to a 3D |
---|
| 320 | %% similarity transform, i.e. up to an unknown global rotation, translation, |
---|
| 321 | %% and scale change. |
---|
| 322 | figure(1); clf; |
---|
| 323 | showWire(PestQ', f); |
---|
| 324 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 325 | title('Euclidean Reconstruction'); |
---|
| 326 | |
---|
| 327 | fprintf(2,'Press any key to continue...'); |
---|
| 328 | pause; fprintf(2,'ok\n'); |
---|
| 329 | end |
---|
| 330 | |
---|
| 331 | elseif nIm==2 |
---|
| 332 | |
---|
| 333 | %% We cannot solve for Q, we have only 5 constraints for 6 unknowns in QQT. |
---|
| 334 | %% This is the bas relief ambiguity in which the overall depth |
---|
| 335 | %% variation of the object is linked to an unkown rotation. |
---|
| 336 | |
---|
| 337 | %% Since C is only 5 x 6 we only have 5 singular values. |
---|
| 338 | [Uc Sc Vc] = svd(C); Sc = diag(Sc); |
---|
| 339 | log10(Sc) %% The 5 singular values are nonzero...unless the two images are |
---|
| 340 | %% have the same projection direction. |
---|
| 341 | |
---|
| 342 | %% Let's solve C*q2 = b in the least squares sense. |
---|
| 343 | tmp = Uc' * b; |
---|
| 344 | tmp = tmp(1:5)./Sc(1:5); |
---|
| 345 | q2part = Vc(:, 1:5) * tmp; %% Least squares solution. |
---|
| 346 | %% Since C is 5 x 6, we have C * (q2part + alpha * q2null) = b is also |
---|
| 347 | %% a least squares solution for any real number alpha, where q2null is |
---|
| 348 | %% a right null vector for C, i.e. C * q2null = 0. |
---|
| 349 | q2null = Vc(:,6); |
---|
| 350 | |
---|
| 351 | %% How well have we solved the system for q2? |
---|
| 352 | log10(abs(C*q2part - b)) |
---|
| 353 | |
---|
| 354 | %% Find a range of values for alpha for which QQT has real factors Q |
---|
| 355 | %% and Q'. |
---|
| 356 | |
---|
| 357 | %% Try alpha=0. |
---|
| 358 | alpha = 0; |
---|
| 359 | q2 = q2part+alpha*q2null; |
---|
| 360 | QQT = [q2(1:3)'; q2(2) q2(4:5)'; q2(3) q2(5) q2(6)]; |
---|
| 361 | [Uq Sq Vq] = svd(QQT,0); Sq = diag(Sq); |
---|
| 362 | sgn = diag(Vq' * Uq); %% We have real factors iff these are all non-negative. |
---|
| 363 | sgnOkZero = all(sgn>0); |
---|
| 364 | if sgnOkZero |
---|
| 365 | %% We can factor the case when alpha=0. Let's do it. |
---|
| 366 | Q = Uq * diag([(Sq).^0.5]) * Vq'; |
---|
| 367 | if NeckerReversal |
---|
| 368 | Q = -Q; |
---|
| 369 | end |
---|
| 370 | PestQ = inv(Q) * Pest; |
---|
| 371 | %% Keep track of the aspect ratio of the reconstruction. |
---|
| 372 | [Vp Sp Up] = svd(PestQ', 0); Sp = diag(Sp); |
---|
| 373 | zAspect = Sp(3)/Sp(1); %% This is actually the inverse aspect ratio, |
---|
| 374 | %to avoid divide by zeros. |
---|
| 375 | end |
---|
| 376 | |
---|
| 377 | %% Check positive and negative values of alpha for ranges in |
---|
| 378 | %% which q2 = q2part+alpha*q2null provides a matrix QQT with real factors. |
---|
| 379 | pAlpha = []; |
---|
| 380 | nAlpha = []; |
---|
| 381 | nAspect = []; |
---|
| 382 | pAspect = []; |
---|
| 383 | for la10 = -10:0.1:10 |
---|
| 384 | alpha = 10.0^la10; %% Use equal spacing in log10(alpha) to check a large range of values |
---|
| 385 | |
---|
| 386 | %% Check out factorization of corresponding QQT for this value of alpha. |
---|
| 387 | q2 = q2part+alpha*q2null; |
---|
| 388 | QQT = [q2(1:3)'; q2(2) q2(4:5)'; q2(3) q2(5) q2(6)]; |
---|
| 389 | [Uq Sq Vq] = svd(QQT,0); Sq = diag(Sq); |
---|
| 390 | sgn = diag(Vq' * Uq); |
---|
| 391 | if all(sgn>0) %% Factorization of QQT will be ok. |
---|
| 392 | %% Remember the value of alpha. |
---|
| 393 | pAlpha = [pAlpha alpha]; |
---|
| 394 | %% Do the factorization (because we want to compute the aspect |
---|
| 395 | %% of the reconstruction below). |
---|
| 396 | Q = Uq * diag([(Sq).^0.5]) * Vq'; |
---|
| 397 | if NeckerReversal |
---|
| 398 | Q = -Q; |
---|
| 399 | end |
---|
| 400 | %% Make note of the (inverse) aspect ratio of the reconstruction. |
---|
| 401 | PestQ = inv(Q) * Pest; |
---|
| 402 | [Vp Sp Up] = svd(PestQ', 0); Sp = diag(Sp); |
---|
| 403 | pAspect = [pAspect Sp(3)/Sp(1)]; |
---|
| 404 | end |
---|
| 405 | |
---|
| 406 | %% Do the same for alpha with the opposite sign (-alpha). |
---|
| 407 | alpha = -alpha; |
---|
| 408 | q2 = q2part+alpha*q2null; |
---|
| 409 | QQT = [q2(1:3)'; q2(2) q2(4:5)'; q2(3) q2(5) q2(6)]; |
---|
| 410 | [Uq Sq Vq] = svd(QQT,0); Sq = diag(Sq); |
---|
| 411 | sgn = diag(Vq' * Uq); |
---|
| 412 | if all(sgn>0) %% Factorization of QQT will be ok. |
---|
| 413 | %% Remember the value of alpha. |
---|
| 414 | nAlpha = [alpha nAlpha]; |
---|
| 415 | %% Do the factorization (because we want to compute the aspect |
---|
| 416 | %% of the reconstruction below). |
---|
| 417 | Q = Uq * diag([(Sq).^0.5]) * Vq'; |
---|
| 418 | if NeckerReversal |
---|
| 419 | Q = -Q; |
---|
| 420 | end |
---|
| 421 | %% Make note of the (inverse) aspect ratio of the reconstruction. |
---|
| 422 | PestQ = inv(Q) * Pest; |
---|
| 423 | [Vp Sp Up] = svd(PestQ', 0); Sp = diag(Sp); |
---|
| 424 | nAspect = [ Sp(3)/Sp(1) nAspect]; |
---|
| 425 | end |
---|
| 426 | end %% Finished checking out a range of positive and negative alpha's. |
---|
| 427 | |
---|
| 428 | %% Put together a range of alpha's for which QQT has real factors. |
---|
| 429 | if sgnOkZero |
---|
| 430 | alpha = [0 pAlpha]; |
---|
| 431 | aspect = [zAspect pAspect]; |
---|
| 432 | if size(nAlpha,1)>0 |
---|
| 433 | alpha = [nAlpha alpha]; |
---|
| 434 | aspect = [nAspect aspect]; |
---|
| 435 | end |
---|
| 436 | elseif size(nAlpha,1)==0 |
---|
| 437 | alpha = pAlpha; |
---|
| 438 | aspect = pAspect; |
---|
| 439 | elseif size(pAlpha,1) == 0 |
---|
| 440 | alpha = nAlpha; |
---|
| 441 | aspect = nAspect; |
---|
| 442 | else |
---|
| 443 | fprintf(2, 'Found two seqments of alpha\n'); |
---|
| 444 | alpha = [nAlpha pAlpha]; |
---|
| 445 | aspect = [nAspect pAspect]; |
---|
| 446 | end |
---|
| 447 | |
---|
| 448 | %% Let's look at some of the reconstructions for different alpha's |
---|
| 449 | if length(alpha) > 0 |
---|
| 450 | |
---|
| 451 | %% Select just a few values of alpha to display. |
---|
| 452 | k = ceil(length(alpha)/5); |
---|
| 453 | kal = [1:k:length(alpha)]; |
---|
| 454 | if (length(alpha)-kal(end))/k > 0.5 |
---|
| 455 | kal = [kal length(alpha)]; |
---|
| 456 | else |
---|
| 457 | kal(end) = length(alpha); |
---|
| 458 | end |
---|
| 459 | |
---|
| 460 | %% Display the affine reconstructions for the selected alpha's. |
---|
| 461 | for alp = alpha(kal) |
---|
| 462 | %% Do the factorization. |
---|
| 463 | q2 = q2part+alp*q2null; |
---|
| 464 | QQT = [q2(1:3)'; q2(2) q2(4:5)'; q2(3) q2(5) q2(6)]; |
---|
| 465 | [Uq Sq Vq] = svd(QQT,0); Sq = diag(Sq); |
---|
| 466 | sgn = diag(Vq' * Uq); %% Should be all positive (we don't check). |
---|
| 467 | Q = Uq * diag([(Sq).^0.5]) * Vq'; |
---|
| 468 | if NeckerReversal |
---|
| 469 | Q = -Q; |
---|
| 470 | end |
---|
| 471 | %% Find the camera and shape matrices. |
---|
| 472 | MestQ = Mest * Q; |
---|
| 473 | PestQ = inv(Q) * Pest; |
---|
| 474 | |
---|
| 475 | %% Show the affine reconstruction. |
---|
| 476 | %% Note the shape is elongated or flattened, depending on the value |
---|
| 477 | %% of alpha. This is the bas relief ambiguity, where the amount of |
---|
| 478 | %% rotation in depth is confounded with the amount of depth variation. |
---|
| 479 | %%%%% Surface plot of data. |
---|
| 480 | figure(1); clf; |
---|
| 481 | for k = 1:length(f) |
---|
| 482 | vf = PestQ(:, f{k}); |
---|
| 483 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 484 | end |
---|
| 485 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 486 | axis vis3d; axis square; axis equal; |
---|
| 487 | title(sprintf('Affine Reconstruction (alpha = %9.2e)', alp)); |
---|
| 488 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 489 | fprintf(2,'Rotate this figure.\n'); |
---|
| 490 | |
---|
| 491 | if FALSE |
---|
| 492 | figure(1); clf; |
---|
| 493 | showWire(PestQ', f); |
---|
| 494 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 495 | title(sprintf('Affine Reconstruction (alpha = %9.2e)', alp)); |
---|
| 496 | end |
---|
| 497 | |
---|
| 498 | fprintf(2,'Press any key to continue...'); |
---|
| 499 | pause; fprintf(2,'ok\n'); |
---|
| 500 | end |
---|
| 501 | end |
---|
| 502 | end %% Done solving for Q. |
---|
| 503 | |
---|
| 504 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 505 | %% Show surface plot of reconstruction. |
---|
| 506 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 507 | |
---|
| 508 | if nIm ==2 |
---|
| 509 | %% For nIm == 2, choose the minimum aspect ratio depth estimate. This |
---|
| 510 | %% avoids reconstructions that are flat, or stretched in 1D due to the |
---|
| 511 | %% bas relief ambiguity we just saw. |
---|
| 512 | [asp k] = max(aspect); |
---|
| 513 | alp = alpha(k); |
---|
| 514 | |
---|
| 515 | %% Factor QQT, solve for Q |
---|
| 516 | q2 = q2part+alp*q2null; |
---|
| 517 | QQT = [q2(1:3)'; q2(2) q2(4:5)'; q2(3) q2(5) q2(6)]; |
---|
| 518 | [Uq Sq Vq] = svd(QQT,0); Sq = diag(Sq) |
---|
| 519 | sgn = diag(Vq' * Uq) |
---|
| 520 | Q = Uq * diag([(Sq).^0.5]) * Vq'; |
---|
| 521 | if NeckerReversal |
---|
| 522 | Q = -Q; |
---|
| 523 | end |
---|
| 524 | %% Generate camera and shape matrices. |
---|
| 525 | MestQ = Mest * Q; |
---|
| 526 | sum((MestQ).^2,2) |
---|
| 527 | PestQ = inv(Q) * Pest; |
---|
| 528 | |
---|
| 529 | %% Show wire-frame reconstruction. |
---|
| 530 | figure(1); clf; |
---|
| 531 | showWire(PestQ', f); |
---|
| 532 | title('Selected Affine Reconstruction'); |
---|
| 533 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 534 | fprintf(2,'Press any key to continue...'); |
---|
| 535 | pause; fprintf(2,'ok\n'); |
---|
| 536 | |
---|
| 537 | %%%%% Surface plot of selected affine reconstruction. |
---|
| 538 | figure(3); clf; |
---|
| 539 | for k = 1:length(f) |
---|
| 540 | vf = PestQ(:,f{k}); |
---|
| 541 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 542 | end |
---|
| 543 | set(gca,'YDir', 'reverse'); |
---|
| 544 | axis vis3d; axis square; axis equal; |
---|
| 545 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 546 | title('Selected Affine Reconstruction'); |
---|
| 547 | fprintf(2,'Rotate this figure.\n'); |
---|
| 548 | |
---|
| 549 | fprintf(2,'Press any key to continue...'); |
---|
| 550 | pause; fprintf(2,'ok\n'); |
---|
| 551 | |
---|
| 552 | elseif nIm > 2 %% Show Euclidean reconstruction. |
---|
| 553 | |
---|
| 554 | %% We have already solved for Q, MestQ, and PestQ above. |
---|
| 555 | %% Q is unique up to the sign (assuming the projection directions are |
---|
| 556 | %% not coplanar). Changing the sign of Q flips the reconstruction in depth. |
---|
| 557 | %% This depth reversal is called the Necker ambiguity. |
---|
| 558 | |
---|
| 559 | %%%%% Surface plot of Euclidean reconstruction. |
---|
| 560 | figure(3); clf; |
---|
| 561 | for k = 1:length(f) |
---|
| 562 | vf = PestQ(:,f{k}); |
---|
| 563 | patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)'); |
---|
| 564 | end |
---|
| 565 | set(gca,'YDir', 'reverse', 'FontSize', 14); |
---|
| 566 | axis vis3d; axis square; axis equal; |
---|
| 567 | xlabel('X'); ylabel('Y'), zlabel('Z'); |
---|
| 568 | title('Euclidean Reconstruction'); |
---|
| 569 | fprintf(2,'Rotate this figure.\n'); |
---|
| 570 | |
---|
| 571 | fprintf(2,'Press any key to continue...'); |
---|
| 572 | pause; fprintf(2,'ok\n'); |
---|
| 573 | |
---|
| 574 | end |
---|
| 575 | |
---|
| 576 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 577 | %%%% Check out the focal length estimate (scale factor). |
---|
| 578 | %%%% Does it approximate the ground truth data? |
---|
| 579 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 580 | fMest = sqrt(sum(reshape(sum((MestQ).^2,2), 2, nIm), 1)/2); |
---|
| 581 | %% Match over-all scale factor for comparison purposes. |
---|
| 582 | %% We couldn't do this without some absolute scale factor. In that |
---|
| 583 | %% case we would still recover the relative scales of all the images. |
---|
| 584 | sclFac = (fMest * fMat(1:nIm)')/(fMest * fMest') |
---|
| 585 | figure(4); clf; |
---|
| 586 | plot(sclFac*fMest, 'b*-'); hold on; |
---|
| 587 | plot(fMat(1:nIm), 'ro-'); |
---|
| 588 | plot(100*abs(sclFac*fMest-fMat(1:nIm))./fMat(1:nIm), 'k'); |
---|
| 589 | set(gca,'FontSize', 14); |
---|
| 590 | title('Scale estimation (Est(b) True(r) %Error(k))'); |
---|
| 591 | xlabel('Image Number'); |
---|
| 592 | ylabel('Scale or % Error'); |
---|
| 593 | |
---|
| 594 | |
---|
| 595 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 596 | %%%% Check out affine reconstruction. |
---|
| 597 | %%%% Does it approximate the ground truth data up to a 3D affine transform? |
---|
| 598 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 599 | |
---|
| 600 | %% Fit a 3d affine transform for the estimated reconstruction, PestQ, from the |
---|
| 601 | %% ground truth data P0. That is, solve (A b) * P0 = PestQ |
---|
| 602 | |
---|
| 603 | Ab = ( inv(P0 * P0') * (P0 * PestQ'))'; |
---|
| 604 | Paff = Ab*P0; |
---|
| 605 | |
---|
| 606 | %% Show the error (in units of pixel size in X,Y and ALSO Z) |
---|
| 607 | errPnts = sqrt(sum((Paff - PestQ).^2 ,1)); |
---|
| 608 | figure(1); clf; plot(errPnts); |
---|
| 609 | title(sprintf('Affine Error in Recovered Points')); |
---|
| 610 | xlabel('Point index'); |
---|
| 611 | ylabel('Euclidean Distance (pixels)'); |
---|
| 612 | pause(0.1); |
---|
| 613 | |
---|
| 614 | %% Show individual coordinates of the recovered shape, X,Y and Z. |
---|
| 615 | %% Compare these with the ground truth. |
---|
| 616 | figure(2); clf; |
---|
| 617 | coord = 'XYZ'; |
---|
| 618 | for k = 1:3 |
---|
| 619 | subplot(1,3,k); |
---|
| 620 | plot(PestQ(k,:),'b', 'MarkerSize', 14, 'LineWidth', 2); |
---|
| 621 | hold on; |
---|
| 622 | plot(Paff(k,:),'r', 'MarkerSize', 14, 'LineWidth', 2); |
---|
| 623 | set(gca, 'FontSize', 12); |
---|
| 624 | title(sprintf('Recovered Coord (b), True(r)')); |
---|
| 625 | xlabel('Point index'); |
---|
| 626 | ylabel(['3D coord ' coord(k)]); |
---|
| 627 | end |
---|
| 628 | pause(0.1); |
---|
| 629 | |
---|
| 630 | %% The above error estimates compare the affine reconstruction to |
---|
| 631 | %% the ground truth. What about the Euclidean reconstruction? |
---|
| 632 | %% Well, to check the Euclidean reconstruction we should find the |
---|
| 633 | %% a similarity transform of P0 (i.e. (sR d) * P0, s>0 a scale |
---|
| 634 | %% factor, R a rotation matrix, and d a translation vector) for which |
---|
| 635 | %% (sR d) P0 provides the best approximation of PestQ. |
---|
| 636 | %% This is a nonlinear optimization problem (because of the rotation |
---|
| 637 | %% R). We do not do this here. |
---|
| 638 | %% |
---|
| 639 | %% Instead, we simply check how close to a rigid transform is the best affine |
---|
| 640 | %% transform from P0 to PestQ. |
---|
| 641 | |
---|
| 642 | %% Find the svd of the A portion of the affine transform given |
---|
| 643 | %% by Ab = (A b) (a 3 x 4 matrix) |
---|
| 644 | [Ua Sa Va] = svd(Ab(:, 1:3)); Sa = diag(Sa); |
---|
| 645 | Sa %% If we have a similarity transform, then the singular values will |
---|
| 646 | %% all be equal. |
---|
| 647 | Sa(3)/Sa(1) %% This should be near 1 if we have a decent Euclidean |
---|
| 648 | %% reconstruction... i.e A approx= sR for some rotation |
---|
| 649 | %% matrix R. It will probably not be near 1 when nIm == 2 |
---|
| 650 | %% because of the bas relief ambiguity. |
---|
| 651 | |
---|
| 652 | %% A guess at the rotation/reflection matrix is obtained by setting all |
---|
| 653 | %% the singular values of A to be equal, providing |
---|
| 654 | R_est = Ua * Va'; |
---|
| 655 | |
---|
| 656 | %% Finally, we need the determinant of R_est to be positive. Otherwise we |
---|
| 657 | %% have reconstructed the depth reversed scene (which is just the Necker |
---|
| 658 | %% ambiguity again). |
---|
| 659 | det(R_est) %% Will be +-1 since the determinant of the product of two |
---|
| 660 | %% matrices is the product of the determinants |
---|
| 661 | %% (i.e. det(M N) = det(M)det(N)) and the determinant of an |
---|
| 662 | %% orthogonal matrix is +-1 (i.e. det(Ua) = det(Va) = +-1 ). |
---|
| 663 | |
---|
| 664 | if det(R_est) < 0 |
---|
| 665 | NeckerReversal = ~NeckerReversal; |
---|
| 666 | %% In order to reverse the depth, paste in from comment |
---|
| 667 | %% "Solve for Q" above, all the way to the end here. |
---|
| 668 | %% (Don't simply rerun the whole file, since the code will pick new |
---|
| 669 | %% noise values, and this may also cause the estimate to reverse in depth.) |
---|
| 670 | end |
---|
| 671 | |
---|
| 672 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 673 | %%%% Check out the recovered projection directions. |
---|
| 674 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 675 | errProjDir = zeros(1, nIm); |
---|
| 676 | for kIm = 1:nIm |
---|
| 677 | |
---|
| 678 | %% Estimate the projection direction for the k-th image. |
---|
| 679 | %% Unpack the k-th camera matrix |
---|
| 680 | Mk = MestQ((2*kIm-1):(2*kIm),:); |
---|
| 681 | %% Find the right null vector of Mk. |
---|
| 682 | [Vm Sm Um] = svd(Mk'); Sm = diag(Sm); |
---|
| 683 | %% Vm(:,3) is the recovered projection direction of the k-th camera |
---|
| 684 | projDirEst = Vm(:,3); |
---|
| 685 | |
---|
| 686 | %% Do the same for the ground truth imaging matrices |
---|
| 687 | %% Unpack the k-th camera matrix |
---|
| 688 | Mk = M_GT((2*kIm-1):(2*kIm),:); |
---|
| 689 | %% Find the right null vector of Mk. |
---|
| 690 | [Vm Sm Um] = svd(Mk'); Sm = diag(Sm); |
---|
| 691 | %% Vm(:,3) is the recovered projection direction of the k-th camera |
---|
| 692 | projDir_GT = Vm(:,3); |
---|
| 693 | |
---|
| 694 | %% The estimated and true directions should be related by the rotation |
---|
| 695 | %% in the Euclidean transformation. |
---|
| 696 | errProjDir(kIm) = asin(norm(cross(projDirEst, R_est * projDir_GT)))*180/pi; |
---|
| 697 | end |
---|
| 698 | |
---|
| 699 | figure(5); clf; |
---|
| 700 | plot(errProjDir, '*-b', 'MarkerSize', 14, 'LineWidth', 2); |
---|
| 701 | set(gca,'FontSize', 14); |
---|
| 702 | title('Error in estimated projection directions'); |
---|
| 703 | xlabel('Image Number'); |
---|
| 704 | ylabel('Error in Degress'); |
---|
| 705 | ax = axis; |
---|
| 706 | axis([ax(1:2) 0 ax(4)]); |
---|
| 707 | |
---|
| 708 | %% Summary: For nIm orthographic images, nIm >=2, we have demonstrated: |
---|
| 709 | %% - Euclidean scene reconstruction from 3 or more orthographic images. |
---|
| 710 | %% - Affine scene reconstruction from nIm = 2 orthographic images, and the |
---|
| 711 | %% associated bas relief ambiguity. |
---|
| 712 | %% - The Necker (depth reversal) ambiguity. |
---|
| 713 | %% - The reconstruction of the viewing directions. |
---|
| 714 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 715 | %%% End: orthoMassageDino.m |
---|
| 716 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|