www.pudn.com > projective-and-affine.rar > projectiveMassageDino.asv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 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 'C:\MATLAB6p5\work\matlabVisTools\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));
%P0 = 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)]; %把p*q*p'=p*p'*q因为这里q写出矢量形式，对*没有影响
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)]';
%只是通过把一个对称矩阵写成矢量的形式来解Aq = b
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 = -5:1:5
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)';%一直都是（1，1，1，-1），直到－4时是（1 1 1 1），然后是接着几个（1 1 1 1），然后是（-1 -1 -1 1），在－4时Detected zero crossing
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%