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;
}
}
}
}