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