[37] | 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(:);
|
---|