OpenCV - compute real distance from disparity map

Question

I have computed disparity map using StereoSGBM algorithm in OpenCV 3.1. I have calibrated stereo camera with small RMS error. And now I want to compute real distance in mm for some points in disparity map.

This seems to be relatively easy problem. From what I understand I can simply use formula

distance = (baseline * focal length) / disparity

where I can use matrix Q (output from stereoRectify). Q[2][3] = focal length, 1/Q[3][2] = baseline.

The computed Q matrix is:

Q: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 
    1., 0., 0., -1.5668458938598633e+02, 
    0., 1., 0., -1.1948609733581543e+02, 
    0., 0., 0., 2.3598119491957863e+02, 
    0., 0., 1.6254073321947445e-02, 0. ]

The problem is that the result does not correspond with the reality. For example, for the camera aiming to the room ceiling in distance approx. 2,5 metres (where the disparity is correctly computed as 12), the real distance is computed as 1,3 m. For very close objects (eg 30 cm), it seems to be correct, but far objects are very incorrect. During the calibration, I specified the exact size of chessboard square in millimetres.

I do exactly the following:

// compute rectification transforms from calibration data
stereoRectify(M1, D1, M2, D2, Size(FRAME_WIDTH, FRAME_HEIGHT), R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 0.0, Size(FRAME_WIDTH, FRAME_HEIGHT), &roi1, &roi2);

// compute the undistortion and rectification transformation maps for each camera
initUndistortRectifyMap(M1, D1, R1, P1, Size(FRAME_WIDTH, FRAME_HEIGHT), CV_16SC2, map11, map12);
initUndistortRectifyMap(M2, D2, R2, P2, Size(FRAME_WIDTH, FRAME_HEIGHT), CV_16SC2, map21, map22);

...

// get images from camera (VideoCapture)
camLeft.read(_frameLeft);
camRight.read(_frameRight);

// remap images using the calibration data
remap(_frameLeft, frameLeft, map11, map12, INTER_LINEAR);
remap(_frameRight, frameRight, map21, map22, INTER_LINEAR);

// compute disparity from undistorted images
stereo->compute(frameLeft, frameRight, disparityMap);

...

// compute the real-world distance [mm]
float fMaxDistance = static_cast<float>((1. / Q.at<double>(3, 2)) * Q.at<double>(2, 3));

// outputDisparityValue is single 16-bit value from disparityMap
// DISP_SCALE = 16
float fDisparity = outputDisparityValue / (float)StereoMatcher::DISP_SCALE;
float fDistance = fMaxDistance / fDisparity;

Is there something I do incorrectly? Thanks in advance.

Answer 1

I know that the stereo algorithms for disparity in OpenCV 2.4 (stereoBM, stereoSGBM) give the disparity as values 16 times greater the real values, as reported in the documentation . I am not used with C++, openCv3 and I don't find the disparity method SGBM specified in your code, but I think it could be the same thing. Try to divide every disparity value by 16 (again, this is surely true for OpenCV 2.4, I don't know 3.0 version)