function v = getxy(Ptilde,mtilde,z)
%
% getxy: calculates x,y intersection of ray at range z
% On input:
%     Ptilde: perspective transform matrix
%     mtilde: 2D projective image point
%     z: 3D range
% On output:
%     v: [x,y,z]' vector
% Call:
%     v = getxy(Ptilde,mtilde,z)
%

R = inv(Ptilde(:,1:3));
lamda = (z-(-R(3,:)*Ptilde(:,4)))/(R(3,:)*mtilde);
x = R(1,:)*(-Ptilde(:,4) + lamda*mtilde);
y = R(2,:)*(-Ptilde(:,4) + lamda*mtilde);
v = [x,y,z]';
