[37] | 1 | %vgg_X_from_xP_nonlin Estimation of 3D point from image matches and camera matrices, nonlinear.
|
---|
| 2 | % X = vgg_X_from_xP_lin(x,P,imsize) computes max. likelihood estimate of projective
|
---|
| 3 | % 3D point X (column 4-vector) from its projections in K images x (3-by-K matrix)
|
---|
| 4 | % and camera matrices P (K-cell of 3-by-4 matrices). Image sizes imsize (2-by-K matrix)
|
---|
| 5 | % are needed for preconditioning.
|
---|
| 6 | % By minimizing reprojection error, Newton iterations.
|
---|
| 7 | %
|
---|
| 8 | % X = vgg_X_from_xP_lin(x,P,imsize,X0) takes initial estimate of X. If X0 is omitted,
|
---|
| 9 | % it is computed by linear algorithm.
|
---|
| 10 | %
|
---|
| 11 | % See also vgg_X_from_xP_lin.
|
---|
| 12 |
|
---|
| 13 | % werner@robots.ox.ac.uk, 2003
|
---|
| 14 |
|
---|
| 15 | function X = X_from_uP_nonlin(u,P,imsize,X)
|
---|
| 16 |
|
---|
| 17 | if iscell(P)
|
---|
| 18 | P = cat(3,P{:});
|
---|
| 19 | end
|
---|
| 20 | K = size(P,3);
|
---|
| 21 | if K < 2
|
---|
| 22 | error('Cannot reconstruct 3D from 1 image');
|
---|
| 23 | end
|
---|
| 24 |
|
---|
| 25 | if nargin<4
|
---|
| 26 | X = vgg_X_from_xP_lin(u,P,imsize);
|
---|
| 27 | end
|
---|
| 28 |
|
---|
| 29 | % precondition
|
---|
| 30 | imsize = mean(imsize,2);
|
---|
| 31 | f = 4/sum(imsize);
|
---|
| 32 | H = [f 0 -f*imsize(1)/2
|
---|
| 33 | 0 f -f*imsize(2)/2
|
---|
| 34 | 0 0 1 ];
|
---|
| 35 | for k = 1:K
|
---|
| 36 | P(:,:,k) = H*P(:,:,k);
|
---|
| 37 | end
|
---|
| 38 | u = homu(H,u);
|
---|
| 39 |
|
---|
| 40 | % Parametrize X such that X = T*[Y;1]; thus x = P*T*[Y;1] = Q*[Y;1]
|
---|
| 41 | [dummy,dummy,T] = svd(X',0);
|
---|
| 42 | T = T(:,[2:end 1]);
|
---|
| 43 | for k = 1:K
|
---|
| 44 | Q(:,:,k) = P(:,:,k)*T;
|
---|
| 45 | end
|
---|
| 46 |
|
---|
| 47 | % Newton
|
---|
| 48 | Y = [0;0;0];
|
---|
| 49 | eprev = inf;
|
---|
| 50 | for n = 1:10
|
---|
| 51 | [e,J] = resid(Y,u,Q);
|
---|
| 52 | if 1-norm(e)/norm(eprev) < 1000*eps
|
---|
| 53 | break
|
---|
| 54 | end
|
---|
| 55 | eprev = e;
|
---|
| 56 | Y = Y - (J'*J)\(J'*e);
|
---|
| 57 | end
|
---|
| 58 |
|
---|
| 59 | X = T*[Y;1];
|
---|
| 60 |
|
---|
| 61 | return
|
---|
| 62 |
|
---|
| 63 | %%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
| 64 |
|
---|
| 65 | function [e,J] = resid(Y,u,Q)
|
---|
| 66 | K = size(Q,3);
|
---|
| 67 | e = [];
|
---|
| 68 | J = [];
|
---|
| 69 | for k = 1:K
|
---|
| 70 | q = Q(:,1:3,k);
|
---|
| 71 | x0 = Q(:,4,k);
|
---|
| 72 | x = q*Y + x0;
|
---|
| 73 | e = [e; x(1:2)/x(3)-u(:,k)];
|
---|
| 74 | J = [J; [x(3)*q(1,:)-x(1)*q(3,:)
|
---|
| 75 | x(3)*q(2,:)-x(2)*q(3,:)]/x(3)^2];
|
---|
| 76 | end
|
---|
| 77 | return
|
---|