Getting Real Depth Help

[deadfire55]

Hi,

I am trying to use libfreenect to detect 30cm rocks on the ground, the Kinect will be aimed down at a known angle and the depth information will be converted into a numerical 0-9 (0 being the floor, 9 being a rock) value and put into a text document.

Problem is, it doesn't seem to be consistently working correctly, if anyone could help me out, it would be greatly appreciated.

I started with the cppview.cpp file as a base and build off of that. So I take a 'snapshot' when the '1' key is pressed:

if (key == '1'){

    static std::vector<uint8_t> depth(640*480*4);
    device->updateState();
    device->getDepth(depth); 
    printf("Taking picture\n");
    info->fWidth = 640;
    info->fHeight = 480;
    readIntoHeightmap(depth, Map, info); //takes the value of depth and transforms it, problem in here
    Map->saveHeightMap(); // prints out the vector into a text file

}

So now that I have the depth values in depth, I take those over into readIntoHeightmap() and for each element of depth, I call getDepth(), which is where I suspect everything is going wrong.

void readIntoHeightmap(vector<uint8_t> pDepth, HeightMap *pHeightMap, Data *pInfo){

int depth;

float xAngle;

float yAngle;

float pitch;

float yaw;

int division = 100;

int DX;

int DY;

int DZ;

 

for(int x = 0;x < pInfo->fWidth;x+=1){//going 0-640

for(int y = 0;y < pInfo->fHeight;y+=1){//going 0-480

depth = getDepth(x, y, pDepth); //guessing this is where the problem is

getAngles(x, y, &xAngle, &yAngle, pInfo);

yaw = (xAngle + pInfo->orientation)*pi/180;

pitch = (yAngle + pInfo->startPitch)*pi/180;

DX = depth*sin(pitch)*cos(yaw)/division;

DY = depth*sin(pitch)*sin(yaw)/division;

DZ = depth*cos(pitch)/division;

if(pHeightMap->getCoordinateValue(x/division,y/division) < pInfo->kinectHeight + DZ){

pHeightMap->setCoordinateValue(pInfo->xPosition + DX, pInfo->yPosition + DY, pInfo->kinectHeight + DZ);

}

}

}

}

This is what I am mostly unsure about:

int getDepth(int x, int y, std::vector<uint8_t> pDepth){ 

//the RGB are store sequentially in memory (eg. depth[0] = red, depth[1] = green, depth[2] = blue)

int redComponent = pDepth[3*(x*480) + y+ 0];

int greenComponent = pDepth[3*(x*480) + y +1];

int blueComponent = pDepth[3*(x*480) + y +2];

int hexcolor = (redComponent << 16) + (greenComponent << 8) + blueComponent; //converts the RGB to a unique hex color

cout << "this: " << hexcolor/160000 << endl;

return hexcolor/160000; //should give me millimeter vales, doesn't

}

Any help is appreciated since we are lost right now, thanks.