Calculate distance (disparity) OpenCV

Odrai picture Odrai · Jun 13, 2013 · Viewed 17.2k times · Source

-- Update 2 --

The following article is really useful (although it is using Python instead of C++) if you are using a single camera to calculate the distance: Find distance from camera to object/marker using Python and OpenCV

Best link is Stereo Webcam Depth Detection. The implementation of this open source project is really clear.

Below is the original question.


For my project I am using two camera's (stereo vision) to track objects and to calculate the distance. I calibrated them with the sample code of OpenCV and generated a disparity map.

I already implemented a method to track objects based on color (this generates a threshold image).

My question: How can I calculate the distance to the tracked colored objects using the disparity map/ matrix?

Below you can find a code snippet that gets the x,y and z coordinates of each pixel. The question: Is Point.z in cm, pixels, mm?

Can I get the distance to the tracked object with this code?

Thank you in advance!

cvReprojectImageTo3D(disparity, Image3D, _Q);

vector<CvPoint3D32f> PointArray;
CvPoint3D32f Point;

for (int y = 0; y < Image3D->rows; y++) {    

    float *data = (float *)(Image3D->data.ptr + y * Image3D->step);  

    for (int x = 0; x < Image3D->cols * 3; x = x + 3) 
    {   
        Point.x = data[x];        
        Point.y =  data[x+1];     
        Point.z = data[x+2];
        PointArray.push_back(Point);
        //Depth > 10
        if(Point.z > 10)
        {
            printf("%f %f %f", Point.x, Point.y, Point.z);              
        }
    }
}
cvReleaseMat(&Image3D);

--Update 1--

For example I generated this thresholded image (of the left camera). I almost have the same of the right camera.

enter image description here

Besides the above threshold image, the application generates a disparity map. How can I get the Z-coordinates of the pixels of the hand in the disparity map?

I actually want to get all the Z-coordinates of the pixels of the hand to calculate the average Z-value (distance) (using the disparity map).