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