gistfile1.txt

	cv::Mat xyz(vdisp.rows, vdisp.cols, CV_32FC3);
	cv::reprojectImageTo3D( vdisp, xyz, Q, 0);
	std::cout << xyz.rows << " " << xyz.cols << std::endl;

	//--Pcl code

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
	cloud->is_dense = false; // ??
	cloud_xyz->is_dense = false; // ??

	int tmode = 2;  // custom reconstruction 
	int i = 0;
	for (int r=0; r<xyz.rows; r++)
	{
		for (int c=0; c<xyz.cols; c++)
		{
			// the colour:
			cv::Vec3b col = org.at<cv::Vec3b>(r,c);
			pcl::PointXYZRGB pt(col[2],col[1],col[0]);

			switch(tmode) 
			{
				default:
				case 0:
				{	// just take them as they are ... 
					float d = float(vdisp.at<uchar>(r,c));
					//if (d==0 ) continue;
					pt.x = c;
					pt.y = r;
					pt.z = d;
					break;
				}
				case 1:
				{	// use the data from reconstruct
					cv::Point3f p = xyz.at<cv::Point3f>(r,c);
					pt.x = p.x;
					pt.y = p.y;
					pt.z = p.z;
					//pt.z = 120.0f + 0.2f * abs(p[2]);
					break;
				} 
				case 2:
				{	// martin's custom reconstruction
					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);
					uchar d = vdisp.at<uchar>(r,c);
					if ( d == 0 ) continue; //Discard bad pixels
					double pw = -1.0 * static_cast<double>(d) * Q32 + Q33; 
					if ( pw == 0 ) continue; // oh noes, NaN
					double px = static_cast<double>(c) + Q03;
					double py = static_cast<double>(r) + Q13;
					double pz = Q23;
					pt.x = px/pw;
					pt.y = py/pw;
					pt.z = pz/pw;
					break;
				}
				case 3:
				{	// try to achieve the same as reprojectImageTo3D, without generating NaN's
					uchar d = vdisp.at<uchar>(r,c);
					cv::Scalar t =  Q * cv::Scalar(c,r,d,1);
					if ( abs(t[3]) < 0.0001 )
						continue;
					pt.x = t[0] / t[3];
					pt.y = t[1] / t[3];
					pt.z = t[2] / t[3];
					break;
				}
			}
			cloud->points.push_back(pt);
			cloud_xyz->points.push_back(pcl::PointXYZ(pt.x,pt.y,pt.z));
			i++;
		}
	}
	std::cout << xyz.cols << " " << xyz.rows << " " << (xyz.cols * xyz.rows) << " " << cloud->points.size()<< std::endl;
	cloud->width  = cloud->points.size();
	cloud->height = 1;
	cloud_xyz->width  = cloud_xyz->points.size();
	cloud_xyz->height = 1;