1 | % uP2X ... linear reconstruction of 3D points |
---|
2 | % from N-perspective views |
---|
3 | % |
---|
4 | % X = uP2X(Umat,Pmat); |
---|
5 | % Umat ... 3*N x n matrix of n homogenous points |
---|
6 | % Ps ... 3 x 4*N matrix of projection matrices |
---|
7 | % |
---|
8 | % X ... 4 x n matrix of homogenous 3D points |
---|
9 | % |
---|
10 | % Algorithm is based on: Hartley and Zisserman, Multiple |
---|
11 | % View Geometry, 2000, pp 297-298 |
---|
12 | % |
---|
13 | % $Id: uP2X.m,v 2.0 2003/06/19 12:07:10 svoboda Exp $ |
---|
14 | |
---|
15 | function X = uP2X(Umat,Ps); |
---|
16 | |
---|
17 | N = size(Umat,1)/3; |
---|
18 | n = size(Umat,2); |
---|
19 | |
---|
20 | % reshuffle the Ps matrix |
---|
21 | Pmat = []; |
---|
22 | for i=1:N; |
---|
23 | Pmat = [Pmat;Ps(:,i*4-3:i*4)]; |
---|
24 | end |
---|
25 | |
---|
26 | X = []; |
---|
27 | for i=1:n, % for all points |
---|
28 | A = []; |
---|
29 | for j=1:N, % for all cameras |
---|
30 | % create the data matrix |
---|
31 | A = [A; Umat(j*3-2,i)*Pmat(j*3,:) - Pmat(j*3-2,:); Umat(j*3-1,i)*Pmat(j*3,:) - Pmat(j*3-1,:)]; |
---|
32 | end |
---|
33 | [u,s,v] = svd(A); |
---|
34 | X = [X,v(:,end)]; |
---|
35 | end |
---|
36 | % normalize reconstructed points |
---|
37 | X = X./repmat(X(4,:),4,1); |
---|
38 | |
---|
39 | return; |
---|