#include <vector>
#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;
cv::Mat applyLookUp(const cv::Mat &image, const cv::MatND &lookup)
{
	cv::Mat result(image.rows, image.cols, CV_8U);

	cv::Mat_<uchar>::iterator itr = result.begin<uchar>();
	cv::Mat_<uchar>::const_iterator it = image.begin<uchar>();
	cv::Mat_<uchar>::const_iterator itend = image.end<uchar>();
	for (; it != itend; ++it, ++itr)
	{

		*itr = lookup.at<uchar>(*it);
	}
	return result;
}
int main()
{

    Mat img1 = imread("left.jpg",0);
    Mat img2 = imread("right.jpg",0);
    cv::StereoSGBM sgbm;
    int SADWindowSize = 9;
    sgbm.preFilterCap = 63;
    sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
    int cn = img1.channels();
    int numberOfDisparities=512;
    sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
    sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
    sgbm.minDisparity = 0;
    sgbm.numberOfDisparities = numberOfDisparities;
    sgbm.uniquenessRatio = 10;
    sgbm.speckleWindowSize = 100;
    sgbm.speckleRange = 32;
    sgbm.disp12MaxDiff = 1;
    Mat disp, disp8;
    int64 t = getTickCount();
    sgbm(img1, img2, disp);
    t = getTickCount() - t;
    cout<<"Time elapsed:"<<t*1000/getTickFrequency()<<endl;
    disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));
    int dim(256);
	cv::Mat lut(1,
				&dim,
				CV_8U);

	for (int i = 0; i < 256; i++)
	{

		lut.at<uchar>(i) = 255 - i;
	}

	cv::namedWindow("Negative image",WINDOW_NORMAL);
	cv::imshow("Negative image", applyLookUp(disp8, lut));
    namedWindow("left", WINDOW_NORMAL);
    imshow("left", img1);
    namedWindow("right", WINDOW_NORMAL);
    imshow("right", img2);
    namedWindow("disparity", WINDOW_NORMAL);
    imshow("disparity", disp8);
    waitKey();
    imwrite("sgbm_disparity.png", disp8);   
    cvDestroyAllWindows();
    return 0;
}