1 | % vgg_selfcalib_qaffine Upgrading projective to quasi-affine reconstruction.
|
---|
2 | %
|
---|
3 | % Given projective reconstruction [P,X] with correct signs of P and X
|
---|
4 | % (the output of vgg_signsPX_from_x), it finds homography H transforming
|
---|
5 | % [P,X] to quasi-affine reconstruction [Pq,Xq] = [P*inv(H),H*X].
|
---|
6 | % Let Ainf=[0 0 0 1] be plane at infinity, then [Pq,Xq] has the property that :-
|
---|
7 | %
|
---|
8 | % Ainf * Xq > 0 (all scene points in front of plane at infty)
|
---|
9 | % Ainf * vgg_wedge(Pq{k}) > 0 (all camera centers in front of plane at infty)
|
---|
10 | %
|
---|
11 | % H = vgg_selfcalib_qaffine(P,X), where
|
---|
12 | % P ... cell(K) of double(3,4), camera matrices. K is number of cameras.
|
---|
13 | % P also can be 3x4xK array.
|
---|
14 | % X ... double(4,N), scene points in homog. coordinates.
|
---|
15 | % H ... cell{I} of double(4,4), homographies upgrading [P,X] to quasi-affine reconstruction.
|
---|
16 | % There can be 0, 1, or 2 solution classes (corresponding to I=0,1,2) :-
|
---|
17 | % - I==0 ... no solution, ie [P,X] cannot be transformed to any affine scene.
|
---|
18 | % - I==1 ... 1 solution, ie camera centers and scene points
|
---|
19 | % are not separable by a plane in the true scene.
|
---|
20 | % - I==2 ... 2 solutions, ie camera centers and scene points are separable
|
---|
21 | % by a plane in the true scene. Then there are two solutions for plane at infinity,
|
---|
22 | % differing by sign(det(H{i})). The two reconstruction corresponding to H{1} and H{2}
|
---|
23 | % have oppposite handedness and we cannot say which handedness is that of the true scene.
|
---|
24 | % (Note: by 'solution' we mean rather 'class of solutions' - indeed there are infinitely many
|
---|
25 | % solutions if I>0, and linear programming chooses a single solution out of them.)
|
---|
26 | %
|
---|
27 | % EXAMPLE: Let [P,X] be a projective reconstruciton from homogeneous image points x.
|
---|
28 | % Upgrade to quasi-affine reconstruction is done as follows:
|
---|
29 | % [P,X] = vgg_signsPX_from_x(P,X,x);
|
---|
30 | % H = vgg_selfcalib_qaffine(P,X);
|
---|
31 | % H = H{1}; % single solution assumed
|
---|
32 | % P = P*inv(H);
|
---|
33 | % X = H*X;
|
---|
34 | % If either of rows 1 and 2 returns no solution, there's something wrong with
|
---|
35 | % the reconstruction, eg an outlier.
|
---|
36 |
|
---|
37 | % T.Werner, Feb 2002, werner@robots.ox.ac.uk
|
---|
38 |
|
---|
39 | function H = vgg_selfcalib_qaffine(P,X)
|
---|
40 |
|
---|
41 | if ndims(P)==3
|
---|
42 | for k = 1:size(P,3)
|
---|
43 | Q{k} = P(:,:,k);
|
---|
44 | end
|
---|
45 | P = Q;
|
---|
46 | end
|
---|
47 |
|
---|
48 | [D N] = size(X);
|
---|
49 | K = length(P);
|
---|
50 |
|
---|
51 | for k = 1:K
|
---|
52 | C(:,k) = vgg_wedge(P{k}); % oriented camera centers
|
---|
53 | end
|
---|
54 |
|
---|
55 | % Solve chiral equalities:
|
---|
56 | %
|
---|
57 | % A := found plane at infinity
|
---|
58 | % detH := required det(H)
|
---|
59 | % (A and detH can be none, one, or two according to the number of solution classes)
|
---|
60 | detH = [];
|
---|
61 | A = [];
|
---|
62 | for detHa = [-1 1]
|
---|
63 | Aa = sephplane([X detHa*C]);
|
---|
64 | if ~isempty(Aa)
|
---|
65 | A = [A; Aa];
|
---|
66 | detH = [detH; detHa];
|
---|
67 | end
|
---|
68 | end
|
---|
69 |
|
---|
70 | if isempty(A)
|
---|
71 | H = {};
|
---|
72 | return
|
---|
73 | end
|
---|
74 |
|
---|
75 |
|
---|
76 | % compose final homography H
|
---|
77 | for i = 1:size(A,1)
|
---|
78 |
|
---|
79 | % find H{i} such that H{i}(4,:)==A
|
---|
80 | [dummy,dummy,H{i}] = svd(A(i,:),0);
|
---|
81 | H{i} = H{i}(end:-1:1,:);
|
---|
82 |
|
---|
83 | % make det(H{i}) the same sign as detH(i)
|
---|
84 | if det(H{i})*detH(i) < 0
|
---|
85 | H{i} = H{i}([2 1 3 4],:);
|
---|
86 | end
|
---|
87 |
|
---|
88 | % 'beautifier' of X:
|
---|
89 | % Do singular value equalization on the set X,
|
---|
90 | % i.e., make mean(nhom(H{i}*X),2)==[0;0;0] and svd(nhom(H{i}*X))==[1;1;1].
|
---|
91 | Xi = vgg_get_nonhomg(H{i}*X);
|
---|
92 | c = mean(Xi,2); % centroid
|
---|
93 | Xi = Xi - c*ones(1,N);
|
---|
94 | [U,S] = eig(Xi*Xi'); % sv equalization
|
---|
95 | S = diag(1./sqrt(diag(S)));
|
---|
96 | K = S*U';
|
---|
97 | if det(K) < 0 % we want the sv equalization to be parity-preserving
|
---|
98 | K = -K;
|
---|
99 | end
|
---|
100 | H{i} = [ K -K*c; 0 0 0 1 ]*H{i};
|
---|
101 |
|
---|
102 | end
|
---|
103 |
|
---|
104 | return
|
---|
105 |
|
---|
106 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
107 |
|
---|
108 |
|
---|
109 | % A = sephplane(X) Finds separating hyperplane A such that all(A*X)>0.
|
---|
110 | % If no solution exists, A = [].
|
---|
111 | % Works for any dimension of X.
|
---|
112 | function A = sephplane(X)
|
---|
113 |
|
---|
114 | [D,N] = size(X);
|
---|
115 |
|
---|
116 | X = X ./ (ones(D,1)*sqrt(sum(X.*X)));
|
---|
117 | A = [-X' ones(N,1)];
|
---|
118 | b = zeros(size(A,1),1);
|
---|
119 | f = [zeros(1,D) -1]';
|
---|
120 | LB = [-ones(1,D) 0];
|
---|
121 | UB = [ones(1,D) Inf];
|
---|
122 | fprintf('vgg selfcalib_qaffine: linprog for %d %dd pts ... ', size(X,2), D);
|
---|
123 | options = optimset('linprog');
|
---|
124 | options.Display = 'off';
|
---|
125 | [res,FVAL,EXITFLAG] = linprog(f,A,b,[],[],LB,UB, [], options);
|
---|
126 | if isempty(res)
|
---|
127 | fprintf('no feasible plane\n');
|
---|
128 | A = [];
|
---|
129 | return
|
---|
130 | end
|
---|
131 | A = res(1:D)';
|
---|
132 |
|
---|
133 | if ~all(A*X > 0)
|
---|
134 | fprintf('feasible plane returned, but is not in fact feasible\n');
|
---|
135 | A = [];
|
---|
136 | end
|
---|
137 |
|
---|
138 | fprintf('Got plane [%.2f %.2f %.2f %.2f]\n', A);
|
---|
139 |
|
---|
140 | return
|
---|
141 |
|
---|
142 |
|
---|
143 | %i = k2i(k)
|
---|
144 | % Computes indices of joint point matrix rows corresponding to views k.
|
---|
145 | %% function i = k2i(k,step)
|
---|
146 | %% k = k(:)';
|
---|
147 | %% i = [1:3]'*ones(size(k)) + 3*(ones(3,1)*k-1);
|
---|
148 | %% i = i(:);
|
---|