1 | % vgg_line3d_from_lP_nonlin Non-linear estimation of (possibly constrained) 3D line segment from image line segments.
|
---|
2 | %
|
---|
3 | % SYNOPSIS
|
---|
4 | % L = vgg_line3d_from_lP_nonlin(s,P [,imsize] [,L0] [,X] [,nonlin_opt])
|
---|
5 | %
|
---|
6 | % s ... cell(K) of double(3,3), inv. covariance matrices of the K image line segments:-
|
---|
7 | % - If the segments are estimated from edges, it is s(:,k) = x*x',
|
---|
8 | % where x (3-by-N) are homog. coordinates of the edgels with last components 1.
|
---|
9 | % - If only end points are available, s(:,k) = d*x*y' where x, y (column 2-vectors)
|
---|
10 | % are the segment's end points and d its length.
|
---|
11 | %
|
---|
12 | % P ... K-cell with 3-by-4 camera matrices
|
---|
13 | %
|
---|
14 | % imsize ... size (2,K), image size(s) for preconditiong.
|
---|
15 | % Omit if s and P are already preconditioned.
|
---|
16 | %
|
---|
17 | % L0 ... double(4,2), initial scene line (optional). Homogeneous points L0(:,i) span
|
---|
18 | % the line. If omitted, linear estimation is done first.
|
---|
19 | %
|
---|
20 | % X ... constraint on L :-
|
---|
21 | % - if X is omitted: no constraint on L
|
---|
22 | % - if X is double(4,1): L goes thru point X
|
---|
23 | % - if X is double(4,2): L goes thru 3D line spanned by X(:,i)
|
---|
24 | %
|
---|
25 | % nonlin_opt ... options for Levenberg-Marquardt. It is comma-separated list
|
---|
26 | % of pairs ['option',value]. Possible options are :-
|
---|
27 | % opt ... options structure with possible fields :-
|
---|
28 | % - verbose ... 1 or 0 (default: 0)
|
---|
29 | % - niter_term ... maximum number of iterations
|
---|
30 | % - rmsstep_term ... terminating step of rms of residuals
|
---|
31 | % - lambda_term ... terminating value of lambda (default: 1e10)
|
---|
32 | % - lambda_init ... initial value of lambda
|
---|
33 | % E.g., vgg_line3d_from_lP_nonlin(...,'lambda_init',1e-9,'niter_term',5).
|
---|
34 | %
|
---|
35 | % L ... double(4,2), estimated 3D line. Points L(:,i) span the line.
|
---|
36 | %
|
---|
37 | % Note: use [] if you want to omit a parameter and use a later one, e.g.
|
---|
38 | % vgg_line3d_from_lP_nonlin(s,P,imsize,[],[],'verbose',1,'lam_init',1e-9)
|
---|
39 | %
|
---|
40 | % ALGORITHM
|
---|
41 | % - Minimization is done by Levenberg-Marquardt.
|
---|
42 | % - 3D line L is parameterized by image lines in the first two images.
|
---|
43 | % The positions of these image lines are possibly constrained by X.
|
---|
44 |
|
---|
45 | % T.Werner, Feb 2002
|
---|
46 |
|
---|
47 | function L = vgg_line3d_from_lP_nonlin(s,P,imsize,L,X,varargin)
|
---|
48 |
|
---|
49 | if nargin < 3, imsize = []; end
|
---|
50 | if nargin < 4, L = []; end
|
---|
51 | if nargin < 5, X = []; end
|
---|
52 |
|
---|
53 | if isempty(L)
|
---|
54 | L = vgg_line3d_from_lP_lin(s,P,imsize);
|
---|
55 | end
|
---|
56 | K = length(P); % number of images
|
---|
57 | if K<2
|
---|
58 | error('Cannot reconstruct 3D line from 1 image');
|
---|
59 | end
|
---|
60 | if isempty(X) & K==2 % no need for non-linear minimization
|
---|
61 | return
|
---|
62 | end
|
---|
63 |
|
---|
64 | % Prepare square root of covariance matrices; now s{k}(:,n) has meaning of 3 homogeneous image points
|
---|
65 | for k = 1:K
|
---|
66 | [us,ss,vs] = svd(s{k},0);
|
---|
67 | s{k} = us*sqrt(ss);
|
---|
68 | end
|
---|
69 |
|
---|
70 | % Preconditioning
|
---|
71 | if ~isempty(imsize)
|
---|
72 | for k = 1:K
|
---|
73 | H = vgg_conditioner_from_image(imsize(:,k));
|
---|
74 | P{k} = H*P{k};
|
---|
75 | s{k} = H*s{k};
|
---|
76 | scale(k) = H(1,1); % save the scales for evaluating objective function
|
---|
77 | end
|
---|
78 | else
|
---|
79 | scale = ones(1,K);
|
---|
80 | end
|
---|
81 |
|
---|
82 | switch size(X,2)
|
---|
83 | case 0
|
---|
84 | % Scene line L is unconstrained, having thus 4 DOF.
|
---|
85 | % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows:
|
---|
86 | % l1 = l0(1,:) + p(1:2)'*ldelta{1}
|
---|
87 | % l2 = l0(2,:) + p(3:4)'*ldelta{2}
|
---|
88 | % where row 4-vector p represents 4 DOF of L.
|
---|
89 | for k = 1:2
|
---|
90 | l0(k,:) = normx(vgg_wedge(P{k}*L)')';
|
---|
91 | ldelta{k} = null(l0(k,:))';
|
---|
92 | end
|
---|
93 |
|
---|
94 | % optimization
|
---|
95 | p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},...
|
---|
96 | @normsolve,...
|
---|
97 | [0;0;0;0],...
|
---|
98 | varargin{:});
|
---|
99 | l = l12_from_p(p,l0,ldelta);
|
---|
100 |
|
---|
101 | case 1
|
---|
102 | % Scene line L is constrained to intersect the scene point X, having thus 2 DOF.
|
---|
103 | % L is parameterized by two image lines in images 1 and 2, each having 1 DOF, as follows:
|
---|
104 | % l1 = l0(1,:) + p(1)*ldelta{1}
|
---|
105 | % l2 = l0(2,:) + p(2)*ldelta{2}
|
---|
106 | % where 2-vector p represents 2 DOF of L.
|
---|
107 | for k = 1:2
|
---|
108 | l0(k,:) = normx(vgg_wedge(P{k}*L)')';
|
---|
109 | x = P{k}*X;
|
---|
110 |
|
---|
111 | % Since L might not intersect X, move l0(k,:) 'as little as possible' to intersect x.
|
---|
112 | l0(k,:) = l0(k,:) - (l0(k,:)*x)/(x'*x).*x';
|
---|
113 |
|
---|
114 | Q = null(x')';
|
---|
115 | ldelta{k} = null(l0(k,:)*pinv(Q))'*Q;
|
---|
116 | end
|
---|
117 |
|
---|
118 | % optimization
|
---|
119 | p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},...
|
---|
120 | @normsolve,...
|
---|
121 | [0;0],...
|
---|
122 | varargin{:});
|
---|
123 | l = l12_from_p(p,l0,ldelta);
|
---|
124 |
|
---|
125 | case 2
|
---|
126 | % Scene line L is constrained to intersect the scene line given by 2 points X.
|
---|
127 | % This constraint is given by
|
---|
128 | % l1*G*l2' = 0
|
---|
129 | % where G is 3x3 rank 2 matrix (analogical in fact to fundamental matrix) and
|
---|
130 | % G = P{1}*M*P{2}'
|
---|
131 | % where M is Pluecker matrix of line given by X, M = X(:,1)*X(:,2)'-X(:,2)*X(:,1).
|
---|
132 | %
|
---|
133 | % This constraint can be written as
|
---|
134 | % p(1:2)*D*p(3:4)' + p(1:2)*d{2}' + d{1}*p(3:4)' + c = 0
|
---|
135 | % where D, d, c are given below and 4-vector p are 4 parameters of L.
|
---|
136 | %
|
---|
137 | % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows:
|
---|
138 | % l1 = l0(1,:) + p(1:2)'*ldelta{1}
|
---|
139 | % l2 = l0(2,:) + p(3:4)'*ldelta{2}
|
---|
140 | % where p(1:3) are chosen freely and p(4) is computed from the above formula as
|
---|
141 | % p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2)).
|
---|
142 | Lpm = X(:,1)*X(:,2)' - X(:,2)*X(:,1)';
|
---|
143 | G = P{1}*Lpm*P{2}';
|
---|
144 | for k = 1:2
|
---|
145 | l0(k,:) = normx(vgg_wedge(P{k}*L)')';
|
---|
146 | end
|
---|
147 |
|
---|
148 | % As L might not intersect line X, move l0(2,:) 'as little as possible' to enforce l0(1,:)*G*l0(2,:)'==0.
|
---|
149 | x = (l0(1,:)*G)';
|
---|
150 | l0(2,:) = l0(2,:) - (l0(2,:)*x)/(x'*x).*x';
|
---|
151 |
|
---|
152 | for k = 1:2
|
---|
153 | ldelta{k} = null(l0(k,:))';
|
---|
154 | end
|
---|
155 |
|
---|
156 | D = ldelta{1}*G*ldelta{2}';
|
---|
157 | d{1} = ldelta{1}*G*l0(2,:)';
|
---|
158 | d{2} = ldelta{2}*G'*l0(1,:)';
|
---|
159 | c = l0(1,:)*G*l0(2,:)';
|
---|
160 |
|
---|
161 | % optimization
|
---|
162 | p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta,D,d,c},...
|
---|
163 | @normsolve,...
|
---|
164 | [0;0;0],...
|
---|
165 | varargin{:});
|
---|
166 | l = l12_from_p(p,l0,ldelta,D,d,c);
|
---|
167 |
|
---|
168 | end
|
---|
169 | if all(~isnan(l(:)))
|
---|
170 | L = null([l(1,:)*P{1}; l(2,:)*P{2}]);
|
---|
171 | end
|
---|
172 |
|
---|
173 | return
|
---|
174 |
|
---|
175 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% objective function
|
---|
176 |
|
---|
177 |
|
---|
178 | % Objective function of Levenberg-Marquardt
|
---|
179 | function [y,w,J] = F(p,P,s,scale,varargin)
|
---|
180 | K = length(s);
|
---|
181 |
|
---|
182 | % l := lines in images 1, 2 from p
|
---|
183 | l = l12_from_p(p,varargin{:});
|
---|
184 |
|
---|
185 | % l := reprojection of lines in images 1, 2 to all images
|
---|
186 | if all(abs(p) < inf)
|
---|
187 | [dummy,dummy,L]= svd([l(1,:)*P(1:3,:); l(2,:)*P(4:6,:)],0);
|
---|
188 | else
|
---|
189 | L = inf*ones(4);
|
---|
190 | end
|
---|
191 | l = norml(vgg_wedge(reshape(P*L(:,3),[3 K]),reshape(P*L(:,4),[3 K])));
|
---|
192 |
|
---|
193 | % compute residual function
|
---|
194 | y = [];
|
---|
195 | for k = 1:K
|
---|
196 | y = [y l(k,:)*s{k}];
|
---|
197 | end
|
---|
198 | y = y';
|
---|
199 |
|
---|
200 | w = [1;1;1] * (1./scale);
|
---|
201 | w = w(:);
|
---|
202 |
|
---|
203 | % else, compute also jacobian
|
---|
204 | if nargout < 2
|
---|
205 | return
|
---|
206 | end
|
---|
207 | dif = 1e-6;
|
---|
208 | J = zeros(length(y),length(p));
|
---|
209 | for i = 1:length(p)
|
---|
210 | pdif = p;
|
---|
211 | pdif(i) = pdif(i) + dif;
|
---|
212 | J(:,i) = (F(pdif,P,s,scale,varargin{:}) - y)/dif;
|
---|
213 | end
|
---|
214 | return
|
---|
215 |
|
---|
216 |
|
---|
217 | % The following function computes lines in the first two images
|
---|
218 | % from parameters p. Explanation see above.
|
---|
219 | function l = l12_from_p(p,l0,ldelta,D,d,c)
|
---|
220 | switch length(p)
|
---|
221 | case 4 % unconstrained
|
---|
222 | l = [l0(1,:) + p(1:2)'*ldelta{1}
|
---|
223 | l0(2,:) + p(3:4)'*ldelta{2}];
|
---|
224 | case 2 % going thru X
|
---|
225 | l = [l0(1,:) + p(1)*ldelta{1}
|
---|
226 | l0(2,:) + p(2)*ldelta{2}];
|
---|
227 | case 3 % going thru L
|
---|
228 | p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2));
|
---|
229 | l = [l0(1,:) + p(1:2)'*ldelta{1}
|
---|
230 | l0(2,:) + p(3:4)'*ldelta{2}];
|
---|
231 | end
|
---|
232 | return
|
---|
233 |
|
---|
234 |
|
---|
235 | function dp = normsolve(J,Y,w,lambda)
|
---|
236 | OLDWARN = warning('off');
|
---|
237 | dp = -( J'*diag(w)*J + lambda*eye(size(J,2)) ) \ ( J'*(Y.*w) );
|
---|
238 | warning(OLDWARN);
|
---|
239 | return
|
---|
240 |
|
---|
241 | function x = normx(x)
|
---|
242 | if ~isempty(x)
|
---|
243 | x = x./(ones(size(x,1),1)*sqrt(sum(x.*x)));
|
---|
244 | end
|
---|
245 |
|
---|
246 | function l = norml(l)
|
---|
247 | % l = norml(l) Multiplies hyperplane l by scalar so that for each n, norm(l(1:end-1,n))==1.
|
---|
248 | l = l./(sqrt(sum(l(:,1:end-1).^2,2))*ones(1,size(l,2)));
|
---|
249 |
|
---|
250 |
|
---|
251 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
252 |
|
---|
253 | % a = levmarq(@RES,PARAMS,@NORMSOLVE,a [,opt]) Non-linear least-squares by Levenberg-Marquardt.
|
---|
254 | %
|
---|
255 | % Minimizes f(a)'*W*f(a) over a.
|
---|
256 | %
|
---|
257 | % @RES ... residual function f called like [e,w,J] = RES(a,PARAMS{:}), where
|
---|
258 | % - a ... double(M,1), parameter vector
|
---|
259 | % - e ... double(N,1), residual vector
|
---|
260 | % - J ... double(N,M), derivative of e wrt a
|
---|
261 | % - w ... double(N,1), weights of e; covariance matrix of e is diag(1/e.^2).
|
---|
262 | % Use 1 instead of ones(N,1).
|
---|
263 | % For efficiency, RES should not compute the jacobian if called with two output parameters only.
|
---|
264 | %
|
---|
265 | % @NORMSOLVE ... function solving normal equations, called like da = NORMSOLVE(J,e,W,lambda).
|
---|
266 | % a ... initial parameter vector
|
---|
267 | % opt ... options structure, see code
|
---|
268 |
|
---|
269 | function [a,w] = levmarq(RES,PARAMS,NORMSOLVE,a0,varargin)
|
---|
270 |
|
---|
271 | % options
|
---|
272 | [opt,rem_opt] = vgg_argparse( { 'niter_term', +inf,...
|
---|
273 | 'drmsrel_term', 0,...
|
---|
274 | 'loglambda_term', 6,...
|
---|
275 | 'loglambda_init', -4,...
|
---|
276 | 'verbose', 0 },...
|
---|
277 | varargin );
|
---|
278 | if ~isempty(rem_opt), if ~isempty(fieldnames(rem_opt))
|
---|
279 | error(['Unknown option(s) ' fieldnames(rem_opt)]);
|
---|
280 | end, end
|
---|
281 |
|
---|
282 |
|
---|
283 | % Initial statistics
|
---|
284 | if opt.verbose
|
---|
285 | [e0,w0] = feval(RES,a0,PARAMS{:});
|
---|
286 | ssd0 = sum( (e0.*w0).^2 );
|
---|
287 | fprintf( ' [rms=%14.12g] [maxabs=%14.12g]\n',...
|
---|
288 | sqrt(ssd0/length(e0)),...
|
---|
289 | max(abs(e0.*w0)) );
|
---|
290 | end
|
---|
291 |
|
---|
292 |
|
---|
293 | loglambda = opt.loglambda_init;
|
---|
294 | niter = 0;
|
---|
295 | while 1
|
---|
296 |
|
---|
297 | % Compute actual residual and jacobian
|
---|
298 | [e0,w0,J] = feval(RES,a0,PARAMS{:});
|
---|
299 |
|
---|
300 | % Update a as a := a0 + da, by finding
|
---|
301 | % optimal lambda and solving normal equations for da.
|
---|
302 | nfail = 1;
|
---|
303 | while (loglambda < opt.loglambda_term)
|
---|
304 | niter = niter + 1;
|
---|
305 |
|
---|
306 | a = a0 + feval(NORMSOLVE,J,e0,w0,10^loglambda);
|
---|
307 | [e,w] = feval(RES,a,PARAMS{:});
|
---|
308 |
|
---|
309 | if sum((e.*w).^2) < sum((e0.*w0).^2) % success
|
---|
310 | a0 = a;
|
---|
311 | loglambda = loglambda - 1;
|
---|
312 | break
|
---|
313 | end
|
---|
314 |
|
---|
315 | if opt.verbose
|
---|
316 | fprintf('%4i.%.2i: [loglambda=%3i] [REJECTED]\n',niter,nfail,loglambda);
|
---|
317 | end
|
---|
318 |
|
---|
319 | loglambda = loglambda + 1;
|
---|
320 | nfail = nfail + 1;
|
---|
321 | end
|
---|
322 |
|
---|
323 | % Print statistic after successful iteration
|
---|
324 | ssd0 = sum( (e0.*w0).^2 );
|
---|
325 | ssd = sum( (e.*w).^2 );
|
---|
326 | if opt.verbose
|
---|
327 | fprintf( '%4i : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%4g%%]\n',...
|
---|
328 | niter,...
|
---|
329 | round(loglambda),...
|
---|
330 | sqrt(ssd/length(e)),...
|
---|
331 | max(abs(e.*w)),...
|
---|
332 | 100*(1-sqrt(ssd/ssd0)) );
|
---|
333 | end
|
---|
334 |
|
---|
335 | % Termination criteria
|
---|
336 | test(1) = loglambda < opt.loglambda_term;
|
---|
337 | test(2) = ssd0-ssd >= opt.drmsrel_term^2*ssd0;
|
---|
338 | test(3) = niter < opt.niter_term;
|
---|
339 | if any(test==0)
|
---|
340 | break
|
---|
341 | end
|
---|
342 | end
|
---|
343 |
|
---|
344 | if opt.verbose
|
---|
345 | onoff = {'YES','no'};
|
---|
346 | fprintf( ' Levenberg-Marquardt finished succesfully.\n Reason for termination:\n' );
|
---|
347 | fprintf( ' lambda = %s\n', onoff{test(1)+1} );
|
---|
348 | fprintf( ' drmsrel = %s\n', onoff{test(2)+1} );
|
---|
349 | fprintf( ' niter = %s\n', onoff{test(3)+1} );
|
---|
350 | end
|
---|
351 |
|
---|
352 | return
|
---|
353 |
|
---|
354 |
|
---|
355 |
|
---|
356 | function print_statistics(niter,loglambda,e0,w0,e,w,opt)
|
---|
357 | if opt.verbose
|
---|
358 | ssd0 = sum( (e0.*w0).^2 );
|
---|
359 | ssd = sum( (e.*w).^2 );
|
---|
360 | fprintf( '%4i : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%11.5g]\n',...
|
---|
361 | niter,...
|
---|
362 | round(loglambda),...
|
---|
363 | sqrt(ssd/length(e)),...
|
---|
364 | max(abs(e.*w)),...
|
---|
365 | sqrt(1-ssd/ssd0) );
|
---|
366 | end
|
---|
367 | return
|
---|
368 |
|
---|