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 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|