Revision history [back]

const double hFOV = 62.85 * (CV_PI / 180.0);
const double vFOV = 47.13 * (CV_PI / 180.0);
const double baseline = 3.0;
const double depthConst = baseline / (2.0*tan(hFOV/2.0));
const double depthFactorW = 2.0 * tan(hFOV/2.0);
const double depthFactorH = 2.0 * tan(vFOV/2.0);

Mat zVals = Mat::zeros(sizeIn, CV_32FC1);
for(int i = 0; i < sizeIn.height; i++) {
    for(int j = 0; j < sizeIn.width; j++) {
        if (disparityFrame.disparity16.at<short>(i,j) > 0) {

            float z = (float) -(((float) depthConst) * (((float) sizeIn.width) / ((float) (disparityFrame.disparity16.at<short>(i,j)))));
            z = z * 16.0f;

            zVals.at<float>(i,j) = z;           

            //fullDepthImage.at<Vec3f>(i,j)[2] = z;
            //fullDepthImage.at<Vec3f>(i,j)[0] = -((j - (sizeIn.width  / 2.0))/sizeIn.width)  * depthFactorW * z;
            //fullDepthImage.at<Vec3f>(i,j)[1] =  ((i - (sizeIn.height / 2.0))/sizeIn.height) * depthFactorH * z;
        }               
    }
}

This assumes that your image center is the optical center and that your disparity values are scaled by 16. But you could adjust accordingly.The units I used were were cm.