1 | % [P,X] = vgg_signsPX_from_x(P,X,x) Finds signs of P and X in a projective reconstruction.
|
---|
2 | %
|
---|
3 | % Given a projective reconstruction, i.e. P, X, and x such that
|
---|
4 | % s_n^k x_n^k = P^k X_n,
|
---|
5 | % where
|
---|
6 | % - P^k is k-th camera matrix
|
---|
7 | % - X_n is n-th scene point
|
---|
8 | % - x_n^k is image projection of X_n in camera P^k
|
---|
9 | % - s_n^k is scale,
|
---|
10 | % it will change signs of P^k and X_n such that all s_n^k are positive. Positivity of s_n^k
|
---|
11 | % determines the signs of P and X uniquely up to a single overall sign.
|
---|
12 | %
|
---|
13 | % Parameters:
|
---|
14 | % P ... cell(K) of 3-by-4 matrices, camera matrices.
|
---|
15 | % P also can be (3*K)-by-4 joint camera matrix.
|
---|
16 | % P also can be 3-by-4-by-K array.
|
---|
17 | % X ... double(4,N), scene points in homog. coordinates
|
---|
18 | % x ... cell(K) of double(3,N), image points in homog. coordinates.
|
---|
19 | % If an image point is missing, set x{k}(:,n) = [NaN;NaN;NaN].
|
---|
20 | % x also can be joint (3*K)-by-N joint image point matrix, again with NaNs if a point is missing.
|
---|
21 | % x also can be 3-by-N-by-K array.
|
---|
22 | %
|
---|
23 | % If it is not possible to change signs of P^k and X_n such that s_n^k are positive, it is P=X=[].
|
---|
24 | % This means that the projective reconstruction [P,X,x] does not correspond to any real scene.
|
---|
25 | %
|
---|
26 | % The function works for any dimension, ie, D-by-(D+1) camera matrices.
|
---|
27 | %
|
---|
28 | % See also vgg_selfcalib_qaffine.
|
---|
29 |
|
---|
30 | function [P0,X] = vgg_signsPX_from_x(P0,X,u0)
|
---|
31 |
|
---|
32 | % Re-arrange input data to joint camera matrix and joint image points.
|
---|
33 | if iscell(P0)
|
---|
34 | P = vertcat(P0{:});
|
---|
35 | u = vertcat(u0{:});
|
---|
36 | else
|
---|
37 | if ndims(P0)==2 % joint camera matrix
|
---|
38 | P = P0;
|
---|
39 | u = u0;
|
---|
40 | else % P(:,:,k) is k-th camera matrix
|
---|
41 | P = [];
|
---|
42 | u = [];
|
---|
43 | for k = 1:size(P0,3)
|
---|
44 | P = [P; P0(:,:,k)];
|
---|
45 | u = [u; u0(:,:,k)];
|
---|
46 | end
|
---|
47 | end
|
---|
48 | end
|
---|
49 |
|
---|
50 | [D N] = size(X);
|
---|
51 | K = size(P,1)/(D-1);
|
---|
52 |
|
---|
53 | % Do sign swapping in joint image / joint camera matrix format
|
---|
54 | if any(isnan(u(:)))
|
---|
55 | [P,X] = signsPX_from_x_occl(P,X,u); % slower code but can handle undefined points
|
---|
56 | else
|
---|
57 | [P,X] = signsPX_from_x(P,X,u); % faster code if all image points are defined
|
---|
58 | end
|
---|
59 |
|
---|
60 | if isempty(P)
|
---|
61 | P0 = [];
|
---|
62 | return
|
---|
63 | end
|
---|
64 |
|
---|
65 | % Re-arrange back to original format
|
---|
66 | if iscell(P0)
|
---|
67 | for k = 1:length(P0)
|
---|
68 | P0{k} = P([1:D-1]+(D-1)*(k-1),:);
|
---|
69 | end
|
---|
70 | else
|
---|
71 | if ndims(P0)==2
|
---|
72 | P0 = P;
|
---|
73 | else
|
---|
74 | for k = 1:size(P0,3)
|
---|
75 | P0(:,:,k) = P([1:D-1]+(D-1)*(k-1),:);
|
---|
76 | end
|
---|
77 | end
|
---|
78 | end
|
---|
79 |
|
---|
80 | return
|
---|
81 |
|
---|
82 |
|
---|
83 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
84 |
|
---|
85 |
|
---|
86 | % Does the sign swapping if all image points are defined (ie, no nans are in u).
|
---|
87 | function [P,X] = signsPX_from_x(P,X,x)
|
---|
88 |
|
---|
89 | [D N] = size(X);
|
---|
90 | K = size(P,1)/(D-1);
|
---|
91 |
|
---|
92 | s = sign( reshape( sum(reshape(x,D-1,K*N).*reshape(P*X,D-1,K*N)), K,N) );
|
---|
93 |
|
---|
94 | sP = s(:,1);
|
---|
95 | s = sP(:,ones(1,N)) .* s;
|
---|
96 |
|
---|
97 | sX = s(1,:);
|
---|
98 | s = sX(ones(1,K),:) .* s;
|
---|
99 |
|
---|
100 | if any(s(:)<0)
|
---|
101 | P = [];
|
---|
102 | X = [];
|
---|
103 | return
|
---|
104 | end
|
---|
105 |
|
---|
106 | aux = sP(:,ones(1,D-1))'; aux = aux(:); P = P .* aux(:,ones(1,D));
|
---|
107 | X = X .* sX(ones(1,D),:);
|
---|
108 |
|
---|
109 | return
|
---|
110 |
|
---|
111 |
|
---|
112 | % Does sign swapping if there are undefined points (nans) in x.
|
---|
113 | function [P,X] = signsPX_from_x_occl(P,X,u)
|
---|
114 |
|
---|
115 | [D N] = size(X);
|
---|
116 | K = size(P,1)/(D-1);
|
---|
117 |
|
---|
118 | PX = reshape( dot(reshape(u,D-1,K*N),reshape(P*X,D-1,K*N)), K,N);
|
---|
119 | for initp = 1:K
|
---|
120 | p = NaN*ones(K,1);
|
---|
121 | x = NaN*ones(1,N);
|
---|
122 | p(initp) = 1;
|
---|
123 | n = 1;
|
---|
124 | oldn = 0;
|
---|
125 | while n-oldn > 0
|
---|
126 | oldn = n;
|
---|
127 | x = updatej(p,x,PX);
|
---|
128 | p = updatej(x',p',PX')';
|
---|
129 | n = nnz(~isnan([p' x]));
|
---|
130 | end
|
---|
131 | if all(all(isnan(PX) | ~isnan((p*x).*PX)))
|
---|
132 | break
|
---|
133 | end
|
---|
134 | end
|
---|
135 | P = (reshape(ones(D-1,1)*p',(D-1)*K,1)*ones(1,D)) .* P;
|
---|
136 | X = (ones(D,1)*x) .* X;
|
---|
137 |
|
---|
138 | % check if the sign changing process was succesful
|
---|
139 | uPX = reshape( dot(reshape(u,D-1,K*N),reshape(P*X,D-1,K*N)), K,N);
|
---|
140 | if ~all(all( (uPX>0) | isnan(uPX) ))
|
---|
141 | P = [];
|
---|
142 | X = [];
|
---|
143 | end
|
---|
144 |
|
---|
145 | return
|
---|
146 |
|
---|
147 |
|
---|
148 | % Auxiliary function used for swaping the signs of P^k, X_n.
|
---|
149 | function j = updatej(i,j,IJ)
|
---|
150 | mj = any(~isnan(i*ones(1,length(j))) & isnan(ones(length(i),1)*j) & ~isnan(IJ));
|
---|
151 | nj = IJ(:,mj) .* (i*ones(1,nnz(mj)));
|
---|
152 | nj_ = nj(:); nj_(isnan(nj_)) = 0; nj(:) = nj_;
|
---|
153 | j(mj) = sign(sum(nj));
|
---|
154 | return
|
---|