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