# How to find the depth (distance from the camera to the object) in m from disparity map, knowing all calibration parameters ?

Hi

Im using two point Grey Chameleon3 mono camera set up as Master Salve and synchronized so can work as stereo camera. Then was following the OpenCV tutorial to calibrate the cameras. I got 70 samples for the calibration and got the Calibration results. I have all Camera parameters (intrinsic and extrinsic) and have access to them.

So I took the left and right images, undistort them using the camera matrix and distortion coefficients, and rectify them into epipolar form using the translation and rotation matrices. Then used use calib3d module's StereoSGBM class to create a disparity map. Here is my code

#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <math.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d.hpp>

using namespace cv;
using namespace std;

int main( int argc, char** argv )
{
int i, j, lr;

String imageName_left( "/home/OpenCVCode/left_7m.pgm" ); // by default
if( argc > 1)
{
imageName_left = argv;
}

Mat image_left;
image_left = imread( imageName_left, IMREAD_COLOR ); // Read the file
if( image_left.empty() )                      // Check for invalid input
{
cout <<  "Could not open or find the image" << std::endl ;
return -1;
}

String imageName_right( "/home/OpenCVCode/right_7m.pgm" ); // by default
if( argc > 1)
{
imageName_right = argv;
}

Mat image_right;
image_right = imread( imageName_right, IMREAD_COLOR ); // Read the file
if( image_right.empty() )                      // Check for invalid input
{
cout <<  "Could not open or find the image" << std::endl ;
return -1;
}
cv::Size imageSize;
imageSize = image_left.size();
//Mat outputImage = image.clone();

vector<cv::Point3f> lines;
//double err = 0;

Mat R1=Mat1d(3, 3);
Mat R2=Mat1d(3, 3);
Mat P1=Mat1d(3, 4);
Mat P2=Mat1d(3, 4);
Mat Q=Mat1d(4, 4);
Rect validRoi;

double R_data[] = {0.9997286422545486, 0.000835337139108146, -0.023279692174526096,
0.0008925647184101439, 0.9998880281815514, -0.00244797956076857,
0.02327756826002481, 0.0024680939144444804, 0.9997259941245548};
double T_data[] = {-0.13808630092854335, -0.0016795993989732654, -0.005964101318274714};

cv::Mat R(3, 3, CV_64F, R_data);
cv::Mat T(3, 1, CV_64F, T_data);

Mat Camera_Matrix_Left = (Mat1d(3, 3) << 446.441726, 0, 266.768096, 0, 448.805499, 186.652251, 0, 0, 1);
Mat Distortion_Coefficients_Left = (Mat1d(1, 5) << -0.174502, 0.079787, 0.001010, 0.000899, 0);
Mat Camera_Matrix_Right = (Mat1d(3, 3) << 440.825465, 0, 277.733688, 0, 442.324307, 198.863384, 0, 0, 1);
Mat Distortion_Coefficients_Right = (Mat1d(1, 5) << -0.226564, 0.290791, 0.000979, 0.003149, 0);

Mat Matrix_R = (Mat1d(3, 3) << 0.9997286422545486, 0.000835337139108146, -0.023279692174526096,
0.0008925647184101439, 0.9998880281815514, -0.00244797956076857,
0.02327756826002481, 0.0024680939144444804, 0.9997259941245548);
Mat Matrix_T = (Mat1d(3, 1) << -0.13808630092854335, -0.0016795993989732654, -0.005964101318274714);

//undistort(image, outputImage, Camera_Matrix, Distortion_Coefficients);
stereoRectify(Camera_Matrix_Left, Distortion_Coefficients_Left, Camera_Matrix_Right, Distortion_Coefficients_Right, image_left.size(), R, T, R1, R2, P1, P2, Q, 0, 1, imageSize, &validRoi);
FileStorage fs1("4m2.yml", FileStorage::WRITE);
fs1 << "R1" << R1;
fs1 << "R2" << R2;
fs1 << "P1" << P1;
fs1 << "P2" << P2;
fs1 << "Q" << Q;
fs1.release();

// Maps for ...
edit retag close merge delete