Ask Your Question

Project depth image to floor

asked 2012-12-19 07:30:21 -0500

manssone gravatar image


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(<unsigned short>(j,i)>0 &&<unsigned short>(j,i)<viewDistance)
                    X_rw = ( (float)i / (float)g_frameWidth - 0.5f ) * (float)<unsigned short>(j,i) * XtoZ;
                    Y_rw = ( 0.5f - (float)j / (float)g_frameHeight ) * (float)<unsigned short>(j,i) * YtoZ;
                    Z_rw = (float)<unsigned short>(j,i);
                    float ifZeroIsOnFloor = floorCoords.vNormal.X*(X_rw-floorCoords.ptPoint.X) + 
                                        floorCoords.vNormal.Y*(Y_rw-floorCoords.ptPoint.Y) + 
                    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)
                  <unsigned short>(row,col) += 1;
edit retag flag offensive close merge delete

1 answer

Sort by » oldest newest most voted

answered 2012-12-19 08:24:18 -0500

manssone gravatar image

updated 2020-05-04 04:16:03 -0500

supra56 gravatar image

I just notice that<unsigned short>(row,col) += 1;, the last row in the loop is the big issue. if i remove that the time spent here goes from 90ms to something that I cant even measure. Any other way to increase a value in a Mat? gonna use it for HoughLines after the loop.

edit flag offensive delete link more

Question Tools


Asked: 2012-12-19 07:30:21 -0500

Seen: 437 times

Last updated: Dec 19 '12