#include <iostream>
#include <fstream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/gpu/gpu.hpp>

using namespace std;
using namespace cv;
using namespace cv::gpu;

int main(int argc, char **argv)
{

   resetDevice();

   string sImg1="left.png";
   string sImg2="right.png";
   string sOut="disp.png";
   int lNumDisparities=144;

   Mat img_left = imread(sImg1, IMREAD_GRAYSCALE);
   Mat img_right = imread(sImg2, IMREAD_GRAYSCALE);
   GpuMat imgLeftGPU(img_left);
   GpuMat imgRightGPU(img_right);
   GpuMat imgDispGPU(img_left.size(), CV_16SC1);

   int ndisp; int iters; int levels; int nr_plane;
   gpu::StereoConstantSpaceBP::estimateRecommendedParams(img_left.cols,img_left.rows,ndisp,iters,levels,nr_plane);
   gpu::StereoConstantSpaceBP bp(lNumDisparities,iters,levels,nr_plane,CV_16SC1);
   bp.use_local_init_data_cost=false;
   bp(imgLeftGPU,imgRightGPU,imgDispGPU);

   Mat imgDisp(img_left.size(), CV_16SC1);
   imgDispGPU.download(imgDisp);
   imwrite(sOut,imgDisp);
   return 0;
}