Depth / Ranged Image to Point Cloud

Question

I am trying to create a pointcloud from a depth image / ranged image / displacement map (grey values which correspond to z depth).

Here is the depth image of an trailer vehicle: http://de.tinypic.com/r/rlvp80/8

Camera Parameters, no rotation: x,y,z: 0,25,0

I get this pointcloud with a aslope ground plane, which should be even (just a quad at 0,0,0 with no rotation): rotated: http://de.tinypic.com/r/2mnglj6/8

Here is my code, I try to do this:

normalizedCameraRay = normalize(CameraRay); Point_in_3D = zDepthValueOfPixelXY /normalizedCameraRay.zValue; //zValue is <= 1

This doesnt seem to work. If I use zDepthValueOfPixelXY * normalizedCameraRay, I got Radial Disortion (everything looks a bit like a sphere).

Any Ideas how to fix this? I attached my code.

Thank you for your ideas

float startPositionX = -((float)image.rows)/2;
float startPositionY = -((float)image.cols)/2;


glm::vec3 cameraWorldPosition(x,y,z);

qDebug() << x << "," << y << ", " << z;

for(int a = 0; a < image.rows;a++)
{
    for(int b = 0; b < image.cols;b++)
    {
        float movementX = startPositionX+b;
        float movementY = startPositionY+a;

        glm::vec3 ray(movementX, movementY, focal_length);

        ray = glm::normalize(ray);
        float rayFactor =  ((float)image.at<u_int16_t>(a,b))  / ray[2];


        glm::vec3 completeRay = rayFactor*ray;
        pointCloud[a][b] = completeRay;
        /*
        ray = completeRay - cameraWorldPosition;
        ray = glm::normalize(ray);
        rayFactor =  ((float)image.at<u_int16_t>(a,b))  / ray[2];
        ray = rayFactor * ray;
        pointCloud[a][b] = ray;*/


        /*
        ray = glm::vec3(startPositionX+b,startPositionY-a, image.at<u_int16_t>(a,b));
        pointCloud[a][b] = ray;
        */



    }
}