plot pixels 3D - Kinect depth image

Hello, It has been so long that had not touche Matlab.

For a project I'm working with the Kinect sensor and i must determinate the orientation of the 3d head of the user relative to the sensor. I did extract the depth image directly on matlab using IMAC Kinect toolbox , so i get the depth Map and i save it a the matrix "depth". this matrix contains only the Z pixels (distance in the space )

To plot the 3D image with matlab i have to calculate the X and Y of each Pixel Z using the following equations:

X = Z*2tan(fovH/2)*Xp/Xres

Y = Z*2tan(fovV/2)*Yp/Yres

where: - fovV and fovH are the horizontal and vertical field of vision of KINECTS (in radians)

- Xres and Yres: are the the horizontal and vertical resolution of the depth map

Xp and Yp the coordinates of the pixel explained in this figure: http://ensiwiki.ensimag.fr/images/0/0b/Shema_projection.jpg

the problem is, i don't know how to calculate the Xp and the Yp.

From the figure, i understood that Xp and Yp are distances between the Pixel and the horizontal and vertical borders of the plan where the pixel is.

this is the peace of code i wrote:

          % clear all;
  clc;
  load('depth ahead.mat');
  %m = imshow(depth);
  n = depth(70:180 ,280:420);  %centration in the region of the head
  imshow(n);
  [x,y] = meshgrid(1:size(n,2),1:size(n,1));
  fovH = 57;
  fovV = 43;
  Xres = size(depth,2);
  Yres = size(depth,1);

for i=1:1:size(n,1)

       for j=1:1:size(n,2)

           if (n(i,j)<1113)||(n(i,j)>1500)

                   n(i,j)=NaN;

           end
       end
  end

figure;
plot3(x,y,n, 'b')

I get something like this: http://www.hostingpics.net/viewer.php?id=525703fig4.png

or i should get a result like this: http://www.developpez.net/forums/attachment.php?attachmentid=98099&d=1341073756

i will appreciate any help. thanks in advance