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