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