source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/3dRecon/projective/projectiveMassageDino.m @ 37

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Script file: projectiveMassageDino.m 
%%
%% Demonstrate 3D affine and Euclidean reconstructions from corresponding
%% points in perspective projection.
%%
%% The perspective factorization algorithm is due to Mahamud, Hebert,
%% Omori, and Ponce, CVPR, 2001.  I replaced the generalized eigenvalue
%% problem with a few optimization steps along the gradient. 
%%
%% Note, the self calibration follows the algorithm of Polleyfeys, Koch, and van Gool,
%% IJCV, 1998, except we search for a nearby rank 3 Q matrix.
%%
%% TTD:
%%  - The self-calibration method is quite sensitive to rescaling.
%%    Some thoughts on how to do this are warranted.
%%  - We should be solving a CONSTRAINED (and normalized) least squares
%%    problem for Q, where the constraint is that Q has rank 3.  See literature?
%%  - Turning the self-calibration off is now a problem.  Scaling issue?
%%
%% ADJ, Nov. 03. 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Initialization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear
global TRUE FALSE;
global matlabVisRoot;

TRUE = 1==1;
FALSE = ~TRUE;

if length(matlabVisRoot) == 0
  dir = pwd;
  cd '/h/51/jepson/pub/matlab';  % CHANGE THIS
  %cd 'C:/work/Matlab'
  startup;
  cd(dir);
end
reconRoot = [matlabVisRoot '/utvisToolbox/tutorials'];

addpath([reconRoot '/3dRecon/utils']);
addpath([reconRoot '/3dRecon/projective']);

% Random number generator seed:
seed = round(sum(1000*clock));
rand('state', seed);
seed0 = seed;
% We also need to start randn. Use a seedn generated from seed:
seedn = round(rand(1,1) * 1.0e+6);
randn('state', seedn);

%% Control Flags
DEBUG = FALSE;         %% TRUE => Extra output
HOMOG_DEMO = TRUE;     %% Show effect of simple homography on 3D data points
NUM_RESCALE = TRUE;    %% Use Hartley's rescaling of image data for stability
SELF_CALIBRATE = TRUE;  %% Use only self-calibration constraints in metric
                        %% estimation. (FALSE => broken, for some reason)
UNDO_RESCALE = FALSE & NUM_RESCALE; %% TRUE => Use original scaling of image data 
                                    %% during metric upgrade. (Not good.)

%% Parameters
sigma = 1.0;     %% Std Dev of noise (in pixels) in point locations
tol = 1.0e-9;    %% Convergence tolerance for z-estimation in projective fit
nIm = 10;        %% Number of data images to use (try 3-10).
                 %% nIm = 2 is enough for projective reconstruction, but not
                 %% for metric reconstruction algorithm.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Read  Dino data set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[v f] = getHalfDino;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Place Dino in a fixed 3D pose in world coordinate frame and display
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Set canonical 3D pose
R0 = [1 0 0; 0 0 1; 0 -1 0];        %% Rotate and reflect dino (concave away).
mn0 = [0 0 0]';                   %% Place Dino's mean on Z axis.
P0 = R0 * v' + repmat(mn0, 1, size(v,1));
if size(P0,1)==3
  P0 = [P0; ones(1,size(P0,2))];
end
fprintf(2,'Depth statistics...');
imStats(P0(3,:));
nPts = size(P0,2);

%%%%% Surface plot of data.
figure(3); clf;
for k = 1:length(f)
  vf = P0(:, f{k});
  patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)');
end
set(gca,'YDir', 'reverse', 'FontSize', 14);
axis vis3d; axis square; axis equal;
title('Dino Model');
xlabel('X'); ylabel('Y'), zlabel('Z');
fprintf(2,'Rotate this figure.\n');
fprintf(2,'Press any key to continue...');
pause; fprintf(2,'ok\n');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create some camera locations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Columns of dMat give X,Y,Z locations of camera in world coordinate frame.
dMat = [ 0    0    0   50   -70  -50    10   80     2.5    10
         0   50 -100  -20    10    0   -50  -20    10     -10
      -150 -140 -145 -160  -145  -155 -150 -160  -140    -145];
%% Focal lengths for each of these 10 cameras.
fMat = [100 125 150 125 125 120 120 120 120 120]';
%% Camera rotation will be chosen to fixate Dino's center.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Grab nIm Scaled - Orthographic images  (nIm <= length(fMat) == 10)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
imPts = [];
zPts = [];
M_GT = [];
for k = 1:nIm
  
  d = dMat(:,k);  % Camera center
  f1 = fMat(k);   % Focal length
  
  %% Choose rotation to fixate mn0, say
  %% That is solve:  R * (mn0 - d) = [0 0 1]';
  R = eye(3);
  R(:,3) = (mn0 - d)/norm(mn0 - d);
  R(:,2) = R(:,2) - (R(:,3)' * R(:,2)) * R(:,3);
  R(:,2) = R(:,2)/norm(R(:,2));
  R(:,1) = R(:,1) - R(:,2:3) * (R(:,2:3)' * R(:,1));
  R(:,1) = R(:,1)/norm(R(:,1));
  R = R';
  if DEBUG
    fprintf(2,'Sanity check:\n   Should be [0 0 Center_Depth]: %f %f %f\n', ...
            (R * (mn0 - d)));
  end

  %% Build camera matrix K and image formation matrix M.
  K = diag([f1, f1, 1]);
  M = K * [R -R*d];
  if size(M_GT,1)>0
    M_GT = [M_GT; M];
  else
    M_GT = M;
  end

  %% Compute the projected image locations
  p = M * P0;
  
  %% Concatenate ground truth depth values over multiple frames.
  if size(zPts,1)>0
    zPts = cat(3, zPts, p(3,:));
  else
    zPts = p(3,:);
  end

  %% Concatenate noisy image points over multiple frames.
  p = p ./ repmat(p(3,:),3,1);
  p(1:2,:) = p(1:2,:) + sigma * randn(2,nPts);  %% Add noise
  if size(imPts,1)>0
    imPts = cat(3, imPts, p(1:2,:));
  else
    imPts = p(1:2,:);
  end

  %% Show image
  figure(10); clf;
  h = plot(p(1,:), p(2,:), '.b');
  set(gca,'YDir', 'reverse', 'FontSize', 14);
  axis([-150 150 -150 150]);
  title(sprintf('Image %d',k));
  fprintf(2,'Press any key to continue...');
  pause;
  fprintf(2,'ok\n');
  
end  %% Forming images.


%% Reorder the matrices zPts and imPts in the form nIm by nPts
if size(zPts,1) == 1 & size(zPts,2) == nPts & size(zPts,3) == nIm
  zPts = permute(zPts, [3 2 1]);
  zPts = reshape(zPts, nIm, nPts);
end
if size(imPts,1) == 2
  imPts = cat(1, imPts, ones(1, nPts,nIm));
end
if size(imPts,2) == nPts & size(imPts,3) == nIm
  imPts = permute(imPts,[1, 3, 2]);
end
%% Set homogeneous 3rd coord in imPts
imPts(3,:,:) = 1;
imPtsSave = imPts;  %% Save the image points in case we mess up...

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% END of data generation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Rescale image data (attempting to obtain more numerical stability).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Knum = repmat(eye(3), [1 1 nIm]);
if NUM_RESCALE
  %%% Rescale for numerical stability
  mn = sum(imPts(1:2,:,:),3)/nPts;
  %mns = reshape(mn, [2 nIm 1]);
  var = sum(sum((imPts(1:2,:,:)-repmat(mn, [1  1 nPts])).^2,3)/nPts, 1);
  %% Scale image points so that sum of variances of x and y = 2.
  scl = sqrt(2./var(:));
  %% Sanity: varScl =  var .* reshape(scl.^2, [1 nIm]); % Should be all 2's
  
  %% Scale so x and y variance is roughly 1, translate so image mean (x,y) is zero.
  Knum(1:2,3,:) = -reshape(mn, [2, 1, nIm]);
  Knum(1:2,:,:) = Knum(1:2,:,:).*repmat(reshape(scl, [1 1 nIm]), [2, 3,1]);
  for kIm = 1:nIm
    imPts(:,kIm, :) = reshape(Knum(:,:,kIm),3,3) * reshape(imPts(:,kIm, :),3,nPts);
  end
  %% Sanity check
  % sum(imPts(1:2,:,:),3)/nPts  % Should be [0 0]'
  % sum(sum(imPts(1:2,:,:).^2,3)/nPts,1) % Should be 2.
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Do Projective Reconstruction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Initial guess for projective depths
z = ones( nIm, nPts);
z = reshape(z, [1 nIm nPts]);

%%% Do projective factorization iterations
for its=1:50 
  
  %% Data matrix
  D = reshape(imPts .* repmat(z, [3, 1, 1]), 3 * nIm, nPts);
  W = sqrt(sum(D .* D, 1)).^-1;
  D = D .* repmat(W, 3*nIm, 1);

  %% Factor data matrix
  if size(D,2)<=size(D,1)
    [U S V] = svd(D,0); 
  else
    [V S U] = svd(D',0);
  end
  S = diag(S);
  fprintf(2, 'it %d, sv(5) %e\n', its, S(5));
  if its == 1
    figure(1); clf; 
    hand1 = plot(S, '-*g', 'MarkerSize', 14, 'LineWidth', 2);
    set(gca,'FontSize', 14);
    title('Data Matrix Singular Values'); 
    xlabel('Singular Value Index');
    ylabel('Sigular Value');
    Sstart = S;
  else
    figure(1); clf; 
    hand1=plot(Sstart, '-*g', 'MarkerSize', 14, 'LineWidth', 2);
    hold on;
    hand2=plot(S, '-*r', 'MarkerSize', 14, 'LineWidth', 2);
    set(gca,'FontSize', 14);
    title('Singular Values of Data Matrix');
    xlabel('Singular Value Index');
    ylabel('Singular Value');
    legend([hand1 hand2], 'Iteration 1', sprintf('Iteration %d', its));
  end
  pause(0.1);

  %% Extract estimates for camera matrices, Mfac, and projective points, Pfac
  Mfac = U(:,1:4);
  Sfac = S(1:4);
  Pfac = V(:,1:4)';
  
  %% Given Mfac, Sfac and Pfac, re-estimate projective depths z.
  zEst = zeros(size(z));
  for j=1:nPts
    C = imPts(:,:,j);
    Ckron = zeros(3*nIm, nIm);
    for k = 1:nIm
      ei = zeros(nIm,1);
      ei(k) = 1;
      Ckron(:,k) = kron(ei, C(:,k));
    end

    %% Minimize || Ckron * z - Mfac * Pfac(:,j) ||^2 subject to ||Ckron * z|| = 1
    Cz = Ckron * z(1, :, j)';
    b = Mfac * diag(Sfac) * Pfac(:,j);
    scl = norm(Cz).^-1;
    f0 = sum((scl * Cz - b).^2);
    
    if f0 > tol %% Objective function is large enough, try to decrease it. 
      
      %%Require || Ckron z || = 1 = || diag(sCk) * vCk' * z_k ||
      %% Set y = diag(sCk) * vCk' z_k, then ||y||_2 = 1.
      [uCk sCk vCk] = svd(Ckron, 0); sCk=diag(sCk);
      if DEBUG
        if sCk(1)/sCk(end) > 1000
          fprintf(2,' Cond number Ckron(%d): %e\n',j, sCk(1)/sCk(end));
        end
      end
      scl = sCk.^-1;
      
      %% Minimize ||Pk * y - Mfac * diag(Sfac) * Pfac(:,j)|| with ||y||=1
      %% where Pk is given by:
      Pk = Ckron * (vCk .* repmat(scl', nIm, 1));
      
      %% Current guess is y0 = diag(sCk) * vCk' z_k.
      y0 = sCk.* (vCk' * z(1,:,j)');
      y0 = y0/norm(y0);
      
      %% Simplify ||Pk * y - Mfac* diag(Sfac) * Pfac(:,j)|| to ||diag(sPk) * x - b ||,
      %% subject to ||x|| = 1, where Pk = uPk * diag(sPk) * vPk', and vPk' * y = x
      [uPk sPk vPk] = svd(Pk, 0); sPk=diag(sPk);
      if DEBUG
        if sPk(1)/sPk(end) > 1000
          fprintf(2,' Cond number Pk(%d): %e\n',j, sPk(1)/sPk(end));
        end
      end
      b = uPk'*(Mfac * diag(Sfac) * Pfac(:,j));
      x0 = vPk' * y0;
      
      %% Minimize ||diag(sPk) * x - b|| with ||x||=1, initial guess x0.
      %% Compute the negative gradient direction
      delta = sPk .* (b - sPk .* x0);
      delta = delta - x0 * (x0' * delta);  %% delta must be perp to x0.
      nDelta = norm(delta);
      delta = delta / max(nDelta, 1);
      
      %% Try a step in the negative gradient direction of length alpha1
      alpha = 0;
      alpha1 = 1;
      cnt = 0;
      %% Shrink alpha1 until objective function decreases.
      while alpha1 * nDelta > 10e-6
        x1 = x0 + alpha1 * delta;
        x1 = x1/norm(x1);
        f1 = sum((sPk .* x1 - b).^2);
        cnt = cnt+1;
        if f1 < f0 
          alpha = alpha1;
          break;
        end
        alpha1 = alpha1/2;
      end
      %% Here alpha is either 0, or the objective function f1 is smaller
      %% at this alpha and we can update the projective depths.
      
      %% Compute projective depths for this alpha
      x = x0 + alpha * delta;
      x = x/norm(x);
      y = vPk * x;
      zEst(1,:,j) = vCk * (scl .* y);
      
    else  %% Objective function small enough already.  Keep projective depths.
      zEst(1,:,j) = z(1,:,j);
    end
    
  end %% End of z-estimation loop

  z = zEst;
  
end %% End of projective factorization loop.
% Reshape projective depths from (1,nIm,nPts) to  (nIm, nPts).
z = reshape(z, nIm, nPts);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% End of Projective Reconstruction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Check out projective reconstruction.  
%%%% Does it approximate the ground truth data up to a 3D Homography?
%%%% The problem here is that we cannot expect to simply display projective
%%%% reconstructions.  We first need to get the homography roughly right.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Pest = Pfac;
Mest = Mfac * diag(Sfac(1:4));
MASSAGED_MEST = FALSE;

%% Fit a homography from the estimated reconstruction, Pest, to the
%% ground truth data P0.
[Hmassaged Sa] = linEstH3D(P0, Pest);

Pmassaged = Hmassaged * Pest;
XYZmassaged = Pmassaged./repmat(Pmassaged(4,:),4,1);

%%% Show 3D coords for transformed reconstruction.
%%% NOTE: The homography has be chosen using the ground truth.
%%% This just illustrates the consistency of the projective
%%% reconstruction with the original data.
figure(1); clf;
for k = 1:3
  subplot(1,3,k)
  hand1=plot(XYZmassaged(k,:), 'b', 'LineWidth', 2);
  set(gca,'FontSize', 14);
  hold on;
  hand2=plot(P0(k,:), 'r');
  if k == 2
    title('Recovered Projective Model (b), Ground Truth (r)');
  end
  xlabel('Point index');
  ylabel(sprintf('X(%d)\n',k));
end
fprintf(2,'Press any key to continue...');
pause;
fprintf(2,'ok\n');

%% Summarize errors.
fprintf(2,'RMS Error: %f %f %f\n', sqrt(sum((P0(1:3,:) - XYZmassaged(1:3,:)).^2,2)/nPts));
mn0 = sum(P0(1:3,:),2)/nPts;
fprintf(2,'Data Point RMS variation: %f %f %f\n', sqrt(sum((P0(1:3,:) - repmat(mn0,1,nPts)).^2,2)/nPts));

%%%%% Surface plot of transformed projective reconstruction.
figure(3); clf;
for k = 1:length(f)
  vf = XYZmassaged(:,f{k});
  patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)');
end
set(gca,'YDir', 'reverse');
axis vis3d; axis square; axis equal;
title('Projective Reconstruction (see NOTE)');
fprintf(2,'Rotate this figure.\n');
fprintf(2,'Press any key to continue...');
pause;
fprintf(2,'ok\n');

%%%%% Surface plot of ground truth data
figure(10); clf;
for k = 1:length(f)
  vf = P0(:,f{k});
  patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)');
end
set(gca,'YDir', 'reverse');
axis vis3d; axis square; axis equal;
title('Ground Truth Data');
fprintf(2,'Rotate this figure.\n');
fprintf(2,'Press any key to continue...');
pause;
fprintf(2,'ok\n');

%%%%% Show effect of various (non-affine) homographies
if HOMOG_DEMO
  for k = 1:10
    H = eye(4);
    H(4,1:3) = 0.02*(rand(1,3)-0.5);

    Ph = H * Pmassaged;

    %%%%% Surface plot of data
    figure(4); clf;
    for k = 1:length(f)
      vf = Ph(1:3,f{k})./repmat(Ph(4,f{k}),3,1);
      patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)');
    end
    set(gca, 'FontSize', 14);
    axis vis3d; axis square; axis equal;
    title('Equivalent Projective Reconstruction');
    fprintf(2,'Press any key to continue...');
    pause;   fprintf(2,'ok\n');
  end
  clear H Ph;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Finished checking out projective reconstruction.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Metric Upgrade (and an attempt at self-calibration).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~MASSAGED_MEST  %% Only do the following permutation of the Mest array once.
  if UNDO_RESCALE  
    %% Undo the image transformation used for numerical stability, Knum
    %% Not recommended.
    Mest = permute(reshape(Mest, [3, nIm, 4]), [1 3 2]);
    for kIm = 1:nIm
      Mest(:,:, kIm) = inv(reshape(Knum(:,:,kIm),3,3)) * reshape(Mest(:,:,kIm), 3, 4);
      Knum(:,:,kIm) = eye(3,3);
    end
    Mest = permute(Mest, [1 3 2]);
    Mest = reshape(Mest, [3*nIm, 4]);
  end
  Mest = reshape(Mest, [3, nIm, 4]);
  Mest = permute(Mest, [1 3 2]);
  MASSAGED_MEST = TRUE;
end

%%% Compute constraints on Qinf, the absolute dual quadric.
%%% Know Mest(i,:,k) * Qinf  * Mest(j,:,k)' = element i,j of K(:,:,k) * K(:,:,k)'
%%% K(:,:,k) the camera to image coordinate 3 x 3 matrix Knum * diag([f(k), f(k),1])
Acum = [];
bcum = [];
for iIm = 1:nIm
  
  %% Compute constraints on Qinf from current image formation
  %% matrix Mest(:,:,iIm).
  A = zeros(6,10);
  b = zeros(6,1);
  nc = 0;
  for i=1:3
    for j= i:3
      nc = nc+1;
      q = reshape(Mest(i,:,iIm),4,1) * reshape(Mest(j,:, iIm), 1,4);
      q = q + q' - diag(diag(q));
      A(nc,:) = [q(1,1:4) q(2,2:4) q(3,3:4) q(4,4)];
    end
  end
  KK = reshape(Knum(:,:,iIm),3,3) * diag([fMat(iIm) fMat(iIm) 1]);
  KK = KK * KK';
  %% For (i,j)=(3,3) currently 6th row in A and b
  b = [KK(1, 1:3) KK(2, 2:3) KK(3,3)]';
  
  if SELF_CALIBRATE
    %% For (i,j)=(1,1) and (2,2), 1st and 4th elements in A and b, get
    %% combined constraint.  This cancels the dependence on the focal length.
    A(4,:) = A(4,:) - A(1,:);
    b(4) = b(4) - b(1);
    %% Delete 1st row of both A and b to avoid extra constraint on (1,1) element.
    A = A(2:end, :);
    b = b(2:end);
  end
  
  %% Concatenate constraints on Qinf across all images.
  if size(Acum,1)>0
    Acum = [Acum; A];
    bcum = [bcum; b];
  else
    Acum = A;
    bcum = b;
  end
  
end

%%% Solve A q = b for q and hence for Qinf.
[uA sA vA] = svd(Acum,0); sA = diag(sA);
% log10(sA(1:10)+eps)'
%% Should be rank 10
if sA(10)/sA(1) < sqrt(eps)
  %% Oh, oh.  Trouble!!!!
  fprintf(2,'Warning: System for Qinf nearly singular.\n');
  fprintf(2,'Press any key to continue...');
  pause;
  %%  Solve A q = b, ignore singular element
  q = vA * (diag([sA(1:9).^-1 ; 0]) * (uA' * bcum));
  q0 = q;
  fprintf(2,'ok\n');
else
  %%  Solve A q = b
  q = vA * (diag([sA.^-1]) * (uA' * bcum));
  q0 = q;
end
if DEBUG
  fprintf(2, 'Residuals in solving for Qinf...');
  imStats(Acum * q - bcum);
end

%% Search along the least constrained direction for Q for a rank 3 Q matrix.
CHATTY = TRUE;
foundMetric = FALSE;
foundZero = FALSE;
minEQ = nan;
if CHATTY
  fprintf(2,'Eigenvalues of estimated Qinf at each value of itNull:\n'); 
end
for itNull = -50:1:50
  q = q0 + itNull * vA(:,10);

  %% Unpack q into symmetric 4x4 matrix
  Qinf = [q(1) q(2) q(3) q(4);
          q(2) q(5) q(6) q(7);
          q(3) q(6) q(8) q(9);
          q(4) q(7) q(9) q(10)];

  %% Check out eigenvalues of estimated Qinf
  [uQ sQ vQ] = svd(Qinf); sQ = diag(sQ);
  sgn = diag(uQ' * vQ)';
  if CHATTY
    fprintf(2,'%f: ', itNull);
    fprintf(2,' %e',sQ(:).*sgn(:)); 
    fprintf(2,'\n');
  end
    
  if all(sgn(1:3)>0)
    if isnan(minEQ) 
      minEQ = sgn(4);
    elseif  minEQ * sgn(4) < 0 & ~foundZero
      if CHATTY
        fprintf(2, 'Detected zero crossing, itNull = %f\n',itNull);
      end
      foundMetric = TRUE;
      minEQ = sgn(3);
      itNull0 = itNull;
      foundZero = TRUE;
    end
  end
end

if ~foundMetric
  fprintf(2, 'Failed to do self-calibration\n');
else
  % Use itNull0
  q = q0 + itNull0 * vA(:,10);

  %% Unpack q into symmetric 4x4 matrix
  Qinf = [q(1) q(2) q(3) q(4);
          q(2) q(5) q(6) q(7);
          q(3) q(6) q(8) q(9);
          q(4) q(7) q(9) q(10)];

  %% Check out eigenvalues of estimated Qinf
  [uQ sQ vQ] = svd(Qinf); sQ = diag(sQ);
  sgn = diag(uQ' * vQ)';
  %% Find closest non-negative definite rank 3 matrix to estimated Qinf
  kSv = 4; %% Set smallest sv to set to zero
  sQinf = sQ;
  sQinf(kSv) = 0;
  Qinf = uQ * diag(sQinf) * uQ';  %% Reconstructed rank 3 Qinf.

  %%% Estimate homography for metric upgrade from this Qinf.
  sH = zeros(4,1);
  uH = zeros(4,4);
  n = 1;
  for k=1:4
    if k ~= kSv
      sH(n) = sQ(k); 
      uH(:,n) = uQ(:,k);
      n= n+1;
    end
  end
  uH(:,4) = uQ(:,kSv);
  sH(4) = 1;
  sH = sH .^ -0.5;
  H = (uH * diag(sH))';
  Hinv = uH' * diag(sH.^-1);

  if DEBUG
    fprintf(2, 'Sanity check; Transformed Qinf should be apprx diag([1 1 1 0])\n');
    H * Qinf * H'
  end

  %%%% Given this estimated homography H, apply it the the projective
  %reconstruction.
  Hm2 = H;
  Pm2 = H * Pest;
  Pm2 = Pm2 ./ repmat(Pm2(4,:),4,1);

  %%%%% Surface plot of metric reconstruction.
  figure(3); clf;
  for k = 1:length(f)
    vf = Pm2(:,f{k});
    patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)');
  end
  set(gca,'YDir', 'reverse', 'FontSize', 14);
  axis vis3d; axis square; axis equal;
  title('Euclidean Reconstruction');
  fprintf(2,' Rotate this reconstruction.\n');
  fprintf(2,...
          ' Note: The colour map is NOT supposed to be the same, just the shape.\n');

  pause;


  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%%% Check out metric reconstruction.  
  %%%% Does it approximate the ground truth data up to a 3D Homography?
  %%%% The problem here is that we cannot expect to simply display projective
  %%%% reconstructions.  We first need to get the homography roughly right.
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  %% Fit a rigid transform from the estimated reconstruction, Pest, to the
  %% ground truth data P0.

  [Haff] = linEstAff3D(P0, Pm2);

  %% FInd a nearby rigid transform;
  R0 = Haff(1:3,1:3);
  [Ur Sr Vr] = svd(R0); Sr = diag(Sr);
  Sr = sum(Sr)/3 * eye(3);
  R = Ur * Sr * Vr';
  Haff(1:3,:) = [R, R * inv(R0)* Haff(1:3,4)];

  Pmassaged = Haff* Pm2;
  XYZmassaged = Pmassaged./repmat(Pmassaged(4,:),4,1);

  %%% Show 3D coords for transformed reconstruction.
  %%% NOTE: The homography has be chosen using the ground truth.
  %%% This just illustrates the consistency of the projective
  %%% reconstruction with the original data.
  figure(1); clf;
  for k = 1:3
    subplot(1,3,k)
    hand1=plot(XYZmassaged(k,:), 'b', 'LineWidth', 2);
    set(gca,'FontSize', 14);
    hold on;
    hand2=plot(P0(k,:), 'r');
    if k == 2
      title('Recovered Euclidean Model (b), Ground Truth (r)');
    end
    xlabel('Point index');
    ylabel(sprintf('X(%d)\n',k));
  end
  fprintf(2,'Press any key to continue...');
  pause;
  fprintf(2,'ok\n');

  %% Summarize errors.
  fprintf(2,'RMS Error: %f %f %f\n', sqrt(sum((P0(1:3,:) - XYZmassaged(1:3,:)).^2,2)/nPts));
  mn0 = sum(P0(1:3,:),2)/nPts;
  fprintf(2,'Data Point RMS variation: %f %f %f\n', sqrt(sum((P0(1:3,:) - repmat(mn0,1,nPts)).^2,2)/nPts));

  %%%% Surface plot of metric reconstruction.
  figure(3); clf;
  for k = 1:length(f)
    vf = XYZmassaged(:,f{k});
    patch(vf(1,:)', vf(2, :)', vf(3,:)', -vf(3,:)');
  end
  set(gca,'YDir', 'reverse', 'FontSize', 14);
  axis vis3d; axis square; axis equal;
  title('Euclidean Reconstruction');
  fprintf(2,' Rotate this reconstruction.\n');
end


%% Summary: For nIm perspective images, nIm >=3, we have demonstrated:
%%   - Euclidean scene reconstruction from 3 or more orthographic images.
%%   - The reconstruction of the viewing directions.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% End: projectiveMassageDino.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%