Stereo map using Post-filtering

Question

I tried using the code to find the disparity given in the link: link
However the disparity map appears to be wrong in some areas. Some objects far from the camera appear to be brighter than the closer objects. I tried to calculate the actual depth by multiplying the disparity value by calibration matrix Q. The depth calculated are way off from the real world measured values(off by 20-25 cm). I am confident that the matrix Q is correct since my rectified image seems to be good. My square size value for calibration was also accurate (0.05 meters). My disparity images are attached:

This is the added code for calculating the actual depth from the filtered disparity image stored in filtered_disp_vis.

fs1["Q"] >> Q;
Mat Image;
Mat V = Mat(4, 1, CV_64FC1);
Mat pos = Mat(4, 1, CV_64FC1);
vector< Point3d > points;
//float fMaxDistance = static_cast<float>((1. / Q.at<double>(3, 2)) * 
Q.at<double>(2, 3));
//filtered_disp_vis.convertTo(filtered_disp_vis, CV_64FC1, 1.0 / 16.0, 0.0);
//imshow("filtered disparity", filtered_disp_vis);
// outputDisparityValue is single 16-bit value from disparityMap
// DISP_SCALE = 16
//float fDisparity = outputDisparityValue / 
(float)StereoMatcher::DISP_SCALE;
//float fDistance = fMaxDistance / fDisparity;
reprojectImageTo3D(filtered_disp_vis, Image, Q, false, CV_32F);
//cout << Image;
for (int i = 0; i < filtered_disp_vis.cols; i++)
{
for (int j = 0; j < filtered_disp_vis.rows; j++)
{
int d = filtered_disp_vis.at<uchar>(j, i);
//filtered_disp_vis.convertTo(filtered_disp_vis, CV_32F, 1.0 / 16.0, 0.0);
//int l = img_left.at<uchar>(j, i);
//cout << "(" << j << "," << i << ")" << "=" << d;
//out << endl;
// if low disparity, then ignore
/*if (d < 2) {
continue;
}*/
// V is the vector to be multiplied to Q to get
// the 3D homogenous coordinates of the image point
V.at<double>(0, 0) = (double)(i);
V.at<double>(1, 0) = (double)(j);
V.at<double>(2, 0) = (double)d;
V.at<double>(3, 0) = 1.;
pos = Q * V; // 3D homogeneous coordinate
double X = pos.at<double>(0, 0) / pos.at<double>(3, 0);
double Y = pos.at<double>(1, 0) / pos.at<double>(3, 0);
double Z = pos.at<double>(2, 0) / pos.at<double>(3, 0);

if (i == 446 && j == 362)
{
cout << "(" << j << "," << i << ")" << " =   ";

cout << X << " " << Y << " " << Z << " " << d;
cout << endl;
}

Mat point3d_cam = Mat(3, 1, CV_64FC1);
point3d_cam.at<double>(0, 0) = X;
point3d_cam.at<double>(1, 0) = Y;
point3d_cam.at<double>(2, 0) = Z;
// transform 3D point from camera frame to robot frame
//Mat point3d_robot = XR * point3d_cam + XT;
points.push_back(Point3d(point3d_cam));
}

Where am I going wrong? Any modifications to my snippet or different recommendations to get proper disparity maps with accurate depth values will be appreciated.

ThiagoRTK ThiagoRTK · Accepted Answer · 2017-11-27T01:38:33

I can't add a comment in your question yet (because minimum reputation). So i will comment here:

1 - Have you tried using a pair of frames from the KITTI dataset only with the post-filtering? If so, did the result also go bad?

2 - If with KITTI dataset was ok, so the problem can be with your calibration. After the stereo calibration and rectifying, are you applying undistort (for both cameras calib)?

*undistort:

Second Part (I tried to put it in response to your comment, but my response was too big for the comment field...) =P

Sorry about the delay but I was pretty busy these past few days. So, let's go in parts:

1) I used filtered_disp_vis.

2) If with KITTI is ok, so the problem probably is with your cameras or rectification. What kind of cameras are you using? Are your cameras standing still?

2.1) I used to use simple webcams (Rolling shutter system), but most of the time the results look bad. The ideal for stereo vision are cameras with the Global Shutter system.

3) For different colors I used "applyColorMap" from OpenCV.

Third Part (too long again):

a) Well, looks like this camera sensor is Rolling Shutter, but i'm not sure... Can works, but if you want to keep moving your camera you will have problems. Try not to hold the cameras during calibration, try to put on a surface and let it stop. Here has nice tips about calibration: camera calibration

b) It is not in real time, but it is as fast as the CPU can. I just take the frames with VideoCapture, and while the frames keep coming I move to cv :: Mat inside a "while" loop.

cv::Mat imgOne, imgTwo;

VideoCapture videoOne("path/To/The/Frames/%10d.png");
VideoCapture videoTwo("path/To/The/FramesTwo/%10d.png");

while(1){
    videoOne >> imgOne;
    videoTwo >> imgTwo;

    //rest of code.
}

c) I did not perform a 3D reconstruction.

d) With Vec3b to take a position, and uchar to take values in each channel. I did not perform any extra transformations.

Stereo map using Post-filtering

1 Answers