Q matrix for the reprojectImageTo3D function in opencv

Question

I am doing a project in opencv to detect obstacle in the path of a blind person using stereo calibration. I have calculated the disparity map correctly. Now to find the distance of obstacle from the camera, I want its 3D coordinates [X,Y,Z] , which I am guessing can be found by reprojectImageTo3D(), but I dont have the Q matrix to use in this function because the Q matrix I am getting from stereoRectify() is coming null probably because I used pre calibrated images. Although I do have the inrinsic and extrinsic parameters of my camera. So my question is that how can I manually create the Q matrix to directly use in the function reprojectImageTo3D(), if I know the focal length, baseline and everything else about my camera? What is the basic format of the Q matrix?

Solution 1

The form of the Q matrix is given as follows:

In that image, c_x and c_y are the coordinates of the principal point in the left camera (if you did stereo matching with the left camera dominant), c'_x is the x-coordinate of the principal point in the right camera (c_x and c'_x will be the same if you specified the CV_CALIB_ZERO_DISPARITY flag for stereoRectify()), f is the focal length and T_x is the baseline length (possibly the negative of the baseline length, it's the translation from one optical centre to the other I think).

I would suggest having a look at the book Learning OpenCV for more information. It's still based on the older C interface, but does a good job of explaining the underlying theory, and is where I sourced the form of the Q matrix from.

Solution 2

if you want to create directly Q matrix:

cv::Mat Q;
Q.at<double>(0,0)=1.0;
Q.at<double>(0,1)=0.0;
Q.at<double>(0,2)=0.0;
Q.at<double>(0,3)=-160; //cx
Q.at<double>(1,0)=0.0;
Q.at<double>(1,1)=1.0;
Q.at<double>(1,2)=0.0;
Q.at<double>(1,3)=-120;  //cy
Q.at<double>(2,0)=0.0;
Q.at<double>(2,1)=0.0;
Q.at<double>(2,2)=0.0;
Q.at<double>(2,3)=348.087;  //Focal
Q.at<double>(3,0)=0.0;
Q.at<double>(3,1)=0.0;
Q.at<double>(3,2)=1.0/95;    //1.0/BaseLine
Q.at<double>(3,3)=0.0;    //cx - cx'

But you should calibrate both cameras and then get Q matrix from cv::stereoRectify. Be carefull, read Q matrix as double values.