% [X,lambda] = compute3DStructure(p, q, R, T) % p - first image coordinates % q - second image coordinates % R, T - displacement between the views % X - 3D coordinates of the points with respect to 1st and 2nd view % lambda - scales with respect to the first view (lambda x = X) % The basic linear triangulation algorithm % for recovering the depth of a point given the projection onto % two images and the relative pose of the cameras, % as described in Chapter 5, "An introduction to 3-D Vision" % by Y. Ma, S. Soatto, J. Kosecka, S. Sastry (MASKS) % % Code distributed free for non-commercial use % Copyright (c) MASKS, 2003 % % Last modified 5/5/2003 function [XP, lambda] = compute3DStructure(p, q, R, T); nc = size(q, 2); % linear triangulation method M = []; for i=1:nc A = [ 0 -1 p(2,i) 0; -1 0 p(1,i) 0; (-R(2,:) + q(2,i)*R(3,:)) -T(2) + q(2,i)*T(3); (-R(1,:) + q(1,i)*R(3,:)) -T(1) + q(1,i)*T(3)]; [ua,sa,va] = svd(A); X(:,i) = va(:,4); end XP(:,:,1) = [X(1,:)./X(4,:);X(2,:)./X(4,:);X(3,:)./X(4,:); X(4,:)./X(4,:)]; lambda = XP(3,:,1); XP(:,:,2) = [R, T; 0 0 0 1]*XP(:,:,1);