// Cpp-pcl.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/surface/gp3.h>
#include <pcl/console/time.h>   // TicToc
#include <pcl/filters/random_sample.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/gpu/containers/initialization.h>
#include <pcl/gpu/octree/octree.hpp>
#include <pcl/gpu/features/features.hpp>
#include <pcl/gpu/segmentation/gpu_extract_clusters.h>
#include <pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;



// µãÔÆÊý¾Ý
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_trans(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);

bool next_iteration = false;

void print4x4Matrix(const Eigen::Matrix4d & matrix)
{
	printf("Rotation matrix :\n");
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,void* nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}



// 1.¶ÁÈ¡µãÔÆ
// 2.È¥³ýÔؾ߲¿·ÖµãÔÆ
// 3.±£´æµãÔÆ
void step1(const std::string &fileName,bool isSave = false)
{
	pcl::console::TicToc time;
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>); // ´´½¨µãÔÆ£¨Ö¸Õ룩 	

	time.tic();

	if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName, *cloud_in) < 0) //* ¶ÁÈëPCD¸ñʽµÄÎļþ£¬Èç¹ûÎļþ²»´æÔÚ£¬·µ»Ø-1	
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //Îļþ²»´æÔÚʱ£¬·µ»Ø´íÎó£¬ÖÕÖ¹³ÌÐò¡£		
		return;
	}

	cout << "Loaded file (" << cloud_in->size()<< " points) in " << time.toc() << " ms" << endl;

	time.tic();

	pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
	
	
	//======================CPU version===========================//
	/*pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
	normal_estimator.setSearchMethod(tree);
	normal_estimator.setInputCloud(cloud_in);
	normal_estimator.setKSearch(50);
	normal_estimator.compute(*normals);*/
	//===========================================================//

	//=====================GPU version===========================//
	pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud_in));
	pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
	pcl::gpu::Octree::PointCloud cloud_device;
	cloud_device.upload(cloud_in->points);
	normals_device.upload(normals_p->points);

	pcl::gpu::NormalEstimation normal_estimator;
	normal_estimator.setRadiusSearch(2.0,50);
	normal_estimator.setInputCloud(cloud_device);
	normal_estimator.compute(normals_device);

	normals_device.download(normals_p->points);

	for (int i = 0; i < normals_p->size(); i++)
	{
		pcl::Normal normal;

		normal.normal_x = normals_p->points[i].x;
		normal.normal_y = normals_p->points[i].y;
		normal.normal_z = normals_p->points[i].z;

		normals->push_back(normal);
	}
	//=========================================================//


	std::cout << "Estimation Normal in:" << time.toc() << "ms" << std::endl;
	time.tic();
	
	//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Viewer"));
	//viewer->setBackgroundColor(0, 0, 0);
	//viewer->addPointCloud<pcl::PointXYZ>(cloud_in, "cloud");
	//viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_in, normals, 1, 0.05, "normals");//ʵÏÖ¶ÔµãÔÆ·¨ÏßµÄÏÔʾ
	//
	//while (!viewer->wasStopped())
	//{
	//	viewer->spinOnce();
	//}
	//return;

	pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
	reg.setMinClusterSize(50000);
	reg.setMaxClusterSize(1000000);
	reg.setSearchMethod(tree);
	reg.setNumberOfNeighbours(30);
	reg.setInputCloud(cloud_in);
	reg.setInputNormals(normals);
	reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
	reg.setCurvatureThreshold(1.0);

	std::vector <pcl::PointIndices> clusters;
	reg.extract(clusters);

	std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
	std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;
	
	boost::shared_ptr<std::vector<int>> index_ptr;
	pcl::ExtractIndices<pcl::PointXYZ> extract(new pcl::ExtractIndices<pcl::PointXYZ>);

	index_ptr = boost::make_shared<std::vector<int>>(clusters[0].indices);
	extract.setInputCloud(cloud_in);
	extract.setIndices(index_ptr);
	extract.setNegative(false);
	extract.filter(*cloud);

	std::cout << "Segmentation cloud in:" << time.toc() << "ms" << std::endl;

	if (isSave)
	{
		pcl::io::savePCDFile<pcl::PointXYZ>("D:\\test\\Plane.pcd", *cloud);
	}
	

	/*index_ptr = boost::make_shared<std::vector<int>>(clusters[1].indices);
	extract.setInputCloud(cloud);
	extract.setIndices(index_ptr);
	extract.setNegative(false);
	extract.filter(*cloud_p);
	pcl::io::savePLYFileBinary<pcl::PointXYZ>("D:\\test\\stand2.ply", *cloud_p);

	index_ptr = boost::make_shared<std::vector<int>>(clusters[2].indices);
	extract.setInputCloud(cloud);
	extract.setIndices(index_ptr);
	extract.setNegative(false);
	extract.filter(*cloud_p);
	pcl::io::savePLYFileBinary<pcl::PointXYZ>("D:\\test\\stand3.ply", *cloud_p);*/

	/*int counter = 0;
	while (counter < clusters[0].indices.size())
	{
		std::cout << clusters[0].indices[counter] << ", ";
		counter++;
		if (counter % 10 == 0)
			std::cout << std::endl;
	}*/
	


	//չʾµãÔÆ
	pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
	pcl::visualization::CloudViewer viewer("Cluster viewer");
	viewer.showCloud(colored_cloud);
	while (!viewer.wasStopped())
	{

	}
	return;
}

void step1_gpu(const std::string &fileName, bool isSave = false)
{
	pcl::console::TicToc time;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>); // ´´½¨µãÔÆ£¨Ö¸Õ룩 	

	time.tic();

	if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName, *cloud_in) < 0) //* ¶ÁÈëPCD¸ñʽµÄÎļþ£¬Èç¹ûÎļþ²»´æÔÚ£¬·µ»Ø-1	
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //Îļþ²»´æÔÚʱ£¬·µ»Ø´íÎó£¬ÖÕÖ¹³ÌÐò¡£		
		return;
	}

	cout << "Loaded file (" << cloud_in->size() << " points) in " << time.toc() << " ms" << endl;

	time.tic();

	pcl::gpu::Octree::PointCloud cloud_device;
	cloud_device.upload(cloud_in->points);

	pcl::gpu::Octree::Ptr octree_device(new pcl::gpu::Octree);
	octree_device->setCloud(cloud_device);
	octree_device->build();

	std::vector<pcl::PointIndices> cluster_indices_gpu;
	pcl::gpu::EuclideanClusterExtraction gec;
	gec.setClusterTolerance(0.6); // 2mm
	gec.setMinClusterSize(100000);
	gec.setMaxClusterSize(250000);
	gec.setSearchMethod(octree_device);
	gec.setHostCloud(cloud_in);
	gec.extract(cluster_indices_gpu);


	std::cout << "Number of clusters is equal to " << cluster_indices_gpu.size() << std::endl;
	std::cout << "First cluster has " << cluster_indices_gpu[0].indices.size() << " points." << endl;

	boost::shared_ptr<std::vector<int>> index_ptr;
	pcl::ExtractIndices<pcl::PointXYZ> extract(new pcl::ExtractIndices<pcl::PointXYZ>);

	index_ptr = boost::make_shared<std::vector<int>>(cluster_indices_gpu[0].indices);
	extract.setInputCloud(cloud_in);
	extract.setIndices(index_ptr);
	extract.setNegative(false);
	extract.filter(*cloud);

	std::cout << "segmentation cloud in:" << time.toc() << "ms" << std::endl;

	if (isSave)
	{
		pcl::io::savePCDFile<pcl::PointXYZ>("D:\\test\\Plane.pcd", *cloud);
	}
}

// 1.¶ÁÈ¡µãÔÆ
// 2.Èý½Ç²âÁ¿Éú³Émesh
// 3.±£´æmesh
void step2(const std::string &fileName = "")
{
	pcl::console::TicToc time;

	if (fileName != "")
	{
		time.tic();

		// Load input file into a PointCloud<T> with an appropriate type

		pcl::PCLPointCloud2 cloud_blob;
		if (pcl::io::loadPCDFile(fileName, cloud_blob) < 0)
		{
			PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //Îļþ²»´æÔÚʱ£¬·µ»Ø´íÎó£¬ÖÕÖ¹³ÌÐò¡£		
			return;
		}
		pcl::fromPCLPointCloud2(cloud_blob, *cloud);
		//* the data should be available in cloud

		std::cout << "cloud size: " << cloud->size() << "," << time.toc() << "ms" << endl;
	}
	

	time.tic();

	// Normal estimation*
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(cloud);
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(20);
	n.compute(*normals);
	//* normals should not contain the point normals + surface curvatures

	// Concatenate the XYZ and normal fields*
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
	//* cloud_with_normals = cloud + normals

	// Create search tree*
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
	tree2->setInputCloud(cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
	pcl::PolygonMesh triangles;

	// Set the maximum distance between connected points (maximum edge length)
	gp3.setSearchRadius(25);

	// Set typical values for the parameters
	gp3.setMu(25);
	gp3.setMaximumNearestNeighbors(200);
	gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees
	gp3.setMinimumAngle(M_PI / 18); // 10 degrees
	gp3.setMaximumAngle(2 * M_PI / 3); // 120 degrees
	gp3.setNormalConsistency(false);

	// Get result
	gp3.setInputCloud(cloud_with_normals);
	gp3.setSearchMethod(tree2);
	gp3.reconstruct(triangles);

	std::cout << "create mesh in " << time.toc() << "ms" << endl;

	pcl::io::saveVTKFile("D:\\test\\Mesh.vtk", triangles);

	// Finish
	return;

}

// 1.¶ÁÈ¡mesh
// 2.icp
// 3.±£´æ¾­¹ýת»»ºóµÄµãÔÆ
void step3(const std::string &fileName1,const std::string &fileName2,bool isShow,bool isSave)
{
	pcl::console::TicToc time;

	// The point clouds we will be using
	PointCloudT::Ptr cloud_source(new PointCloudT);

	PointCloudT::Ptr cloud_in(new PointCloudT);  // Original point cloud
	PointCloudT::Ptr cloud_tr(new PointCloudT);  // Transformed point cloud
	PointCloudT::Ptr cloud_standard(new PointCloudT);  // ICP output point cloud

	pcl::RandomSample<PointT> rs;
	rs.setSample(10000);

	int iterations = 200;  // Default number of ICP iterations
	

	time.tic();

	if (fileName1 == "")
	{
		cloud_source = cloud;
	}
	else
	{
		if (pcl::io::loadPLYFile(fileName1, *cloud_source) < 0)
		{
			PCL_ERROR("Error loading cloud %s.\n", fileName1);
			return;
		}
	}
	
	rs.setInputCloud(cloud_source);
	rs.filter(*cloud_in);
	std::cout << "\nLoaded file " << fileName1 << " (" << cloud_in->size() << " points) in " << time.toc() << " ms\n" << std::endl;
	
	if (pcl::io::loadPLYFile(fileName2, *cloud_target) < 0)
	{
		PCL_ERROR("Error loading cloud %s.\n", fileName2);
		return;
	}
	rs.setInputCloud(cloud_target);
	rs.filter(*cloud_standard);
	std::cout << "\nLoaded file " << fileName2 << " (" << cloud_standard->size() << " points) in " << time.toc() << " ms\n" << std::endl;
	
	*cloud_tr = *cloud_in;  // We backup cloud_icp into cloud_tr for later use

	// Defining a rotation matrix and translation vector
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();

	// The Iterative Closest Point algorithm
	time.tic();
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaximumIterations(iterations);
	icp.setInputSource(cloud_in);
	icp.setInputTarget(cloud_standard);
	icp.align(*cloud_in);

	std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;
	
	if (icp.hasConverged())
	{
		transformation_matrix = icp.getFinalTransformation().cast<double>();
		print4x4Matrix(transformation_matrix);
		icp.setMaximumIterations(1);
		icp.align(*cloud_in);
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "\nICP transformation " << iterations << " : cloud_in -> cloud_standard" << std::endl;

		// ±£´æת»»ºóµÄµãÔÆ
		pcl::transformPointCloud(*cloud_source, *cloud_trans, transformation_matrix);
		if (isSave)
		{
			pcl::io::savePLYFile("D:\\test\\mesh_trans.ply", *cloud_trans);
		}
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
		return;
	}

	if (isShow)
	{
		// Visualization
		pcl::visualization::PCLVisualizer viewer("ICP demo");
		// Create two vertically separated viewports
		int v1(0);
		int v2(1);
		viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
		viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);

		// The color we will be using
		float bckgr_gray_level = 0.0;  // Black
		float txt_gray_lvl = 1.0 - bckgr_gray_level;

		// Original point cloud is white
		pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_standard, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
			(int)255 * txt_gray_lvl);
		viewer.addPointCloud(cloud_standard, cloud_in_color_h, "cloud_in_v1", v1);
		viewer.addPointCloud(cloud_standard, cloud_in_color_h, "cloud_in_v2", v2);

		// Transformed point cloud is green
		pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 20, 180, 20);
		viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

		// ICP aligned point cloud is red
		pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_in, 180, 20, 20);
		viewer.addPointCloud(cloud_in, cloud_icp_color_h, "cloud_icp_v2", v2);

		// Adding text descriptions in each viewport
		viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
		viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

		std::stringstream ss;
		ss << iterations;
		std::string iterations_cnt = "ICP iterations = " + ss.str();
		viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

		// Set background color
		viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
		viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

		// Set camera position and orientation
		viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
		viewer.setSize(1280, 1024);  // Visualiser window size

		// Register keyboard callback :
		viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

		// Display the visualiser
		while (!viewer.wasStopped())
		{
			viewer.spinOnce();

			// The user pressed "space" :
			if (next_iteration)
			{
				// The Iterative Closest Point algorithm
				time.tic();
				icp.align(*cloud_in);

				std::cout << "Applied 1 ICP iteration in " << time.toc() << " ms" << std::endl;

				if (icp.hasConverged())
				{
					printf("\033[11A");  // Go up 11 lines in terminal output.
					printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());
					std::cout << "\nICP transformation " << ++iterations << " : cloud_in -> cloud_standard" << std::endl;
					transformation_matrix *= icp.getFinalTransformation().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
					print4x4Matrix(transformation_matrix);  // Print the transformation between original pose and current pose

					ss.str("");
					ss << iterations;
					std::string iterations_cnt = "ICP iterations = " + ss.str();
					viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
					viewer.updatePointCloud(cloud_in, cloud_icp_color_h, "cloud_icp_v2");
				}
				else
				{
					PCL_ERROR("\nICP has not converged.\n");
					return;
				}
			}
			next_iteration = false;
		}
	}
}

// 1.¶ÁÈ¡Ô´µãÔƺÍÄ¿±êµãÔÆ
// 2.¼ÆËã·¨Ïß
// 3.¼ÆËãÔ´µãÔƵ½Ä¿±êµãÔƵÄ×îС¾àÀë
// 4.±£´æÊý¾ÝÒÔ¼°Êý¾Ýչʾ
void step4(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target)
{
	pcl::console::TicToc time;

	pcl::registration::CorrespondenceEstimationNormalShooting<PointT, PointT, pcl::Normal> cens;
	pcl::Correspondences correspondence;
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_c(new pcl::PointCloud<pcl::PointXYZI>);


	time.tic();

	pcl::PointCloud <pcl::Normal>::Ptr source_normals(new pcl::PointCloud <pcl::Normal>);

	//======================CPU version===========================//
	//pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	//pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
	//normal_estimator.setInputCloud(cloud_source);
	//normal_estimator.setSearchMethod(tree);
	//normal_estimator.setKSearch(10);
	//normal_estimator.compute(*source_normals);
	//===========================================================//

	//=====================GPU version===========================//
	pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud_source));
	pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
	pcl::gpu::Octree::PointCloud cloud_device;
	cloud_device.upload(cloud_source->points);
	normals_device.upload(normals_p->points);

	pcl::gpu::NormalEstimation normal_estimator;
	normal_estimator.setRadiusSearch(2.0, 50);
	normal_estimator.setInputCloud(cloud_device);
	normal_estimator.compute(normals_device);
	normals_device.download(normals_p->points);

	for (int i = 0; i < normals_p->size(); i++)
	{
		pcl::Normal normal;

		normal.normal_x = normals_p->points[i].x;
		normal.normal_y = normals_p->points[i].y;
		normal.normal_z = normals_p->points[i].z;

		source_normals->push_back(normal);
	}
	//=========================================================//


	std::cout << "Calculate normals in " << time.toc() << " ms" << std::endl;
	time.tic();

	cens.setInputCloud(cloud_source);
	cens.setInputTarget(cloud_target);
	cens.setSourceNormals(source_normals);
	cens.determineCorrespondences(correspondence);



	//ces.setInputCloud(cloud);
	//ces.setInputTarget(cloud_target);
	//ces.determineCorrespondences(correspondence);

	ofstream outFile;
	outFile.open("D:\\data.csv", ios::out); // ´ò¿ªÄ£Ê½¿ÉÊ¡ÂÔ
	for (int i = 0; i < cloud_source->size(); i++)
	{
		pcl::PointXYZI p;

		p.x = cloud_source->points[i].x;
		p.y = cloud_source->points[i].y;
		p.z = cloud_source->points[i].z;

		float delta_x = cloud_target->points[correspondence[i].index_match].x - p.x;
		float delta_y = cloud_target->points[correspondence[i].index_match].y - p.y;
		float delta_z = cloud_target->points[correspondence[i].index_match].z - p.z;

		float distance =
			source_normals->points[i].normal_x * delta_x +
			source_normals->points[i].normal_y * delta_y +
			source_normals->points[i].normal_z * delta_z;

		p.intensity = (distance<0.15 && distance > -0.15) ? distance : 0;

		p.z = p.intensity * 100;

		//outFile << p.x << ',' << p.y << ',' << p.z << "," << p.intensity << endl;

		cloud_c->push_back(p);
	}
	outFile.close();

	std::cout << "Calculate distance in " << time.toc() << " ms" << std::endl;

	//չʾµãÔÆ
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor(cloud_c, "intensity");//°´ÕÕintensity×ֶνøÐÐäÖȾ
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Normals"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZI>(cloud_c, fildColor, "sample");//ÏÔʾµãÔÆ£¬ÆäÖÐfildColorΪÑÕÉ«ÏÔʾ
	//viewer->addPointCloudNormals<pcl::PointXYZI, pcl::Normal>(cloud_c, source_normals, 10, 0.05, "no");//ʵÏÖ¶ÔµãÔÆ·¨ÏßµÄÏÔʾ
	while (!viewer->wasStopped())
	{
		viewer->spinOnce();
	}
	return ;
}

int main()
{

	//step1("D:\\test\\Sample.pcd");

	//step1_gpu("D:\\test\\Sample.pcd");

	//step2("D:\\test\\Plane.pcd");

	step3("D:\\test\\Mesh.ply", "D:\\test\\stand2.ply", false, false);

	/*if (pcl::io::loadPLYFile("D:\\test\\mesh_trans.ply", *cloud_trans) < 0)
	{
		PCL_ERROR("Error loading cloud %s.\n", "D:\\test\\mesh_trans.ply");
		return 0;
	}*/

	/*pcl::RandomSample<pcl::PointXYZ> rs;
	rs.setSample(1000);
	rs.setInputCloud(cloud_trans);
	rs.filter(*cloud_trans);*/

	/*if (pcl::io::loadPLYFile("D:\\test\\stand2.ply", *cloud_target) < 0)
	{
		PCL_ERROR("Error loading cloud %s.\n", "D:\\test\\stand2.ply");
		return 0;
	}

	step4(cloud_trans, cloud_target);*/

	return 0;
   
}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started: 
//   1. Use the Solution Explorer window to add/manage files
//   2. Use the Team Explorer window to connect to source control
//   3. Use the Output window to see build output and other messages
//   4. Use the Error List window to view errors
//   5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
//   6. In the future, to open this project again, go to File > Open > Project and select the .sln file