gistfile1.txt

#include <pcl/visualization/cloud_viewer.h>

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgproc/imgproc_c.h" // cvRemap
#include "opencv2/features2d/features2d.hpp"


using namespace cv;

void 
viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor (0.2, 0.2, 0.2);    
	//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "reconstruction");
}

int main (int argc, char** argv)
{
	////--OpenCv Code

	////cv::Mat img1_load = cv::imread("D:/images/chess_board/canons/exp_6/0_cut.jpg",CV_LOAD_IMAGE_GRAYSCALE);
 //       //cv::Mat img2_load = cv::imread("D:/images/chess_board/canons/exp_6/1_cut.jpg",CV_LOAD_IMAGE_GRAYSCALE);
 //   
 //       IplImage* img1_load;// = 0;
	//img1_load = cvLoadImage("D:/images/chess_board/canons/exp_6/0_cut.jpg",CV_LOAD_IMAGE_GRAYSCALE);
	//
	//IplImage* img2_load;// = 0;
	//img2_load = cvLoadImage("D:/images/chess_board/canons/exp_6/1_cut.jpg",CV_LOAD_IMAGE_GRAYSCALE);

	//CvMat* img1_r = cvCreateMat(img1_load->height,img1_load->width,CV_8U );
	//CvMat* img2_r = cvCreateMat(img1_load->height,img1_load->width,CV_8U );

	//CvMat* disp = cvCreateMat(img1_load->height,img1_load->width,CV_16S );
	//CvMat* vdisp = cvCreateMat(img1_load->height,img1_load->width,CV_8U );

	//CvMat* _mx1 = cvCreateMat( img1_load->height,img1_load->width, CV_32F );
	//CvMat* _my1 = cvCreateMat( img1_load->height,img1_load->width, CV_32F );
	//CvMat* _mx2 = cvCreateMat( img1_load->height,img1_load->width, CV_32F );
	//CvMat* _my2 = cvCreateMat( img1_load->height,img1_load->width, CV_32F );


	//FILE* fr =  fopen("C://Documents and Settings//mansouri.BEHINCO//Desktop//Flynn Matcher//rig_stereo//rig_stereo//working_home//mpps.txt","rb");
	//fread(_mx1->data.fl,sizeof(float),_mx1->rows*_mx1->cols,fr); 
	//fread(_my1->data.fl,sizeof(float),_my1->rows*_my1->cols,fr);  
	//fread(_mx2->data.fl,sizeof(float),_mx2->rows*_mx2->cols,fr);  
	//fread(_my2->data.fl,sizeof(float),_my2->rows*_my2->cols,fr); 
	//fclose(fr);

	//cvRemap( img1_load,img1_r,_mx1,_my1);
	//cvRemap( img2_load,img2_r,_mx2,_my2);
	//imwrite("C:/Documents and Settings/mansouri.BEHINCO/Desktop/Flynn Matcher/rig_stereo/rig_stereo/working_home/remap1.jpg",Mat(img1_r));
	//imwrite("C:/Documents and Settings/mansouri.BEHINCO/Desktop/Flynn Matcher/rig_stereo/rig_stereo/working_home/remap2.jpg",Mat(img2_r));

 //   //--Method (1)
	//cv::Mat dispSGBM, vdispSGBM;
	//int SADWindowSize = 17;
	//int p1 = 4*1*SADWindowSize*SADWindowSize ;
	//int p2 = 32*1*SADWindowSize*SADWindowSize;
	//cv::StereoSGBM stereo(-32,128,SADWindowSize,p1,p2,5,31,0,0,0,0);
	//stereo(cv::Mat(img1_r),cv::Mat(img2_r),Mat(disp));
	//normalize( Mat(disp), Mat(vdisp), 0, 255, NORM_MINMAX, CV_8U );
	//imwrite( "C:/Documents and Settings/mansouri.BEHINCO/Desktop/Flynn Matcher/rig_stereo/rig_stereo/working_home/disparity_SGBM_V.jpg", Mat(vdisp) );

	////cv::Mat disp_face = cvCreateMat(img1_load->height,img1_load->width,CV_16S );
	CvMat *t__Q = (CvMat*)cvLoad("Q.xml" );
	cv::Mat Q(t__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);

	IplImage* vdisp = cvLoadImage("disparity-image.pgm",CV_LOAD_IMAGE_GRAYSCALE);
	IplImage* color = cvLoadImage("rgb-image.ppm");
	CvMat* xyz = cvCreateMat(vdisp->height,vdisp->width,CV_32FC3);
	cvReprojectImageTo3D(vdisp, xyz, t__Q,0);
    

	//--Pcl code

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

	 
	int i = 0;
	for (int ch=0; ch<xyz->rows; ch++)
	{
		for (int cw=0; cw<xyz->cols; cw++)
		{
			// the colour:
			char * c = color->imageData + 3 * (ch * color->width + cw);
			//char * c = color->imageData + 3 * (ch * xyz->rows + cw);
			pcl::PointXYZRGB pt(c[2],c[1],c[0]);

			if ( 0 ) 
			{	// use the data from reconstruct
				float *p = xyz->data.fl + 3 * (ch * xyz->rows + cw);
				pt.x = p[0];
				pt.y = p[1];
				pt.z = p[2];
			} 
			else 
			{	// custom reconstruction, x' = Q*[x,y,disp,1] // well, sort of. i'm unshure
				uchar d = ((uchar*)(vdisp->imageData))[ch*xyz->rows + cw];
				if ( d == 0 ) continue; //Discard bad pixels
				double pw = -1.0 * static_cast<double>(d) * Q32 + Q33; 
				double px = static_cast<double>(cw) + Q03;
				double py = static_cast<double>(ch) + Q13;
				double pz = Q23;
				pt.x = px/pw;
				pt.y = py/pw;
				pt.z = pz/pw;
			}
			cloud->points.push_back(pt);
			i++;
		}
	}
	std::cout << std::endl;
	cloud->width = cloud->points.size();
	cloud->height = 1;


	pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
	viewer.runOnVisualizationThreadOnce (viewerOneOff);
	viewer.showCloud (cloud);
	while (!viewer.wasStopped ())
	{
	}
	return 0;
}