[37] | 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 |
---|