int main(int argc, char* argv[]) {
	if (argc < 5) {
		std::cerr << "usage: sgmstereo left right leftOut rightOut" << std::endl;
		exit(1);
	}
    
    std::string leftImageFilename = argv[1];
    std::string rightImageFilename = argv[2];
    
    png::image<png::rgb_pixel> leftImage(leftImageFilename);
	png::image<png::rgb_pixel> rightImage(rightImageFilename);

	SPSStereo sps;
    sps.setIterationTotal(outerIterationTotal, innerIterationTotal);
    sps.setWeightParameter(lambda_pos, lambda_depth, lambda_bou, lambda_smo);
    sps.setInlierThreshold(lambda_d);
    sps.setPenaltyParameter(lambda_hinge, lambda_occ, lambda_pen);
    
	png::image<png::gray_pixel_16> segmentImage;
	png::image<png::gray_pixel_16> disparityImage;
	png::image<png::gray_pixel_16> disparityImageRight;
	std::vector< std::vector<double> > disparityPlaneParameters;
	std::vector< std::vector<int> > boundaryLabels;
	sps.compute(superpixelTotal, leftImage, rightImage, segmentImage, disparityImage, disparityImageRight, disparityPlaneParameters, boundaryLabels);


	

#if 1
	std::string outputDisparityImageFilename = argv[3];
	disparityImage.write(outputDisparityImageFilename);

	outputDisparityImageFilename = argv[4];
	disparityImageRight.write(outputDisparityImageFilename);
#else
	png::image<png::rgb_pixel> segmentBoundaryImage;
	makeSegmentBoundaryImage(leftImage, segmentImage, boundaryLabels, segmentBoundaryImage);

	//std::string outputBaseFilename = leftImageFilename;
	std::string outputBaseFilename = argv[3];
	size_t slashPosition = outputBaseFilename.rfind('/');
	if (slashPosition != std::string::npos) outputBaseFilename.erase(0, slashPosition+1);
	size_t dotPosition = outputBaseFilename.rfind('.');
	if (dotPosition != std::string::npos) outputBaseFilename.erase(dotPosition);
	//std::string outputDisparityImageFilename = outputBaseFilename + "_left_disparity.png";
	std::string outputDisparityImageFilename = argv[3];
	std::string outputSegmentImageFilename = outputBaseFilename + "_segment.png";
	std::string outputBoundaryImageFilename = outputBaseFilename + "_boundary.png";
	std::string outputDisparityPlaneFilename = outputBaseFilename + "_plane.txt";
	std::string outputBoundaryLabelFilename = outputBaseFilename + "_label.txt";

	std::cout << outputBoundaryImageFilename << "\n";

	disparityImage.write(outputDisparityImageFilename);
	segmentImage.write(outputSegmentImageFilename);
	segmentBoundaryImage.write(outputBoundaryImageFilename);
	writeDisparityPlaneFile(disparityPlaneParameters, outputDisparityPlaneFilename);
	writeBoundaryLabelFile(boundaryLabels, outputBoundaryLabelFilename);
#endif
}
Exemplo n.º 2
0
int main(int argc, char* argv[]) {
	//Load Matrix Q
	cv::FileStorage fs("/home/bodereau/Bureau/OpenCVReprojectImageToPointCloud/Q.xml", cv::FileStorage::READ);
	cv::Mat Q;

	pcl::visualization::CloudViewer viewer ("3D Viewer");

	fs["Q"] >> Q;

	//If size of Q is not 4x4 exit
	if (Q.cols != 4 || Q.rows != 4)
	{
		std::cerr << "ERROR: Could not read matrix Q (doesn't exist or size is not 4x4)" << std::endl;
		return 1;
	}


	//Get the interesting parameters from Q
	double Q03, Q13, Q23, Q32, Q33;
	Q03 = Q.at<double>(0,3);
	Q13 = Q.at<double>(1,3);
	Q23 = Q.at<double>(2,3);
	Q32 = Q.at<double>(3,2);
	Q33 = Q.at<double>(3,3);

	std::cout << "Q(0,3) = "<< Q03 <<"; Q(1,3) = "<< Q13 <<"; Q(2,3) = "<< Q23 <<"; Q(3,2) = "<< Q32 <<"; Q(3,3) = "<< Q33 <<";" << std::endl;

	cv::Size size(752, 480);
	vision::StereoRectifier rectifier(size);
	rectifier.load_intrinsic_params("/home/bodereau/Bureau/intrinsics.yml");
	rectifier.load_extrinsic_params("/home/bodereau/Bureau/extrinsics.yml");
	rectifier.stereo_rectify();
	rectifier.dump_rectification_info();
	int idx = 220;
	 //time
	clock_t start,other_clock,clock2,clock4;
	float time,time2,timtotal,time3,time4;
	timtotal = 0.0;
	start = clock();
	//~time
	cv::namedWindow("vigne");
	cv::namedWindow("bord");
	while(idx++ < 500) {
		//printf("boucle : %d \n",idx);
		std::string ref = std::to_string(idx);


		std::string leftImageFilename = "/home/bodereau/Bureau/tiff2/" + ref + "_l.tiff";

		std::string rightImageFilename = "/home/bodereau/Bureau/tiff2/" + ref + "_r.tiff";
		cv::Mat g1, g2;
		clock2 = clock();
		cv::Size size_rectified = rectifier.get_recified_size();
		cv::Mat3b mat_r = cv::imread(rightImageFilename);
		cv::Mat3b mat_l = cv::imread(leftImageFilename);
		cv::Mat3b rectified_l = cv::Mat3b::zeros(size_rectified);
		cv::Mat3b rectified_r = cv::Mat3b::zeros(size_rectified);
		rectifier.generate_rectified_mat(mat_l, mat_r, rectified_l, rectified_r);

		DTSStereo dtsStereo;
		/*int test = dtsStereo.computeLab(rectified_l.data, size_rectified.area(), size_rectified.width);
			int test2 = dtsStereo.compute(rectified_l.data, size_rectified.area(), size_rectified.width);*/
		int height_without_sky = dtsStereo.computeHisto(rectified_l, size_rectified.area(), size_rectified.width);

		cv::namedWindow("img test1", CV_WINDOW_AUTOSIZE);

		printf("height without sky %d \n", height_without_sky);
		clock2 = clock() - clock2;
		time3 = ((float) (clock2)) / CLOCKS_PER_SEC;
		printf("time for rectifie : %f  \n", time3);
		other_clock = clock();

		cv::cvtColor(rectified_l, g1, CV_BGR2GRAY);
		cv::cvtColor(rectified_r, g2, CV_BGR2GRAY);

		cv::Mat left_image_rectified_crop, right_image_rectified_crop;
		/*cv::Rect win(1, 339-test, 560, test);
		cv::Rect win2(1,339-test2,560,test2);*/
		cv::Rect win3(1, 339 - height_without_sky, 560, height_without_sky);
		rectified_l(win3).copyTo(left_image_rectified_crop);
		rectified_r(win3).copyTo(right_image_rectified_crop);



		//printf("convert to png::image left done \n");
		//png::image<png::rgb_pixel> rightImage("right.png");
		other_clock = clock() - other_clock;
		time2 = ((float) (other_clock)) / CLOCKS_PER_SEC;
		timtotal = timtotal + time2;

		printf("time lost in png make : %f \n", time2);
		//~time*/
		/*idxVect = 0;

		rightImage.write("rgb2.png");*/
		//printf("convert to png::image right done \n");
		SPSStereo sps;
		sps.setIterationTotal(outerIterationTotal, innerIterationTotal);
		sps.setWeightParameter(lambda_pos, lambda_depth, lambda_bou, lambda_smo);
		sps.setInlierThreshold(lambda_d);
		sps.setPenaltyParameter(lambda_hinge, lambda_occ, lambda_pen);

		cv::Mat segmentImage(600, 600, CV_16UC1);

		std::vector <std::vector<double>> disparityPlaneParameters;
		std::vector <std::vector<int>> boundaryLabels;
		//printf("go to compute \n");
		other_clock = clock();
		cv::Mat disparity(600, 600, CV_16UC1);
		sps.compute(superpixelTotal, left_image_rectified_crop, right_image_rectified_crop, segmentImage, disparity, disparityPlaneParameters, boundaryLabels);
		/*cv::StereoSGBM sgbm;
		sgbm.SADWindowSize = 5;
		sgbm.numberOfDisparities = 256;
		sgbm.preFilterCap = 0;
		sgbm.minDisparity = 0;
		sgbm.uniquenessRatio = 1;
		sgbm.speckleWindowSize = 150;
		sgbm.speckleRange = 2;
		sgbm.disp12MaxDiff = 10;
		sgbm.fullDP = false;
		sgbm.P1 = 1000;
		sgbm.P2 = 2400;
		cv::Mat disper;
		sgbm(dst, dst2, disper);
		normalize(disper, disparity, 0, 255, CV_MINMAX, CV_8U);*/
		other_clock = clock() - other_clock;
		time2 = ((float) (other_clock)) / CLOCKS_PER_SEC;
		printf("time to compute  : %f \n", time2);
		//printf("compute done \n");
		other_clock = clock();
		/*png::image<png::rgb_pixel> segmentBoundaryImage;
					makeSegmentBoundaryImage(dst, segmentImage, boundaryLabels, segmentBoundaryImage);
					segmentBoundaryImage.write(ref + "__bound.png");*/
		//disparityImage.write(ref + "__disparity.png");
		other_clock = clock() - other_clock;

		cv::imshow("vigne", disparity);
		/*cv::Mat copy;
		disparity.copyTo(copy);
		STVFlow stvflow;
		stvflow.compute(disparity,dst);*/
		//copy.convertTo(copy, CV_8U,1/255.0,1/255.0);
		/*cv::Mat dst455;
		cv::threshold(disparity,dst455,50, 255, cv::THRESH_BINARY);*/

		/*cv::imshow("bord",disparity);
		cv::threshold(copy,copy,100, 255, cv::THRESH_BINARY);
		cv::imshow("img test1",dst);*/
		cv::waitKey(100);
		cv::Mat img_disparity, img_rgb;
		img_disparity = disparity;
		img_rgb = left_image_rectified_crop;
		img_disparity.convertTo(img_disparity, CV_8U, 1 / 255.0, 1 / 255.0); //A REMETRE EN SPS
		//Create matrix that will contain 3D corrdinates of each pixel
		cv::Mat recons3D(img_disparity.size(), CV_32FC3);

		//Reproject image to 3D
		std::cout << "Reprojecting image to 3D..." << std::endl;
		cv::reprojectImageTo3D(img_disparity, recons3D, Q, false, CV_32F);

		std::cout << "Creating Point Cloud..." << std::endl;
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_all_the_image_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

		double px, py, pz;
		uchar pr, pg, pb;
		clock4 = clock();

		for (int i = 0; i < img_rgb.rows; i++) {
			uchar *rgb_ptr = img_rgb.ptr<uchar>(i);

			uchar *disp_ptr = img_disparity.ptr<uchar>(i);

			//double* recons_ptr = recons3D.ptr<double>(i);
			for (int j = 0; j < img_rgb.cols; j++) {
				//Get 3D coordinates

				uchar d = disp_ptr[j];
				if (d == 0) continue; //Discard bad pixels
				double pw = -1.0 * static_cast<double>(d) * Q32 + Q33;
				px = static_cast<double>(j) + Q03;
				py = static_cast<double>(i) + Q13;
				pz = Q23;

				px = px / pw;
				py = py / pw;
				pz = pz / pw;

				//Get RGB info
				pb = rgb_ptr[3 * j];
				pg = rgb_ptr[3 * j + 1];
				pr = rgb_ptr[3 * j + 2];

				//Insert info into point cloud structure
				pcl::PointXYZRGB point;
				point.x = px;
				point.y = py;

				point.z =-1* pz;
				uint32_t rgb = (static_cast<uint32_t>(pr) << 16 |
						static_cast<uint32_t>(pg) << 8 | static_cast<uint32_t>(pb));
				point.rgb = *reinterpret_cast<float *>(&rgb);
				//if(-1*pz < 90){
					point_cloud_all_the_image_rgb->points.push_back(point);//}
				pcl::PointXYZ pointx;
				pointx.x = px;
				pointx.y = py;
				pointx.z = -1*pz;
				if(-1*pz <90)
					cloud->points.push_back(pointx);
			}
		}

		point_cloud_all_the_image_rgb->width = (int) point_cloud_all_the_image_rgb->points.size();
		point_cloud_all_the_image_rgb->height = 1;
		cloud->width = (int) cloud->points.size();
		cloud->height = 1;
		clock4 = clock() - clock4;
		time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
		printf("temps a faire les pointcloud : %f \n", time4);

		clock4 = clock();
		// Read in the cloud data
		pcl::PCDReader reader;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);


		//cloud = cloud_ptr;
		std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ground(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgbseg(new pcl::PointCloud<pcl::PointXYZRGB>);

		ExtractTheGround extractTheGround;
		extractTheGround.compute(cloud, cloud_filtered, cloud_ground);
		clock4 = clock() - clock4;
		time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
		printf("temps a sortir le sol : %f \n", time4);
		clock4 = clock();

		std::cout << "ytyt" << std::endl;

		std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clusters;

		EuclidianClusters euclidianClusters;
		euclidianClusters.compute(cloud_filtered, clusters);

		clock4 = clock() - clock4;
		time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
		printf("temps a faire la seg euclid : %f \n", time4);
		clock4 = clock();

		/*pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgbseg_less(new pcl::PointCloud<pcl::PointXYZRGB>);
		for(int i=0;i < cloud_filtered->points.size();i++){
			for(int j=0;j < point_cloud_ptr->points.size();j++){
				if(cloud_filtered->points[i].x == point_cloud_ptr->points[j].x && cloud_filtered->points[i].y == point_cloud_ptr->points[j].y &&
						cloud_filtered->points[i].z == point_cloud_ptr->points[j].z){
					cloud_rgbseg_less->points.push_back(point_cloud_ptr->points[j]);
					break;
				}
			}
		}
		RGBSegmentation rgbseg;
		rgbseg.compute(cloud_rgbseg_less, cloud_rgbseg);*/

		//viewer.removeVisualizationCallable("jhjh");
		viewer.removeVisualizationCallable("jhjha");
		viewer.showCloud(point_cloud_all_the_image_rgb,"jhjha");
		clock4 = clock() - clock4;
		time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
		printf("temps afficher image: %f \n", time4);
		clock4 = clock();
		//viewer.showCloud(cloud_rgbseg,"jhjh");

		/*ClustersFilter clustersFilter;
		clustersFilter.compute(clusters);*/

		pcl::PointCloud<pcl::PointXYZ>::Ptr cube(new pcl::PointCloud<pcl::PointXYZ>);
		for(int i=0; i< clusters.size();i++) {
			float minX, minY, minZ,maxZ,maxX,maxY,memY,memZ;
			int nbrY,nbrZ;
			bool dontgo = false;
			bool zminbool = false;

			for(int j=0; j < clusters.at(i)->points.size();j++) {
				//pour chaque points du cluster en question, eh ouais !
				float xpoint = clusters.at(i)->points[j].x;
				float ypoint = clusters.at(i)->points[j].y;
				float zpoint = clusters.at(i)->points[j].z;
				if(j==0){
					minX = xpoint; minY = ypoint; minZ = zpoint;
					maxX = xpoint; maxY = ypoint; maxZ= zpoint;
					memY = ypoint; memZ = zpoint;
				}

				if(memZ == zpoint ){
					nbrZ++;
				}else {

					if (nbrZ >= 50) {
						maxZ = zpoint;
						if (!zminbool) {
							zminbool = true;
							minZ = zpoint;
						}
					}
					nbrZ = 1;
				}

				if(memY != ypoint)
					memY = ypoint;
				if(memZ != zpoint) {
					memZ = zpoint;
				}

				/*if(zpoint > maxZ)
					maxZ = zpoint;*/
				/*if(zpoint < minZ)
					minZ = zpoint;*/
				if(xpoint < minX)
					minX= xpoint;
				if(xpoint > maxX)
					maxX = xpoint;
				if( ypoint < minY)
					minY = ypoint;
				if( ypoint > maxY)
					maxY = ypoint;
				if(maxZ > (minZ +15))
					break;

			}
			if (maxX - minX > 3 * (maxY - minY))
				dontgo = true;
			if(maxZ - minZ < 3)
				dontgo = true;
			if(dontgo)
				continue;

			float xdebut = minX, ydebut = minY, zdebut = minZ, xfin = maxX, yfin = maxY, zfin = maxZ;
			//float xdebut = -10.20, ydebut = 0, zdebut = 12.20, xfin = -50.40, yfin = 15.24, zfin = -13.56;
			if (ydebut > yfin) {
				float tmp = ydebut;
				ydebut = yfin;
				yfin = tmp;
			}
			if (xdebut > xfin) {
				float tmp = xdebut;
				xdebut = xfin;
				xfin = tmp;
			}
			if (zdebut > zfin) {
				float tmp = zdebut;
				zdebut = zfin;
				zfin = tmp;
			}

			for (float i = xdebut; i <= xfin; i+=0.3) {
				for (float j = ydebut; j <= yfin; j+=0.3) {

					pcl::PointXYZ p;
					pcl::PointXYZ p2;
					p.x = i;
					if (i == xdebut || (i >= xfin - 0.3 && i <= xfin + 0.3)) {
						p.y = j;

					}
					else {
						p.y = ydebut;

						p2.y = yfin;
					}
					p.z = zfin;

					p2.x = p.x;
					p2.z = p.z;
					cube->points.push_back(p);
					cube->points.push_back(p2);
					p.z = zdebut;
					p2.z = zdebut;
					cube->points.push_back(p);
					cube->points.push_back(p2);
				}
			}
			for(float j=ydebut; j <=yfin;j+=0.3){
			for(float z =zdebut;z<=zfin;z+=0.3){
				pcl::PointXYZ p;
				pcl::PointXYZ p2;
				p.x=xdebut;
				p2.x=xfin;
				p.y=j;
				p.z=z;
				p2.y=p.y;
				p2.z=p.z;
				if (j == ydebut || (j>= yfin - 0.3 && j <= yfin + 0.3)) {

					cube->points.push_back(p);
					cube->points.push_back(p2);
				}
			}
		}
		}

		viewer.showCloud (cube,"oh");

		/*pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_memory2 (new pcl::PointCloud<pcl::PointXYZRGB>);
		int r2=15,b2=255,g3r=125;
		for(int i = 0; i < clusters.size();i++){
			uint32_t rgb = (static_cast<uint32_t>(r2) << 16 |
					static_cast<uint32_t>(g3r) << 8 | static_cast<uint32_t>(b2));
			for(int j=0;j< clusters.at(i)->points.size();j++){
				clusters.at(i)->points[j].rgb = *reinterpret_cast<float*>(&rgb);
			}
			r2+=20;
			b2+=100;
			g3r+=5;
			if(r2 >252)
				r2=0;
			if(g3r>252)
				g3r=0;
			if(b2>253)
				b2=0;
			*cloud_rgb_memory2 += *clusters.at(i);
		}
		clock4 = clock() - clock4;
		time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
		printf("temps a faire les clusters avant : %f \n", time4);
		clock4 = clock();
		std::cout<<"clusters size :"<<clusters.size() <<std::endl;
		//viewer.showCloud (cloud_rgb_memory2,"eh eh eh");
		clock4 = clock() - clock4;
		time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
		printf("temps a faire les clusters aprés : %f \n", time4);
		clock4 = clock();*/
	}

	//time
	start = clock() - start;
	time = ((float) ( start))/ CLOCKS_PER_SEC;
	printf("time for all : %f and time without loss :%f \n",time,time - timtotal);
}
Exemplo n.º 3
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  
  if (nrhs!=2 && nrhs!=3 && nrhs!=4 && nrhs!=5)
    mexErrMsgTxt("2-5 inputs required: I1 (left image), I2 (right image), [data,smoothness,sgm] parameters");
  if (nlhs!=2) 
    mexErrMsgTxt("2 outputs required: D1 (left disparities), D2 (right disparities)");  
  if (!mxIsUint8(prhs[0]))// || mxGetNumberOfDimensions(prhs[0])!=3)
    mexErrMsgTxt("Input I1 (left image) must be a uint8 image.");
  if (!mxIsUint8(prhs[1]))// || mxGetNumberOfDimensions(prhs[1])!=3)
    mexErrMsgTxt("Input I2 (right image) must be a uint8 image.");
  
  // get input pointers
  unsigned char*   I1 = (unsigned char*)mxGetPr(prhs[0]);
  unsigned char*   I2 = (unsigned char*)mxGetPr(prhs[1]);
  const int32_t *dims = mxGetDimensions(prhs[0]);
  
  // width/height
  int w = dims[1];
  int h = dims[0];

  // create input images
  //rev::Image<unsigned char> leftImage(w,h,1);
  //rev::Image<unsigned char> rightImage(w,h,1);
  png::image<png::rgb_pixel> leftImage(w,h);
	png::image<png::rgb_pixel> rightImage(w,h);
  
  // copy input
  for (int u=0; u<w; u++) {
    for (int v=0; v<h; v++) {
      leftImage.set_pixel(u,v,png::rgb_pixel(I1[u*h+v],I1[u*h+v+w*h],I1[u*h+v+2*(w*h)]));
      rightImage.set_pixel(u,v,png::rgb_pixel(I2[u*h+v],I2[u*h+v+w*h],I2[u*h+v+2*(w*h)]));
    }
  }
  //leftImage.write("test_l.png");
  //rightImage.write("test_r.png");
  
  int disp_num = 256;

  // run sgm
//   SgmStereo sgmStereo;
//   sgmStereo.setDisparityTotal(disp_num);
//   sgmStereo.setOutputDisparityFactor(disp_num);
  
  // run sps
  SPSStereo sps;
  
//   if (nrhs>=3) {
//     double* p = (double*)mxGetPr(prhs[2]);
//     sgmStereo.setDataCostParameters((int)p[0],(int)p[1],(double)p[2],(int)p[3]);
//   }
//   
//   if (nrhs>=4) {
//     double* p = (double*)mxGetPr(prhs[3]);
//     sgmStereo.setSmoothnessCostParameters((int)p[0],(int)p[1]);
//   }
//   
//   if (nrhs>=5) {
//     double* p = (double*)mxGetPr(prhs[4]);
//     sgmStereo.setSgmParameters((bool)p[0],(int)p[1]);
//   }
  
//   rev::Image<unsigned short> leftDisparityImage;
//   rev::Image<unsigned short> rightDisparityImage;
//   sgmStereo.computeLeftRight(leftImage, rightImage, leftDisparityImage, rightDisparityImage);
  
  png::image<png::gray_pixel_16> segmentImage;
	png::image<png::gray_pixel_16> disparityImage;
	std::vector< std::vector<double> > disparityPlaneParameters;
	std::vector< std::vector<int> > boundaryLabels;
	sps.compute(superpixelTotal, leftImage, rightImage, segmentImage, disparityImage, disparityPlaneParameters, boundaryLabels);
  
  //disparityImage.write("test.png");
  
  // create outputs
  plhs[0]      = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL);
  double*   D1 = (double*)mxGetPr(plhs[0]);
  plhs[1]      = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL);
  double*   L  = (double*)mxGetPr(plhs[1]);


 // copy to output
  for (int u=0; u<w; u++) {
    for (int v=0; v<h; v++) {
      D1[u*h+v] = (double)disparityImage.get_pixel(u,v)/(double)disp_num;
      L [u*h+v] = (double)segmentImage.get_pixel(u,v);
      if (D1[u*h+v]<0.001) D1[u*h+v] = -1;
//       if (D2[u*h+v]<0.001) D2[u*h+v] = -1;
    }
  }
}