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;


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("C://Documents and Settings//mansouri.BEHINCO//Desktop//Flynn Matcher//rig_stereo//rig_stereo//working_home//Q.xml" );

	CvMat* xyz = cvCreateMat(img2_r->rows,img2_r->cols,CV_32FC3);
	cvReprojectImageTo3D(vdisp, xyz, t__Q,0);
    

	//--Pcl code

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud->width    = img2_r->cols;
	cloud->height   = img2_r->rows;
	cloud->is_dense = false; // ??
	cloud->points.resize (cloud->width * cloud->height);

	float *data= xyz->data.fl;
	 
	int i = 0;
	for (int ch=0; ch<=xyz->rows; ch++)
	{
		for (int cw=0; cw<=xyz->cols; cw++)
		{
			// x,y,z might need some scaling
			float *p = data + 3 * (ch * xyz->rows + cw);
			cloud->points[i].x = p[0];
			cloud->points[i].y = p[1];
			cloud->points[i].z = p[2];

			cloud->points[i].r = 255;//.x;
			cloud->points[i].g = 0;//.y;
			cloud->points[i].b = 0;//.z;
			i ++;
		}
	}

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

	return 0;
}