1 | % [P,X] = bundle_PX_proj(P0,X0,u,imsize [,opt]) Projective bundle adjustment. |
---|
2 | % |
---|
3 | % P0 ... double(3*K,4), joint camera matrix |
---|
4 | % X0 ... double(4,N), scene points |
---|
5 | % u ... double(2*K,N), joint image point matrix with nonhomogeneous image points |
---|
6 | % imsize ... double(2,K), image sizes: imsize(:,k) is size of image k |
---|
7 | % P, X ... bundled reconstruction |
---|
8 | % |
---|
9 | % P0, X0 and u need not be preconditioned - preconditioning is done inside |
---|
10 | % this function. |
---|
11 | % |
---|
12 | % opt.verbose (default 1) .. whether display info |
---|
13 | % .verbose_short(default 0) .. whether short info |
---|
14 | |
---|
15 | function [P,X] = bundle_PX_proj(P0,X0,q,imsize, opt) |
---|
16 | |
---|
17 | if nargin < 5 | isempty(opt) | ~isfield(opt,'verbose') |
---|
18 | opt.verbose = 1; end |
---|
19 | if ~isfield(opt,'verbose_short') |
---|
20 | opt.verbose_short = 0; end |
---|
21 | |
---|
22 | RADIAL = 0; % Commented by DM (nargin > 4); % don't estimate/estimate radial distortion |
---|
23 | [K,N] = size(q); K = K/2; |
---|
24 | |
---|
25 | % precondition |
---|
26 | for k = 1:K |
---|
27 | H{k} = vgg_conditioner_from_image(imsize(:,k)); |
---|
28 | P0(k2i(k),:) = H{k}*P0(k2i(k),:); |
---|
29 | q(k2i(k,2),:) = nhom(H{k}*hom(q(k2i(k,2),:))); |
---|
30 | end |
---|
31 | |
---|
32 | P0 = normP(P0); |
---|
33 | X0 = normx(X0); |
---|
34 | |
---|
35 | % form observation vector y |
---|
36 | qq = reshape(q,[2 K*N]); |
---|
37 | qivis = reshape(all(~isnan(qq)),[K N]); % visible image points |
---|
38 | qq = qq(:,qivis); |
---|
39 | y = qq(:); |
---|
40 | |
---|
41 | |
---|
42 | % Compute matrices TP,TX, describing local coordinate systems tangent to P0(:), X0. |
---|
43 | % Having homogeneous N-vector X, in the neighborhood of X0 it can be parameterized (minimaly) |
---|
44 | % by inhomogeneous (N-1)-vector iX as X = X0 + TX*iX, where TX is a non-singular (N,N-1) matrix. |
---|
45 | |
---|
46 | for n = 1:N |
---|
47 | [qX,dummy] = qr(X0(:,n)); |
---|
48 | TX{n} = qX(2:end,:)'; |
---|
49 | end |
---|
50 | for k = 1:K |
---|
51 | [qP,dummy] = qr(reshape(P0(k2i(k),:),[12 1])); |
---|
52 | TP{k} = qP(2:end,:)'; |
---|
53 | end |
---|
54 | |
---|
55 | % form initial parameter vector p0 |
---|
56 | p0 = []; |
---|
57 | for k = 1:K |
---|
58 | p0 = [p0; zeros(11,1)]; |
---|
59 | if RADIAL |
---|
60 | p0 = [p0; radial(k).u0; radial(k).kappa]; |
---|
61 | end |
---|
62 | end |
---|
63 | p0 = [p0; zeros(3*N,1)]; |
---|
64 | |
---|
65 | p = levmarq('F',...%@F, % commented by DM (perhaps Matlab version conflict) |
---|
66 | p0,opt,P0,TP,X0,TX,y,qivis,RADIAL); |
---|
67 | |
---|
68 | % rearrange computed parameter vector p to P, X |
---|
69 | for k = 1:K |
---|
70 | P(k2i(k),:) = inv(H{k})*(P0(k2i(k),:) + reshape(TP{k}*p([1:11]+(k-1)*(11+RADIAL*3)),[3 4])); |
---|
71 | if RADIAL |
---|
72 | radial(k).u0 = p([12:13]+(k-1)*(11+RADIAL*3)); |
---|
73 | radial(k).kappa = p([14]+(k-1)*(11+RADIAL*3)); |
---|
74 | end |
---|
75 | end |
---|
76 | for n = 1:N |
---|
77 | X(:,n) = X0(:,n) + TX{n}*p((11+RADIAL*3)*K+[1:3]+(n-1)*3); |
---|
78 | end |
---|
79 | |
---|
80 | if opt.verbose_short, fprintf(')'); end |
---|
81 | return |
---|
82 | |
---|
83 | |
---|
84 | %% |
---|
85 | % function F: (Np,1) --> (Ny,1)x(Ny,Np), [y,J]=F(p), value y and jacobian J=dF(p)/dp of the function. |
---|
86 | % |
---|
87 | % p ... size ((11+3)*K+3*N,1), p = [iP(1); u0(1); kappa(1); .. iP(K); u0(K); kappa(K); iX(1); .. iX(N)], |
---|
88 | % where Np = (11+2+raddeg)*K + 3*N |
---|
89 | % |
---|
90 | % Optional parameters: |
---|
91 | % TP{1:K} ... size (12,11), Pk(:) = P0k + TP{k}*iPk |
---|
92 | % TX{1:N} ... size (4,3), Xn = X0n + TX{n}*iXn |
---|
93 | % qivis ... size (K,N), has 1 in entries corresponding to visible image points |
---|
94 | % y ... size (2*nnz(qivis),1), visible image observations, stacked as [q(1,1); q(2,1); .. q(2*K,N)], unobserved q are omitted |
---|
95 | %% |
---|
96 | % We consider the imaging model as |
---|
97 | % u = hom(x) |
---|
98 | % x = P*X |
---|
99 | % P = P0 + TP*iP |
---|
100 | % X = X0 + TX*iX |
---|
101 | %% |
---|
102 | function [y,J] = F(p,P0,TP,X0,TX,y,qivis,RADIAL) |
---|
103 | % |
---|
104 | [K,N] = size(qivis); |
---|
105 | NR = RADIAL*3; % number of radial distortion paramters |
---|
106 | % |
---|
107 | % rearrange p to P, X [,radial] |
---|
108 | for k = 1:K |
---|
109 | P(k2i(k),:) = P0(k2i(k),:) + reshape(TP{k}*p([1:11]+(k-1)*(11+NR)),[3 4]); |
---|
110 | if RADIAL |
---|
111 | radial(k).u0 = p([12:13]+(k-1)*(11+NR)); |
---|
112 | radial(k).kappa = p([14]+(k-1)*(11+NR)); |
---|
113 | end |
---|
114 | end |
---|
115 | X = zeros(4,N); |
---|
116 | for n = 1:N |
---|
117 | X(:,n) = TX{n}*p((11+NR)*K+[1:3]+(n-1)*3); |
---|
118 | end |
---|
119 | X = X0 + X; |
---|
120 | % |
---|
121 | % compute retina points q |
---|
122 | for k = 1:K |
---|
123 | x(k2i(k),:) = P(k2i(k),:)*X; |
---|
124 | q(k2i(k,2),:) = nhom(x(k2i(k),:)); |
---|
125 | if RADIAL |
---|
126 | q(k2i(k,2),:) = raddist(q(k2i(k,2),:),radial(k).u0,radial(k).kappa); |
---|
127 | end |
---|
128 | end |
---|
129 | qq = reshape(q,[2 K*N]); |
---|
130 | qq = qq(:,qivis); |
---|
131 | y = qq(:)-y; % function value |
---|
132 | % |
---|
133 | % compute Jacobian J = dF(p)/dp |
---|
134 | if nargout<2 |
---|
135 | return |
---|
136 | end |
---|
137 | [kvis,nvis] = find(qivis); |
---|
138 | Ji = zeros(2*length(kvis)*(11+NR+3),1); Jj = zeros(size(Ji)); Jv = zeros(size(Ji)); |
---|
139 | cnt = 0; |
---|
140 | for l = 1:length(kvis) % loop for all VISIBLE points in all cameras |
---|
141 | k = kvis(l); |
---|
142 | n = nvis(l); |
---|
143 | xl = x(k2i(k),n); |
---|
144 | ul = nhom(xl); |
---|
145 | |
---|
146 | % Compute derivatives (Jacobians). Notation: E.g., dudx = du(x)/dx, etc. |
---|
147 | dxdP = kron(X(:,n)',eye(3))*TP{k}; % dx(iP,iX)/diP |
---|
148 | dxdX = P(k2i(k),:)*TX{n}; % dx(iP,iX)/diX |
---|
149 | dudx = [eye(2) -ul]/xl(3); |
---|
150 | if RADIAL |
---|
151 | [dqdu,dqdu0,dqdkappa] = raddist(ul,radial(k).u0,radial(k).kappa); |
---|
152 | else |
---|
153 | dqdu = eye(2); dqdu0 = []; dqdkappa = []; |
---|
154 | end |
---|
155 | dqdP = dqdu*dudx*dxdP; % dq(iP,iX)/diP |
---|
156 | dqdX = dqdu*dudx*dxdX; % dq(iP,iX)/dX |
---|
157 | c = cnt+[1:2*(11+NR+3)]; |
---|
158 | [Ji(c),Jj(c),Jv(c)] = spidx([1:2]+(l-1)*2,[[1:(11+NR)]+(k-1)*(11+NR) (11+NR)*K+[1:3]+(n-1)*3],[dqdP dqdu0 dqdkappa dqdX]); |
---|
159 | cnt = cnt + 2*(11+NR+3); |
---|
160 | end |
---|
161 | J = sparse(Ji,Jj,Jv,length(y),length(p)); |
---|
162 | return |
---|
163 | |
---|
164 | function [i,j,v] = spidx(I,J,V) |
---|
165 | % |
---|
166 | i = I'*ones(1,length(J)); |
---|
167 | j = ones(length(I),1)*J; |
---|
168 | i = i(:); |
---|
169 | j = j(:); |
---|
170 | v = V(:); |
---|
171 | return |
---|
172 | |
---|
173 | |
---|
174 | %%%%%%%%%% local functions %%%%%%%%%%% |
---|
175 | |
---|
176 | % p = levmarq(F,p,opt,varargin) Minimize || F(p,varargin{:}) || over p. By |
---|
177 | % Levenber-Marquardt. |
---|
178 | % |
---|
179 | % F ... handle to function [y,J] = F(p,varargin{:}), yielding value |
---|
180 | % y=F(p,varargin{:}) and jacobian J. aux is typically a structure |
---|
181 | % passing auxiliary arguments to F. |
---|
182 | % p ... initial parameter vector |
---|
183 | % opt ... options structure with possible fields :- |
---|
184 | % - verbose ... printing residuals |
---|
185 | % - res_scale ... residual scale, the prined residuals are multiplied by |
---|
186 | % ressc before printing; useful if normalization were done before calling |
---|
187 | % levmarq |
---|
188 | % - max_niter ... maximum number of iterations |
---|
189 | % - max_stepy ... maximal step of residual value (after multiplying by |
---|
190 | % res_scale) for termination |
---|
191 | % - lam_init ... initial value of lambda |
---|
192 | |
---|
193 | function p = levmarq(F,p,opt,varargin) |
---|
194 | |
---|
195 | % handle options |
---|
196 | if ~isfield(opt,'verbose'), opt.verbose = 0; end |
---|
197 | if ~isfield(opt,'res_scale'), opt.res_scale = 1; end |
---|
198 | if ~isfield(opt,'max_niter'), opt.max_niter = 10000; end |
---|
199 | if ~isfield(opt,'max_stepy'), max_stepy_undef = 1; opt.max_stepy = 100*eps*opt.res_scale; end |
---|
200 | if ~isfield(opt,'lam_init'), opt.lam_init = 1e-4; end |
---|
201 | |
---|
202 | %OLDWARN = warning('off'); % commented by DM (perhaps Matlab version conflict) |
---|
203 | |
---|
204 | % Minimize || F(p) || over p. By Newton/Levenber-Marquardt. |
---|
205 | lastFp = +Inf; |
---|
206 | stepy = +Inf; |
---|
207 | lam = opt.lam_init; |
---|
208 | nfail = 0; |
---|
209 | niter = 0; |
---|
210 | [Fp,J] = feval(F,p,varargin{:}); |
---|
211 | if issparse(J) |
---|
212 | eyeJ = speye(size(J,2)); |
---|
213 | else |
---|
214 | eyeJ = eye(size(J,2)); |
---|
215 | end |
---|
216 | |
---|
217 | % print out initial residuals |
---|
218 | if opt.verbose | opt.verbose_short |
---|
219 | if ~opt.verbose_short |
---|
220 | fprintf(' %14.10g [rms] %14.10g [max]\n',... |
---|
221 | opt.res_scale*sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp))); |
---|
222 | else |
---|
223 | fprintf('(rms/max/stepmax: %g/%g/', ... |
---|
224 | opt.res_scale*sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp))); end |
---|
225 | end |
---|
226 | |
---|
227 | while (nfail < 20) & (stepy*opt.res_scale > opt.max_stepy) & (niter < opt.max_niter) |
---|
228 | |
---|
229 | D = -(J'*J + lam*eyeJ) \ (J'*Fp); |
---|
230 | if any( isnan(D) | abs(D)==inf ) |
---|
231 | p = p*nan; |
---|
232 | return |
---|
233 | end |
---|
234 | FpD = feval(F,p+D,varargin{:}); |
---|
235 | |
---|
236 | if sum((Fp).^2) > sum((FpD).^2) % success |
---|
237 | p = p + D; |
---|
238 | lam = max(lam/10,1e-15); |
---|
239 | stepy = max(abs(Fp-lastFp)); |
---|
240 | lastFp = Fp; |
---|
241 | [Fp,J] = feval(F,p,varargin{:}); |
---|
242 | nfail = 0; |
---|
243 | niter = niter + 1; |
---|
244 | else % failure |
---|
245 | lam = min(lam*10,1e5); |
---|
246 | nfail = nfail + 1; |
---|
247 | end |
---|
248 | |
---|
249 | % if success, print out residuals |
---|
250 | if (opt.verbose | opt.verbose_short) & nfail==0 |
---|
251 | if ~opt.verbose_short |
---|
252 | fprintf(' %7.2g [lam]: %14.10g [rms] %14.10g [max] %10.5g [stepmax]\n',lam,opt.res_scale*sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp)),opt.res_scale*stepy); |
---|
253 | else fprintf(' %g/%g/%g', sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp)),opt.res_scale*stepy); end |
---|
254 | end |
---|
255 | end |
---|
256 | |
---|
257 | if ~exist('max_stepy_undef') & stepy*opt.res_scale <= opt.max_stepy |
---|
258 | fprintf('\n!!! finished because of high opt.max_stepy(=%f)', opt.max_stepy); end |
---|
259 | |
---|
260 | %warning(OLDWARN); % commented by DM (perhaps Matlab version conflict) |
---|
261 | return |
---|
262 | |
---|
263 | |
---|
264 | function [C,invC] = vgg_conditioner_from_image(c,r) |
---|
265 | % |
---|
266 | % function [C,invC] = vgg_conditioner_from_image(image_width, image_height) |
---|
267 | % |
---|
268 | % Makes a similarity metric for conditioning image points. |
---|
269 | % |
---|
270 | % Also can be called as vgg_conditioner_from_image([image_width image_height]) |
---|
271 | % |
---|
272 | % invC is inv(C), obtained more efficiently inside the function. |
---|
273 | |
---|
274 | if nargin<2 |
---|
275 | r = c(2); |
---|
276 | c = c(1); |
---|
277 | end |
---|
278 | |
---|
279 | f = (c+r)/2; |
---|
280 | C = [1/f 0 -c/(2*f) ; |
---|
281 | 0 1/f -r/(2*f) ; |
---|
282 | 0 0 1]; |
---|
283 | |
---|
284 | if nargout > 1 |
---|
285 | invC = [f 0 c/2 ; |
---|
286 | 0 f r/2 ; |
---|
287 | 0 0 1]; |
---|
288 | end |
---|
289 | |
---|
290 | |
---|
291 | function x = nhom(x) |
---|
292 | %nhom Projective to euclidean coordinates. |
---|
293 | % x = nhom(x_) Computes euclidean coordinates by dividing |
---|
294 | % all rows but last by the last row. |
---|
295 | % x_ ... Size (dim+1,N) Projective coordinates. |
---|
296 | % x ... Size (dim,N). Euclidean coordinates. |
---|
297 | % (N can be arbitrary.) |
---|
298 | if isempty(x) |
---|
299 | x = []; |
---|
300 | return; |
---|
301 | end |
---|
302 | d = size(x,1) - 1; |
---|
303 | x = x(1:d,:)./(ones(d,1)*x(end,:)); |
---|
304 | return |
---|
305 | |
---|
306 | |
---|
307 | function x = hom(x) |
---|
308 | %hom Euclidean to projective coordinates. |
---|
309 | % x_ = hom(x) Adds the last row of ones. |
---|
310 | % x ... Size (dim,N). Euclidean coordinates. |
---|
311 | % x_ ... Size (dim+1,N) Projective coordinates. |
---|
312 | % (N can be arbitrary.) |
---|
313 | |
---|
314 | if isempty(x), return, end |
---|
315 | x(end+1,:) = 1; |
---|
316 | return |
---|
317 | |
---|
318 | |
---|
319 | % normP Normalize joint camera matrix so that norm(P(k2i(k),:),'fro') = 1 for each k. |
---|
320 | |
---|
321 | function P = normP(P) |
---|
322 | |
---|
323 | for k = 1:size(P,1)/3 |
---|
324 | Pk = P(k2i(k),:); |
---|
325 | P(k2i(k),:) = Pk/sqrt(sum(sum(Pk.*Pk))); |
---|
326 | end |
---|
327 | |
---|
328 | return |
---|
329 | |
---|
330 | |
---|
331 | % x = normx(x) Normalize MxN matrix so that norm of each its column is 1. |
---|
332 | |
---|
333 | function x = normx(x) |
---|
334 | |
---|
335 | if ~isempty(x) |
---|
336 | x = x./(ones(size(x,1),1)*sqrt(sum(x.*x))); |
---|
337 | end |
---|
338 | |
---|
339 | return |
---|
340 | |
---|
341 | |
---|
342 | function i = k2i(k,step) |
---|
343 | |
---|
344 | %i = k2i(k [,step]) |
---|
345 | % Computes indices of matrix rows corresponding to views k. If k is a scalar, |
---|
346 | % it is i = [1:step]+step*(k-1). |
---|
347 | % implicit: step = 3 |
---|
348 | % E.g., for REC.u, REC.P, REC.X use k2i(k), |
---|
349 | % for REC.q use k2i(k,2). |
---|
350 | |
---|
351 | if nargin<2 |
---|
352 | step = 3; |
---|
353 | end |
---|
354 | |
---|
355 | k = k(:)'; |
---|
356 | i = [1:step]'*ones(size(k)) + step*(ones(step,1)*k-1); |
---|
357 | i = i(:); |
---|
358 | return |
---|