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