1 | %vgg_PX_from_6pts_3img Computes camera matrices and world points from 6 points across 3 images.
|
---|
2 | %
|
---|
3 | % [P,X] = vgg_PX_from_6pts_3img(x), where
|
---|
4 | % x ... double(3,6,3) or cell{3} of double(3,6), 6 homogeneous points in 3 images
|
---|
5 | % P ... double(3,4,3), P(:,:,k) is k-th camera matrix
|
---|
6 | % X ... double(4,6), homogeneous world points
|
---|
7 | % There are 0 to 3 solutions for (P,X). Solutions are pruned by requirement that
|
---|
8 | % scalars s in all projective equations s*x==P*X are positive.
|
---|
9 | % In case of multiple solutions, P and X have one dimension
|
---|
10 | % more such that P(:,:,:,n) and X(:,:,n) is the n-th solution.
|
---|
11 | %
|
---|
12 | % Also the form [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3) is accepted.
|
---|
13 |
|
---|
14 | % Algorithm in Hartley-Zisserman, Alg 19.1 page 493 in 1st edition,
|
---|
15 | Alg 20.1 page 511 in 2nd edition
|
---|
16 | % Coded by werner@robots.ox.ac.uk, Nov 2002.
|
---|
17 |
|
---|
18 | function [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3)
|
---|
19 |
|
---|
20 | if nargin==3
|
---|
21 | x = cat(3,x1,x2,x3);
|
---|
22 | else
|
---|
23 | if iscell(x1)
|
---|
24 | x = cat(3,x1{:});
|
---|
25 | else
|
---|
26 | x = x1;
|
---|
27 | end
|
---|
28 | end
|
---|
29 | if any(size(x)~=[3 6 3])
|
---|
30 | error('Wrong size of input points.');
|
---|
31 | end
|
---|
32 |
|
---|
33 | for k = 1:3
|
---|
34 | % Find homographies H_k mapping first 4 pts in each image to standard projective basis.
|
---|
35 | % Now, x(:,1:4,k) = H(:,:,k)*[eye(3) [1;1;1]].
|
---|
36 | H(:,:,k) = H_from_4x(x(:,3:6,k));
|
---|
37 |
|
---|
38 | % Form transformed points xs(:,1:2,k)
|
---|
39 | xs(:,:,k) = inv(H(:,:,k))*x(:,:,k);
|
---|
40 | end
|
---|
41 |
|
---|
42 | % Compute dual fundamental matrix
|
---|
43 | Fd = Fdual_from_x(xs(:,1:2,:));
|
---|
44 |
|
---|
45 | % Retrieve (non-dual) cameras P and world points X from (each solution for) dual fund. matrix.
|
---|
46 | P = [];
|
---|
47 | X = [];
|
---|
48 | for i = 1:size(Fd,3)
|
---|
49 |
|
---|
50 | % Compute canonical Xi and Pi
|
---|
51 | Xi = X_from_Fdual(Fd(:,:,i));
|
---|
52 | Pi = P_from_Xx_canonical(Xi,xs);
|
---|
53 |
|
---|
54 | % return from canonical to original image bases
|
---|
55 | for k = 1:3
|
---|
56 | Pi(:,:,k) = H(:,:,k)*Pi(:,:,k);
|
---|
57 | end
|
---|
58 |
|
---|
59 | % Compute signs of P and X, if possible. If impossible, Pi==Xi==[].
|
---|
60 | [Pi,Xi] = vgg_signsPX_from_x(Pi,Xi,x);
|
---|
61 | if isempty(Pi), continue, end
|
---|
62 | %for k = 1:3, Pi(:,:,k)*Xi ./ x(:,:,k), end % test code
|
---|
63 |
|
---|
64 | P = cat(4,P,Pi);
|
---|
65 | X = cat(3,X,Xi);
|
---|
66 |
|
---|
67 | end
|
---|
68 |
|
---|
69 | return
|
---|
70 |
|
---|
71 |
|
---|
72 | %%%%%%%%%%%%%%%% auxiliary functions
|
---|
73 |
|
---|
74 |
|
---|
75 | % Solve for dual fundamental matrix.
|
---|
76 | function F = Fdual_from_x(x)
|
---|
77 |
|
---|
78 | % Linear step. After that,
|
---|
79 | % for k=1:3 and i=1:2, it is x(:,1,k)'*F_i*x(:,2,k)==0.
|
---|
80 | A = [];
|
---|
81 | for k = 1:3
|
---|
82 | x1 = x(1,1,k); y1 = x(2,1,k); z1 = x(3,1,k);
|
---|
83 | x2 = x(1,2,k); y2 = x(2,2,k); z2 = x(3,2,k);
|
---|
84 | A = [A; [x1*y2-z1*y2,...
|
---|
85 | x1*z2-z1*y2,...
|
---|
86 | y1*x2-z1*y2,...
|
---|
87 | y1*z2-z1*y2,...
|
---|
88 | z1*x2-z1*y2] ];
|
---|
89 | end
|
---|
90 | [u,s,v] = svd(A,0);
|
---|
91 | v1 = v(:,end-1);
|
---|
92 | v2 = v(:,end);
|
---|
93 | FF{1} = [0 v1(1:2)'; v1(3) 0 v1(4); v1(5) -sum(v1) 0];
|
---|
94 | FF{2} = [0 v2(1:2)'; v2(3) 0 v2(4); v2(5) -sum(v2) 0];
|
---|
95 |
|
---|
96 | % Non-linear step. Find linear combination of F_i having zero determinant.
|
---|
97 | % Dual fund. matrix is now F = a*FF{1} + (1-a)*FF{2}, for each element of a.
|
---|
98 | a = vgg_singF_from_FF(FF);
|
---|
99 | for i = 1:length(a)
|
---|
100 | F(:,:,i) = a(i)*FF{1} + (1-a(i))*FF{2};
|
---|
101 | end
|
---|
102 | return
|
---|
103 |
|
---|
104 |
|
---|
105 | % Given dual fundamental matrix Fd, computes cameras P and world points X such that
|
---|
106 | % for each k, P(:,:,k)*X ~ x(:,:,k).
|
---|
107 | function X = X_from_Fdual(Fd)
|
---|
108 |
|
---|
109 | % Retrieve second dual camera matrix Pd2.
|
---|
110 | % It is done by considering Fdual in form
|
---|
111 | %[0 b*(d-c) -c*(d-b)
|
---|
112 | % -a*(d-c) 0 c*(d-a)
|
---|
113 | % a*(d-b) -b*(d-a) 0].
|
---|
114 |
|
---|
115 | Fd_aux = reshape( Fd([4 7 1 2 1 8 1 3 6]), [3 3] );
|
---|
116 | [u,s,v] = svd(Fd_aux,0);
|
---|
117 | ABC = v(:,3); % It is a:b:c = A:B:C
|
---|
118 | [u,s,v] = svd(Fd',0);
|
---|
119 | KLM = v(:,3); % It is K:L:M = ((d-a):(d-b):(d-c)
|
---|
120 |
|
---|
121 | G = [0 -ABC(3) ABC(2) 0
|
---|
122 | ABC(3) 0 -ABC(1) 0
|
---|
123 | -ABC(2) ABC(1) 0 0
|
---|
124 | KLM(2) -KLM(1) 0 KLM(1)-KLM(2)
|
---|
125 | 0 KLM(3) -KLM(2) KLM(2)-KLM(3)
|
---|
126 | -KLM(3) 0 KLM(1) KLM(3)-KLM(1) ];
|
---|
127 | [u,s,v] = svd(G,0);
|
---|
128 | abcd = v(:,end);
|
---|
129 |
|
---|
130 | % The test code:
|
---|
131 | %Pd1 = [eye(3) [1;1;1]];
|
---|
132 | %Pd2 = [diag(abcd(1:3)) [1;1;1]*abcd(4)];
|
---|
133 | %vgg_F_from_P(Pd1,Pd2) ./ Fd
|
---|
134 |
|
---|
135 | % Retrieve 6 world points X(:,1:6)
|
---|
136 | X = [ abcd [1;1;1;1] eye(4) ];
|
---|
137 |
|
---|
138 | return
|
---|
139 |
|
---|
140 |
|
---|
141 | % Computes camera P from world points X and image points x, everything in canonical form.
|
---|
142 | function P = P_from_Xx_canonical(X,x)
|
---|
143 | X = X(:,1);
|
---|
144 | for k = 1:3
|
---|
145 | A = [contreps(x(:,1,k))*[X(1) 0 0 X(4)
|
---|
146 | 0 X(2) 0 X(4)
|
---|
147 | 0 0 X(3) X(4)]
|
---|
148 | contreps(x(:,2,k))*[eye(3) [1;1;1]]];
|
---|
149 | [u,s,v] = svd(A,0);
|
---|
150 | a = v(:,end);
|
---|
151 | P(:,:,k) = [a(1) 0 0 a(4)
|
---|
152 | 0 a(2) 0 a(4)
|
---|
153 | 0 0 a(3) a(4)];
|
---|
154 | end
|
---|
155 | return
|
---|
156 |
|
---|
157 |
|
---|
158 | % A bit faster vgg_contreps.
|
---|
159 | function X = contreps(x)
|
---|
160 | X = [0 x(3) -x(2)
|
---|
161 | -x(3) 0 x(1)
|
---|
162 | x(2) -x(1) 0];
|
---|
163 | return
|
---|
164 |
|
---|
165 |
|
---|
166 | % H_from_4x Having four point matches, the homography relating them is given by
|
---|
167 | % H = H_from_4x(x2)*inv(H_from_4x(x1)).
|
---|
168 | function H = H_from_4x(x)
|
---|
169 | H = x(:,1:3) * diag(inv(x(:,1:3))*x(:,4));
|
---|
170 | return
|
---|
171 |
|
---|
172 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
173 |
|
---|
174 |
|
---|
175 | % Test code:
|
---|
176 | P = randn(3,4,3); X = randn(4,6);
|
---|
177 | for k = 1:3, x(:,:,k) = P(:,:,k)*X; end
|
---|
178 | [Pn,Xn] = PX_from_6pts(x);
|
---|
179 | for n = 1:size(Pn,4)
|
---|
180 | for k=1:3, Pn(:,:,k,n)*Xn(:,:,n) ./ x(:,:,k), end
|
---|
181 | end
|
---|