reprojectImageTo3D() produces discretized point cloud

C++
,
1

I am trying to generate a point cloud from a stereo pair of images from Tsukuba dataset and I am getting a cone shaped/discretized point cloud.

The camera parameters are given with the dataset and I am creating the Q matrix manually without using stereoRectify() as these are stereo images.

I am using StereoSGBM and have tried tuning the parameters. Also, the disparity image gets trimmed from the left side.

Disparity image

Point cloud front view

image
image797×500 47.4 KB

How can I fix/improve this.

Mat left = imread(left_image_path, IMREAD_COLOR); 
Mat right = imread(right_image_path, IMREAD_COLOR);

Mat disparity, disparitynorm_sgbm;
int cn = left.channels();
int wsize = 7, max_disp = 160;

Ptr<StereoSGBM> stereo  = StereoSGBM::create(0,16,3);
           
    stereo->setPreFilterCap(63); 
    stereo->setBlockSize(wsize);
    stereo->setP1(8*cn*wsize*wsize); 
    stereo->setP2(32*cn*wsize*wsize); 
    stereo->setMinDisparity(0); 
    stereo->setNumDisparities(max_disp);
    stereo->setUniquenessRatio(10); 
    stereo->setSpeckleWindowSize(150); 
    stereo->setSpeckleRange(2);
    stereo->setDisp12MaxDiff(10);
    stereo->setMode(StereoSGBM::MODE_SGBM);
            
    stereo->compute(left, right, disparity);
    double minVal; double maxVal;
    minMaxLoc( disparity, &minVal, &maxVal );
    disparity.convertTo( disparitynorm_sgbm, CV_8UC1, 255/(maxVal - minVal));

   cv::Mat PCL, PCL64;
cv::Mat Q(4, 4, CV_64F);

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)=-320.0; //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)=-240.0;  //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)=615.0;  //Focal
Q.at<double>(3,0)=0.0;
Q.at<double>(3,1)=0.0;
Q.at<double>(3,2)=1.0/0.10;    //BaseLine
Q.at<double>(3,3)= 0.0;  //cx - cx'/baseline

reprojectImageTo3D(disparitynorm_sgbm, PCL, Q, true);

PCL.convertTo(PCL64, CV_64F);