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 |
---|