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"
#include "opencv2/features2d/features2d.hpp"

int main (int argc, char** argv)
{

	//
	// image 1 is special, keep the color info for drawing later  ;)
	//
	cv::Mat img1_color = cv::imread("D:/images/chess_board/canons/exp_6/0_cut.jpg");
	cv::Mat img1_load;
	cv::cvtColor(img1_color,img1_load, CV_RGB2GRAY  );

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

	cv::Mat img1_r;
	cv::Mat img2_r;

	cv::Mat disp;
	cv::Mat vdisp;

	cv::Mat _mx1( img1_load.rows, img1_load.cols, CV_32F );
	cv::Mat _my1( img1_load.rows, img1_load.cols, CV_32F );
	cv::Mat _mx2( img1_load.rows, img1_load.cols, CV_32F );
	cv::Mat _my2( img1_load.rows, img1_load.cols, CV_32F );


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

	cv::remap( img1_load, img1_r, _mx1,_my1, CV_INTER_LINEAR);
	cv::remap( img2_load, img2_r, _mx2,_my2, CV_INTER_LINEAR);
	cv::imwrite("C:/Documents and Settings/mansouri.BEHINCO/Desktop/Flynn Matcher/rig_stereo/rig_stereo/working_home/remap1.jpg",img1_r);
	cv::imwrite("C:/Documents and Settings/mansouri.BEHINCO/Desktop/Flynn Matcher/rig_stereo/rig_stereo/working_home/remap2.jpg",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( img1_r, img2_r, disp );
	cv::normalize( disp, vdisp, 0, 255, cv::NORM_MINMAX, CV_8U );
	cv::imwrite( "C:/Documents and Settings/mansouri.BEHINCO/Desktop/Flynn Matcher/rig_stereo/rig_stereo/working_home/disparity_SGBM_V.jpg", vdisp );

	cv::Mat t__Q((CvMat*)cvLoad("C://Documents and Settings//mansouri.BEHINCO//Desktop//Flynn Matcher//rig_stereo//rig_stereo//working_home//Q.xml" ));

	cv::Mat xyz(img2_r.rows,img2_r.cols,CV_32FC3);
	cv::reprojectImageTo3D(vdisp, xyz, t__Q,0);

	// 
	// pcl visualization
	//
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

	cloud->width    = img2_r.cols * img2_r.rows;
	cloud->height   = 1;
	cloud->is_dense = false; // ?? is this nessecary ?
	cloud->points.resize (cloud->width * cloud->height);

	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
			cv::Point3f p = xyz.at<cv::Point3f>(ch, cw);
			cloud->points[i].x = p.x;
			cloud->points[i].y = p.y;
			cloud->points[i].z = p.z;
			
			// take pixels from left image for colour:
			cv::Point3_<uchar> c = img1_color.at< cv::Point3_<uchar> >(ch, cw);
			cloud->points[i].r = c.x;
			cloud->points[i].g = c.y;
			cloud->points[i].b = c.z;
			i ++;
		}
	}

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