Project depth image to floor

edit

Hi!

Im looking for a fast way of projecting an depth image to a floor plane. I have a Mat that is 320*240 which contain range data, I know the normal to what is the floor plane in the image. And I want to transform that into an image representing a new viewpoint (the image is taken parallel to the floor and I want to transform it so that it looks like if the camera were facing the floor). Hope you understand what I want to do... Now, at this point I´m going through every point in the Mat and calculates 2D-coordinates for the new image and increase that value with 1 (faceing a wall will result in a bright line). And the actual question is: is there a faster way? Or maybe some build in funtion that could speed things up? Tanks for any help!! /Eric

        float viewDistance = 3500;
        float XtoZ = 1.12213;//tan(xtionFOV.fHFOV*0.5f)*2.0f;
        float YtoZ = 0.8416; //tan(xtionFOV.fVFOV*0.5f)*2.0f;

        float X_rw;
        float Y_rw; 
        float Z_rw; 
        for(int j=0;j<g_frameHeight;j++)
        {
            for(int i=0;i<g_frameWidth;i++)
            {   
                if(depth16New.at<unsigned short>(j,i)>0 && depth16New.at<unsigned short>(j,i)<viewDistance)
                {
                    X_rw = ( (float)i / (float)g_frameWidth - 0.5f ) * (float)depth16New.at<unsigned short>(j,i) * XtoZ;
                    Y_rw = ( 0.5f - (float)j / (float)g_frameHeight ) * (float)depth16New.at<unsigned short>(j,i) * YtoZ;
                    Z_rw = (float)depth16New.at<unsigned short>(j,i);
                    float ifZeroIsOnFloor = floorCoords.vNormal.X*(X_rw-floorCoords.ptPoint.X) + 
                                        floorCoords.vNormal.Y*(Y_rw-floorCoords.ptPoint.Y) + 
                                        floorCoords.vNormal.Z*(Z_rw-floorCoords.ptPoint.Z);
                    if(ifZeroIsOnFloor>50) //ignoring floor points
                    {
                        Z_rw = Z_rw*floorCoords.vNormal.Y - Y_rw*floorCoords.vNormal.Z;
                        int row = (int)(g_frameHeight - (g_frameHeight*Z_rw)/viewDistance);
                        int col = (int)(X_rw / ((viewDistance*tan(29.0f*PI/180.0f))/(g_frameWidth*0.5f)) + g_frameWidth*0.5f);
                        if(row < g_frameHeight && row > -1 && col < g_frameWidth && col > -1)
                            proj.at<unsigned short>(row,col) += 1;
                    }
                }
            }
        }