[37] | 1 | % [P,X] = qPXbundle_cmp(P0,X0,q) Bundle adjustment. |
---|
| 2 | % |
---|
| 3 | % P0 ... size (3*K,4), initial camera matrices, K = #images |
---|
| 4 | % X0 ... size (4,N), initial scene points, N = #points |
---|
| 5 | % q ... size (2*K,N), measured image points (in inhomogeneous coordinates) |
---|
| 6 | % P, X ... bundled cameras and scene points, same size as P0, X0 |
---|
| 7 | |
---|
| 8 | % (c) Tom Werner 2001 |
---|
| 9 | |
---|
| 10 | function [P,X,radial] = qPXbundle_cmp(P0,X0,q) |
---|
| 11 | |
---|
| 12 | RADIAL = (nargin > 3); % don't estimate/estimate radial distortion |
---|
| 13 | [K,N] = size(q); K = K/2; |
---|
| 14 | |
---|
| 15 | P0 = normP(P0); |
---|
| 16 | X0 = normx(X0); |
---|
| 17 | |
---|
| 18 | % form observation vector y |
---|
| 19 | qq = reshape(q,[2 K*N]); |
---|
| 20 | aux.qivis = reshape(all(~isnan(qq)),[K N]); % visible image points |
---|
| 21 | qq = qq(:,aux.qivis); |
---|
| 22 | y = qq(:); |
---|
| 23 | |
---|
| 24 | % Choose matrices TP,TX, describing local coordinate systems tangent to P0(:), X0. |
---|
| 25 | % Having homogeneous N-vector X, in the neighborhood of X0 it can be parameterized (minimaly) |
---|
| 26 | % by inhomogeneous (N-1)-vector iX as X = X0 + TX*iX, where TX is a non-singular (N,N-1) matrix. |
---|
| 27 | |
---|
| 28 | for n = 1:N |
---|
| 29 | [qX,dummy] = qr(X0(:,n)); |
---|
| 30 | aux.TX{n} = qX(2:end,:)'; |
---|
| 31 | end |
---|
| 32 | for k = 1:K |
---|
| 33 | [qP,dummy] = qr(reshape(P0(k2idx(k),:),[12 1])); |
---|
| 34 | aux.TP{k} = qP(2:end,:)'; |
---|
| 35 | end |
---|
| 36 | |
---|
| 37 | % form initial parameter vector p0 |
---|
| 38 | p0 = []; |
---|
| 39 | for k = 1:K |
---|
| 40 | p0 = [p0; zeros(11,1)]; |
---|
| 41 | if RADIAL |
---|
| 42 | p0 = [p0; radial(k).u0; radial(k).kappa]; |
---|
| 43 | end |
---|
| 44 | end |
---|
| 45 | p0 = [p0; zeros(3*N,1)]; |
---|
| 46 | |
---|
| 47 | % fill auxiliary structure for evaluating F(p) |
---|
| 48 | aux.P0 = P0; |
---|
| 49 | aux.X0 = X0; |
---|
| 50 | aux.RADIAL = RADIAL; |
---|
| 51 | |
---|
| 52 | % Minimize || F(p) - y || over p. By Newton/Levenber-Marquardt. |
---|
| 53 | p = p0; |
---|
| 54 | lastFp = +Inf; |
---|
| 55 | stepy = +Inf; |
---|
| 56 | lam = .001; |
---|
| 57 | fail_cnt = 0; |
---|
| 58 | [Fp,J] = F(p,aux); |
---|
| 59 | fprintf(' res (rms/max): %.10g / %.10g\n',sqrt(mean((y-Fp).^2)),max(abs(y-Fp))); |
---|
| 60 | while (stepy > 100*eps) & (fail_cnt < 20) |
---|
| 61 | D = (J'*J + lam*speye(size(J,2))) \ (J'*(y-Fp)); |
---|
| 62 | FpD = F(p+D,aux); |
---|
| 63 | if sum((y-Fp).^2) > sum((y-FpD).^2) |
---|
| 64 | p = p + D; |
---|
| 65 | lam = max(lam/10,1e-9); |
---|
| 66 | stepy = max(abs(Fp-lastFp)); lastFp = Fp; |
---|
| 67 | [Fp,J] = F(p,aux); |
---|
| 68 | fail_cnt = 0; |
---|
| 69 | fprintf([' res (rms/max), max_res_step, lam: %.10g / %.10g '... |
---|
| 70 | '%.10g %g\n'],sqrt(mean((y-Fp).^2)),max(abs(y-Fp)),stepy,lam); |
---|
| 71 | %figure(3); plot(abs(y-Fp)); drawnow |
---|
| 72 | else |
---|
| 73 | lam = min(lam*10,1e5); |
---|
| 74 | fail_cnt = fail_cnt + 1; |
---|
| 75 | end |
---|
| 76 | stepy_DanielMartinec = .00005; % .005 0.1 .001 |
---|
| 77 | if stepy < stepy_DanielMartinec |
---|
| 78 | disp(['!!! ended by condition stepy < ' num2str(stepy_DanielMartinec) ... |
---|
| 79 | '. Code modified by Daniel Martinec !!!']); |
---|
| 80 | break; |
---|
| 81 | end |
---|
| 82 | end |
---|
| 83 | |
---|
| 84 | % rearrange computed parameter vector p to P, X |
---|
| 85 | for k = 1:K |
---|
| 86 | P(k2idx(k),:) = (P0(k2idx(k),:) + reshape(aux.TP{k}*p([1:11]+(k-1)*(11+RADIAL*3)),[3 4])); |
---|
| 87 | if RADIAL |
---|
| 88 | radial(k).u0 = p([12:13]+(k-1)*(11+RADIAL*3)); |
---|
| 89 | radial(k).kappa = p([14]+(k-1)*(11+RADIAL*3)); |
---|
| 90 | end |
---|
| 91 | end |
---|
| 92 | for n = 1:N |
---|
| 93 | X(:,n) = X0(:,n) + aux.TX{n}*p((11+RADIAL*3)*K+[1:3]+(n-1)*3); |
---|
| 94 | end |
---|
| 95 | |
---|
| 96 | return |
---|
| 97 | |
---|
| 98 | |
---|
| 99 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 100 | |
---|
| 101 | |
---|
| 102 | %% |
---|
| 103 | % function F: (Np,1) --> (Ny,1)x(Ny,Np), [y,J]=F(p), value y and jacobian J=dF(p)/dp of the function. |
---|
| 104 | % |
---|
| 105 | % 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)], |
---|
| 106 | % where Np = (11+2+raddeg)*K + 3*N |
---|
| 107 | % aux ... auxiliary structure: |
---|
| 108 | % aux.raddeg, aux.P0, aux.X0 ... see above |
---|
| 109 | % aux.TP{1:K} ... size (12,11), Pk(:) = P0k+ aux.TP{k}*iPk |
---|
| 110 | % aux.TX{1:N} ... size (4,3), Xn = X0n + aux.TX{n}*iXn |
---|
| 111 | % aux.qivis ... size (K,N), has 1 in entries corresponding to visible image points |
---|
| 112 | % y ... size (2*nnz(aux.qivis),1), visible image observations, stacked as [q(1,1); q(2,1); .. q(2*K,N)], unobserved q are omitted |
---|
| 113 | %% |
---|
| 114 | % We consider the imaging model as |
---|
| 115 | % u = e2p(x) |
---|
| 116 | % x = P*X |
---|
| 117 | % P = P0 + TP*iP |
---|
| 118 | % X = X0 + TX*iX |
---|
| 119 | %% |
---|
| 120 | function [y,J] = F(p,aux) |
---|
| 121 | % |
---|
| 122 | [K,N] = size(aux.qivis); |
---|
| 123 | NR = aux.RADIAL*3; % number of radial distortion paramters |
---|
| 124 | % |
---|
| 125 | % rearrange p to P, X [,radial] |
---|
| 126 | for k = 1:K |
---|
| 127 | P(k2idx(k),:) = aux.P0(k2idx(k),:) + reshape(aux.TP{k}*p([1:11]+(k-1)*(11+NR)),[3 4]); |
---|
| 128 | if aux.RADIAL |
---|
| 129 | radial(k).u0 = p([12:13]+(k-1)*(11+NR)); |
---|
| 130 | radial(k).kappa = p([14]+(k-1)*(11+NR)); |
---|
| 131 | end |
---|
| 132 | end |
---|
| 133 | X = zeros(4,N); |
---|
| 134 | for n = 1:N |
---|
| 135 | X(:,n) = aux.TX{n}*p((11+NR)*K+[1:3]+(n-1)*3); |
---|
| 136 | end |
---|
| 137 | X = aux.X0 + X; |
---|
| 138 | % |
---|
| 139 | % compute retina points q |
---|
| 140 | for k = 1:K |
---|
| 141 | x(k2idx(k),:) = P(k2idx(k),:)*X; |
---|
| 142 | q(k2idx(k,2),:) = p2e(x(k2idx(k),:)); |
---|
| 143 | if aux.RADIAL |
---|
| 144 | q(k2idx(k,2),:) = raddist(q(k2idx(k,2),:),radial(k).u0,radial(k).kappa); |
---|
| 145 | end |
---|
| 146 | end |
---|
| 147 | qq = reshape(q,[2 K*N]); |
---|
| 148 | qq = qq(:,aux.qivis); |
---|
| 149 | y = qq(:); % function value |
---|
| 150 | % |
---|
| 151 | % compute Jacobian J = dF(p)/dp |
---|
| 152 | if nargout<2, return, end |
---|
| 153 | [kvis,nvis] = find(aux.qivis); |
---|
| 154 | Ji = zeros(2*length(kvis)*(11+NR+3),1); Jj = zeros(size(Ji)); Jv = zeros(size(Ji)); |
---|
| 155 | cnt = 0; |
---|
| 156 | for l = 1:length(kvis) % loop for all VISIBLE points in all cameras |
---|
| 157 | k = kvis(l); |
---|
| 158 | n = nvis(l); |
---|
| 159 | xl = x(k2idx(k),n); |
---|
| 160 | ul = p2e(xl); |
---|
| 161 | |
---|
| 162 | % Compute derivatives (Jacobians). Notation: E.g., dudx = du(x)/dx, etc. |
---|
| 163 | dxdP = kron(X(:,n)',eye(3))*aux.TP{k}; % dx(iP,iX)/diP |
---|
| 164 | dxdX = P(k2idx(k),:)*aux.TX{n}; % dx(iP,iX)/diX |
---|
| 165 | dudx = [eye(2) -ul]/xl(3); |
---|
| 166 | if aux.RADIAL |
---|
| 167 | [dqdu,dqdu0,dqdkappa] = raddist(ul,radial(k).u0,radial(k).kappa); |
---|
| 168 | else |
---|
| 169 | dqdu = eye(2); dqdu0 = []; dqdkappa = []; |
---|
| 170 | end |
---|
| 171 | dqdP = dqdu*dudx*dxdP; % dq(iP,iX)/diP |
---|
| 172 | dqdX = dqdu*dudx*dxdX; % dq(iP,iX)/dX |
---|
| 173 | |
---|
| 174 | c = cnt+[1:2*(11+NR+3)]; |
---|
| 175 | [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]); |
---|
| 176 | cnt = cnt + 2*(11+NR+3); |
---|
| 177 | |
---|
| 178 | end |
---|
| 179 | J = sparse(Ji,Jj,Jv); |
---|
| 180 | return |
---|
| 181 | |
---|
| 182 | |
---|
| 183 | function [i,j,v] = spidx(I,J,V) |
---|
| 184 | % |
---|
| 185 | i = I'*ones(1,length(J)); |
---|
| 186 | j = ones(length(I),1)*J; |
---|
| 187 | i = i(:); |
---|
| 188 | j = j(:); |
---|
| 189 | v = V(:); |
---|
| 190 | return |
---|
| 191 | |
---|
| 192 | |
---|
| 193 | function P = normP(P) |
---|
| 194 | for k = 1:size(P,1)/3 |
---|
| 195 | Pk = P(k2idx(k),:); |
---|
| 196 | P(k2idx(k),:) = Pk/sqrt(sum(sum(Pk.*Pk))); |
---|
| 197 | end |
---|
| 198 | return |
---|
| 199 | |
---|
| 200 | |
---|
| 201 | function x = normx(x) |
---|
| 202 | x = x./(ones(size(x,1),1)*sqrt(sum(x.*x))); |
---|
| 203 | return |
---|