// M063.3D.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include "pch.h"
#include <io.h>
#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/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/crop_box.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/surface/gp3.h>
#include <pcl/console/time.h>   // TicToc
#include <pcl/filters/random_sample.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

// 相机内参
const double camera_factor = 0.0002;
const double camera_cx = 0;
const double camera_cy = 0;
const double camera_fx = 0.015;
const double camera_fy = 0.015;

bool next_iteration;

#define PointT pcl::PointXYZ;

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;
}

void getFiles1(const std::string &path, std::vector<std::string>& files)
{
	//文件句柄  
	//long hFile = 0;  //win7
	intptr_t hFile = 0;   //win10
	//文件信息  
	struct _finddata_t fileinfo;
	std::string p;
	if ((hFile = _findfirst(p.assign(path).append("\\*").c_str(), &fileinfo)) != -1)
		// "\\*"是指读取文件夹下的所有类型的文件，若想读取特定类型的文件，以png为例，则用“\\*.png”
	{
		do
		{
			//如果是目录,迭代之  
			//如果不是,加入列表  
			if ((fileinfo.attrib &  _A_SUBDIR))
			{
				if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)
					getFiles1(p.assign(path).append("\\").append(fileinfo.name), files);
			}
			else
			{
				files.push_back(path + "\\" + fileinfo.name);
			}
		} while (_findnext(hFile, &fileinfo) == 0);
		_findclose(hFile);
	}
}

void string_replace(std::string &strBig, const std::string &strsrc, const std::string &strdst)
{
	std::string::size_type pos = 0;
	std::string::size_type srclen = strsrc.size();
	std::string::size_type dstlen = strdst.size();

	while ((pos = strBig.find(strsrc, pos)) != std::string::npos)
	{
		strBig.replace(pos, srclen, strdst);
		pos += dstlen;
	}
}

void step1(std::string &fileName)
{
	cv::Mat depth;

	depth = cv::imread(fileName, cv::ImreadModes::IMREAD_ANYDEPTH);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);	// 遍历深度图	
	for (int m = 0; m < depth.rows; m += 4)
		for (int n = 0; n < depth.cols; n += 4)
		{
			// 获取深度图中(m,n)处的值			
			ushort d = depth.ptr<ushort>(m)[n];			// d 可能没有值，若如此，跳过此点			
			if (d == 0)
				continue;			// d 存在值，则向点云增加一个点			
			pcl::PointXYZ p; 			// 计算这个点的空间坐标			
			p.z = double(d) * camera_factor;
			p.x = n  * camera_fx;
			p.y = m  * camera_fy;

			// 把p加入到点云中			
			cloud->points.push_back(p);
		}
	// 设置并保存点云	
	cloud->height = 1;
	cloud->width = cloud->points.size();
	cout << "point cloud size = " << cloud->points.size() << endl;
	cloud->is_dense = false;

	string_replace(fileName, ".tif", ".pcd");

	pcl::io::savePCDFile(fileName, *cloud);	// 清除数据并退出	
	cloud->points.clear();
	cout << "Point cloud saved." << endl;
}

void step2()
{
	std::string fileName1 = "D:\\calibration_3d_0331_2\\5_T.pcd";
	std::string fileName2 = "D:\\calibration_3d_0331_2\\6.pcd";

	std::string outFileName = "D:\\calibration_3d_0331_2\\6_T.pcd";

	// Set initial alignment estimate found using robot odometry.
	Eigen::AngleAxisf init_rotation(0, Eigen::Vector3f::UnitZ());
	Eigen::Translation3f init_translation(-200, 0, 0);
	Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName1, *target_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file 1 \n");
		return;
	}
	std::cout << "Loaded " << target_cloud->size() << " data points from "<< fileName1 << std::endl;

	// Loading second scan of room from new perspective.
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName2, *input_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file 2.pcd \n");
		return;
	}
	std::cout << "Loaded " << input_cloud->size() << " data points from" << fileName2 << std::endl;


	// ===================================删除离群点=====================================//
	//// Create the filtering object
	//pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	//sor.setMeanK(50);//50个临近点
	//sor.setStddevMulThresh(1.0);//距离大于1倍标准方差
	//sor.setInputCloud(target_cloud);
	//sor.filter(*target_cloud);
	//sor.setInputCloud(input_cloud);
	//sor.filter(*input_cloud);
	//=================================================================================//


	// Filtering input scan to roughly 10% of original size to increase speed of registration.
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
	approximate_voxel_filter.setLeafSize(0.8, 0.8, 0.8);
	approximate_voxel_filter.setInputCloud(input_cloud);
	approximate_voxel_filter.filter(*filtered_cloud);
	std::cout << "Filtered cloud contains " << filtered_cloud->size()
		<< " data points from "<< fileName2 << std::endl;

	//pcl::CropBox<pcl::PointXYZ> cropBox;
	//cropBox.setMin(Eigen::Vector4f(20, 0, 0, 1.0));
	//cropBox.setMax(Eigen::Vector4f(50, 250, 30, 1.0));
	//cropBox.setInputCloud(filtered_cloud);
	//cropBox.filter(*filtered_cloud);


	// Initializing Normal Distributions Transform (NDT).
	pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

	// Setting scale dependent NDT parameters
	// Setting minimum transformation difference for termination condition.
	ndt.setTransformationEpsilon(0.01);
	// Setting maximum step size for More-Thuente line search.
	ndt.setStepSize(1);
	//Setting Resolution of NDT grid structure (VoxelGridCovariance).
	ndt.setResolution(1);

	// Setting max number of registration iterations.
	ndt.setMaximumIterations(100);

	// Setting point cloud to be aligned.
	ndt.setInputSource(filtered_cloud);
	// Setting point cloud to be aligned to.
	ndt.setInputTarget(target_cloud);

	// Calculating required rigid transform to align the input cloud to the target cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	ndt.align(*output_cloud, init_guess);

	std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
		<< " score: " << ndt.getFitnessScore() << std::endl;
	print4x4Matrix(ndt.getFinalTransformation().cast<double>());

	// Transforming unfiltered, input cloud using found transform.
	pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());

	// Saving transformed input cloud.
	pcl::io::savePCDFile(outFileName, *output_cloud);

	// Initializing point cloud visualizer
	pcl::visualization::PCLVisualizer::Ptr
		viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer_final->setBackgroundColor(0, 0, 0);

	// Coloring and visualizing target cloud (red).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		target_color(target_cloud, 255, 0, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "target cloud");

	// Coloring and visualizing transformed input cloud (green).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		output_color(output_cloud, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "output cloud");

	// Starting visualizer
	viewer_final->addCoordinateSystem(1.0, "global");
	viewer_final->initCameraParameters();

	// Wait until visualizer window is closed.
	while (!viewer_final->wasStopped())
	{
		viewer_final->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

void step3()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Images\\1.pcd", *target_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file 1.pcd \n");
		return;
	}
	std::cout << "Loaded " << target_cloud->size() << " data points from 1.pcd" << std::endl;

	// Loading second scan of room from new perspective.
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Images\\2.pcd", *input_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file 2.pcd \n");
		return;
	}
	std::cout << "Loaded " << input_cloud->size() << " data points from 2.pcd" << std::endl;


	// ===================================删除离群点=====================================//
	// Create the filtering object
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setMeanK(50);//50个临近点
	sor.setStddevMulThresh(1.0);//距离大于1倍标准方差
	
	sor.setInputCloud(target_cloud);
	sor.filter(*target_cloud);

	sor.setInputCloud(input_cloud);
	sor.filter(*input_cloud);
	//=================================================================================//


	// Filtering input scan to roughly 10% of original size to increase speed of registration.
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
	approximate_voxel_filter.setLeafSize(0.8, 0.8, 0.8);
	approximate_voxel_filter.setInputCloud(input_cloud);
	approximate_voxel_filter.filter(*filtered_cloud);
	std::cout << "Filtered cloud contains " << filtered_cloud->size()
		<< " data points from 2.pcd" << std::endl;

	pcl::CropBox<pcl::PointXYZ> cropBox;
	cropBox.setMin(Eigen::Vector4f(15, 0, 0, 1.0));
	cropBox.setMax(Eigen::Vector4f(40, 250, 30, 1.0));
	cropBox.setInputCloud(filtered_cloud);
	cropBox.filter(*filtered_cloud);

	int iterations = 1;

	// Set initial alignment estimate found using robot odometry.
	Eigen::AngleAxisd init_rotation(0, Eigen::Vector3d::UnitZ());
	Eigen::Translation3d init_translation(41, 0, 2);
	Eigen::Matrix4d init_guess = (init_translation * init_rotation).matrix();

	pcl::PointCloud<pcl::PointXYZ>::Ptr testCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*filtered_cloud, *testCloud, init_guess);

	// The Iterative Closest Point algorithm
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

	pcl::console::TicToc time;
	time.tic();
	icp.setMaximumIterations(1);
	icp.setInputSource(testCloud);
	icp.setInputTarget(target_cloud);

	pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	icp.align(*output_cloud);

	

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

	Eigen::Matrix4d finalMat;

	if (icp.hasConverged())
	{
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
		
		finalMat = init_guess * icp.getFinalTransformation().cast<double>();
		pcl::transformPointCloud(*filtered_cloud, *output_cloud, finalMat);
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
		return;
	}

	// 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<pcl::PointXYZ> cloud_target_color_h(target_cloud, 255 ,255 ,255);
	viewer.addPointCloud(target_cloud, cloud_target_color_h, "cloud_in_v1", v1);
	viewer.addPointCloud(target_cloud, cloud_target_color_h, "cloud_in_v2", v2);

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

	// ICP aligned point cloud is red
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_output_color_h(output_cloud, 180, 20, 20);
	viewer.addPointCloud(output_cloud, cloud_output_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.setInputSource(output_cloud);
			icp.setInputTarget(target_cloud);
			icp.align(*output_cloud);
			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_icp -> cloud_in" << std::endl;
				finalMat *= icp.getFinalTransformation().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
				print4x4Matrix(finalMat);  // 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(output_cloud, cloud_output_color_h, "cloud_icp_v2");
			}
			else
			{
				PCL_ERROR("\nICP has not converged.\n");
				return;
			}
		}
		next_iteration = false;
	}

}

void step4()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inliner(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_outliner(new pcl::PointCloud<pcl::PointXYZ>);

	if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Images\\1.pcd", *target_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file 1.pcd \n");
		return;
	}
	std::cout << "Loaded " << target_cloud->size() << " data points from 1.pcd" << std::endl;

	// Create the filtering object
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(target_cloud);
	sor.setMeanK(50);//50个临近点
	sor.setStddevMulThresh(1.0);//距离大于1倍标准方差

	sor.filter(*cloud_inliner);
	
	// Initializing point cloud visualizer
	pcl::visualization::PCLVisualizer::Ptr
		viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer_final->setBackgroundColor(0, 0, 0);

	// Coloring and visualizing target cloud (red).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		target_color(target_cloud, 255, 0, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(cloud_inliner, target_color, "target cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "target cloud");

	// Coloring and visualizing transformed input cloud (green).
	/*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		output_color(output_cloud, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "output cloud");*/

	// Starting visualizer
	viewer_final->addCoordinateSystem(1.0, "global");
	viewer_final->initCameraParameters();

	// Wait until visualizer window is closed.
	while (!viewer_final->wasStopped())
	{
		viewer_final->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

}

int main()
{
	std::vector<std::string> files;

	getFiles1("D:\\1", files);

	for (int i = 0; i < files.size(); i++)
	{
		step1(files[i]);
	}

	return (0);

}

// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门提示: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件