title: OpenCV - compute real distance from disparity map
body: <p>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.</p>
  
  <p>This seems to be relatively easy problem. From what I understand I can simply use formula</p>
  
  <pre><code>distance = (baseline * focal length) / disparity
  </code></pre>
  
  <p>where I can use matrix Q (output from stereoRectify). Q[2][3] = focal length, 1/Q[3][2] = baseline.</p>
  
  <p>The computed Q matrix is:</p>
  
  <pre><code>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. ]
  </code></pre>
  
  <p>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 (e.g. 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.</p>
  
  <p>I do exactly the following:</p>
  
  <pre><code>// 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), &amp;roi1, &amp;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-&gt;compute(frameLeft, frameRight, disparityMap);
  
  ...
  
  // compute the real-world distance [mm]
  float fMaxDistance = static_cast&lt;float&gt;((1. / Q.at&lt;double&gt;(3, 2)) * Q.at&lt;double&gt;(2, 3));
  
  // outputDisparityValue is single 16-bit value from disparityMap
  // DISP_SCALE = 16
  float fDisparity = outputDisparityValue / (float)StereoMatcher::DISP_SCALE;
  float fDistance = fMaxDistance / fDisparity;
  </code></pre>
  
  <p>Is there something I do incorrectly?
  Thanks in advance.</p>