[37] | 1 | % vgg_selfcalib_metric_vansq Metric selfcalibration from 3 orthogonal principal directions and square pixels.
|
---|
| 2 | %
|
---|
| 3 | % DESCRIPTION
|
---|
| 4 | % Given projective camera matrices P and 3 scene points V, it computes 3D-to-3D
|
---|
| 5 | % homography H which upgrades the old reconstruction to metric one, i.e.,
|
---|
| 6 | % differing from the true one only by isotropic scaling.
|
---|
| 7 | % H is computed from the following constraints :-
|
---|
| 8 | % (1) points H*V are at infty and mutually orthogonal (i.e., H*V==eye(4,3)),
|
---|
| 9 | % (2) cameras P*inv(H) have square pixels,
|
---|
| 10 | % Constraint (1) is hard, (2) is soft. I.e., if [P,V] are not consistent
|
---|
| 11 | % (2) will be satisfied only partially (in linear least squares sense).
|
---|
| 12 | %
|
---|
| 13 | % SYNSOPSIS
|
---|
| 14 | % [H,sv] = vgg_selfcalib_metric_vansq(P,V), where
|
---|
| 15 | % P ... cell{K} of double(3,4), projective cameras
|
---|
| 16 | % V ... double(4,3), 3 projective scene points (homog. coords.)
|
---|
| 17 | % H ... double(4,4), upgrading 3D-to-3D homography
|
---|
| 18 | % sv ... 2-vector, last 2 singular values of linear system solving for square pixels.
|
---|
| 19 | % In healthy situation, sv(2) must be tiny and sv(1) reasonably large.
|
---|
| 20 | %
|
---|
| 21 | % NOTE: To get correct handedness (= non-mirroring) of the reconstruction,
|
---|
| 22 | % make sure that V satisfies
|
---|
| 23 | % vgg_wedge(V)*[X C] > 0
|
---|
| 24 | % for all scene points X and camera centers C(:,k) = vgg_wedge(P{k}). This can be
|
---|
| 25 | % achieved by swapping signs of P and X using vgg_signsPX_from_x. The thing
|
---|
| 26 | % requires also positive handedness of image and scene coord. systems.
|
---|
| 27 | %
|
---|
| 28 | % SEE ALSO vgg_selfcalib_qaffine.
|
---|
| 29 |
|
---|
| 30 | % T.Werner, Feb 2002
|
---|
| 31 |
|
---|
| 32 | function H = vgg_selfcalib_metric_vansq(P,V)
|
---|
| 33 |
|
---|
| 34 | K = length(P);
|
---|
| 35 |
|
---|
| 36 | %%%%%%%%%%
|
---|
| 37 | % Step 1:
|
---|
| 38 | % Find 3D homography H1 sending V to eye(4,3).
|
---|
| 39 | % This results in reconstruction differing from metric one only in scaling in axis directions.
|
---|
| 40 | %%%%%%%%%%%
|
---|
| 41 |
|
---|
| 42 |
|
---|
| 43 | H1 = inv(normx([V vgg_wedge(V)']));
|
---|
| 44 | for k = 1:K
|
---|
| 45 | P{k} = P{k}/H1;
|
---|
| 46 | end
|
---|
| 47 |
|
---|
| 48 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
| 49 | % Step 2:
|
---|
| 50 | % Find scaling in axis directions from square pixel assumption.
|
---|
| 51 | % This scaling is represented by diagonal 3D homography H2.
|
---|
| 52 | %
|
---|
| 53 | % Algorithm:
|
---|
| 54 | % Consider input camera matrix P = [M v] and output matrix Q = K*R*[eye(3) -t]. It is Q =~ P*H where H = diag([d 1]).
|
---|
| 55 | % Let O = diag([1 1 1 0]) be absolute quadric. Then Q*O*Q' = DIAC = inv(IAC). For square pixels, IAC(1,1)=IAC(2,2) and IAC(1,2)=0.
|
---|
| 56 | % Substitution gives Q*O*Q' =~ P*H*O*H'*P' where P*H*O*H'*P' = M*diag(d.^2)*M' =~ inv(IAC). I.e.,
|
---|
| 57 | % IAC =~ inv(M)*diag(d.^(-2))*inv(M)'.
|
---|
| 58 | % The RHS of the last expression can be rearranged as
|
---|
| 59 | % vech(IAC) =~ pinv(duplication(3))*kron(inv(M)',inv(M)')*diagonalize(3)*d, where size(d)=[3 1].
|
---|
| 60 | % This is used to build a system of linear equations. We showed things only for onen camera - more cameras can be added to the system easily.
|
---|
| 61 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
| 62 |
|
---|
| 63 | % compose the linear system
|
---|
| 64 | A = [];
|
---|
| 65 | for k = 1:K
|
---|
| 66 | aux = inv(P{k}(:,1:3))';
|
---|
| 67 | aux = pinv(vgg_duplic_matrix(3))*kron(aux,aux)*diagonalize(3);
|
---|
| 68 | A = [A; [aux(1,:)-aux(4,:); aux(2,:)]];
|
---|
| 69 | end
|
---|
| 70 |
|
---|
| 71 | % solve it
|
---|
| 72 | [dummy,sv,d] = svd(A,0);
|
---|
| 73 | d = d(:,end);
|
---|
| 74 | sv = diag(sv);
|
---|
| 75 | sv = sv(2:3)/sv(1); % normalize sing values
|
---|
| 76 |
|
---|
| 77 | % form H
|
---|
| 78 | d = 1./sqrt(abs(d));
|
---|
| 79 | H2 = diag([d;1]);
|
---|
| 80 | H = inv(H2)*H1;
|
---|
| 81 |
|
---|
| 82 | return
|
---|
| 83 |
|
---|
| 84 | %%%%%%%%%%%%%%%%%%%%
|
---|
| 85 |
|
---|
| 86 |
|
---|
| 87 | % G = diagonalize(n) Diagonalization matrix. It is vec(diag(x)) = diagonalize(length(x))*x.
|
---|
| 88 | function G = diagonalize(n)
|
---|
| 89 | G = zeros(n^2,n);
|
---|
| 90 | i = [];
|
---|
| 91 | for j = 0:n-1
|
---|
| 92 | i = [i 1+n*j+j];
|
---|
| 93 | end
|
---|
| 94 | G(i,:) = eye(n);
|
---|
| 95 | return
|
---|
| 96 |
|
---|
| 97 |
|
---|
| 98 | % x = normx(x) Normalize MxN matrix so that norm of each its column is 1.
|
---|
| 99 | function x = normx(x)
|
---|
| 100 | if ~isempty(x)
|
---|
| 101 | x = x./(ones(size(x,1),1)*sqrt(sum(x.*x)));
|
---|
| 102 | end
|
---|
| 103 | return |
---|