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

asked 2018-09-05 04:50:26 -0500

astronaut gravatar image

updated 2018-09-05 04:53:34 -0500

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[1];
        }

        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[1];
        }

        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[2];
    //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[2];

    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[0]);
                    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 ...
(more)
edit retag flag offensive close merge delete

Comments

try something as https://github.com/LaurentBerger/Trai... Structure is here

LBerger gravatar imageLBerger ( 2018-09-05 06:07:29 -0500 )edit

ok. thanks. the real distance measured with laser to the chessboard from the camera is 7.9 m. the focal length is 450 pixels. with that code i dont get good results. also disparity values are too big. Any help?

astronaut gravatar imageastronaut ( 2018-09-06 04:56:05 -0500 )edit