点云匹配，生成

#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <time.h>


#include<res_c_r_w.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>



using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
using namespace cv;

//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
	PointCloud::Ptr pcd_tgt,
	PointCloud::Ptr pcd_final)
{
	//int vp_1, vp_2;
	// Create a PCLVisualizer object
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	//viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
	// viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);
	viewer.addPointCloud(pcd_src, src_h, "source cloud");
	viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");
	viewer.addPointCloud(pcd_final, final_h, "final cloud");
	//viewer.addCoordinateSystem(1.0);
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

//由旋转平移矩阵计算旋转角度
void matrix2angle(Eigen::Matrix4f &result_trans, Eigen::Vector3f &result_angle)
{
	double ax, ay, az;
	if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1)
	{
		az = 0;
		double dlta;
		dlta = atan2(result_trans(0, 1), result_trans(0, 2));
		if (result_trans(2, 0) == -1)
		{
			ay = M_PI / 2;
			ax = az + dlta;
		}
		else
		{
			ay = -M_PI / 2;
			ax = -az + dlta;
		}
	}
	else
	{
		ay = -asin(result_trans(2, 0));
		ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));
		az = atan2(result_trans(1, 0) / cos(ay), result_trans(0, 0) / cos(ay));
	}
	result_angle << ax, ay, az;
}

res_c_r_w res;
void generate_data_ideal(string file_move_hole, string file_fix_hole_no_rand, Point3f tans_vec, Mat& R_idle)
{
	vector<Point3f>move_hole,fix_hole_no_rand;

	move_hole.clear();
	res.txt_read(file_move_hole, move_hole);
	//res.cout_3d_point(move_hole);

	for (int i = 0; i < move_hole.size(); i++)
	{
		Mat l = Mat(3, 1, CV_32F, &move_hole[i]);

		Mat _mat = Mat(R_idle*l).clone();
		Point3f temp = { _mat.at<float>(0, 0), _mat.at<float>(1, 0), _mat.at<float>(2, 0) };

		temp += tans_vec;

		fix_hole_no_rand.push_back(temp);
	}
	//res.cout_3d_point(fix_hole);

	res.txt_record(file_fix_hole_no_rand, fix_hole_no_rand);

	if (fix_hole_no_rand.size() > 6)
	{
		double dot_d = fix_hole_no_rand[4].dot(fix_hole_no_rand[5]);
		cout << dot_d;
	}
}

pcl::PointCloud<pcl::PointXYZ>::Ptr readCloudTxt(char* Filename, bool convert_to_miter = false)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	ifstream Points_in(Filename);
	pcl::PointXYZ tmpoint;
	if (Points_in.is_open())
	{
		while (!Points_in.eof())   //尚未到达文件结尾
		{
			Points_in >> tmpoint.x >> tmpoint.y >> tmpoint.z;

			if (convert_to_miter)
			{
				tmpoint.x /= 1000;
				tmpoint.y /= 1000;
				tmpoint.z /= 1000;
			}

			basic_cloud_ptr->points.push_back(tmpoint);
		}
	}
	basic_cloud_ptr->width = (int)basic_cloud_ptr->points.size();
	basic_cloud_ptr->height = 1;

	return basic_cloud_ptr;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr readCloudTxt(char* Filename, char* pcd_filename, bool convert_to_miter = false)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr = readCloudTxt(Filename, convert_to_miter);

	pcl::PCDWriter writer;
	writer.write(pcd_filename, *basic_cloud_ptr);

	return basic_cloud_ptr;
}

void main_generate_data()
{
	Mat R_idle;
	//给一个四元数
	//myQuaternion Q_idle = { 0.1f, 0.2f, 0.2f, 0.866025f };
	myQuaternion Q_idle = { 0.9449f, 0.1091f, 0.2182f, 0.2182f };
	//float Q_idle[4] = { 0.5, 0.5, 0.5, 0.5 };

	res.MatrixFromQuaternion(Q_idle, R_idle);
	cout << R_idle << endl;

	string file_move_hole = "move_hole.txt";
	string file_fix_hole_no_rand = "fix_hole.txt";
	Point3f tans_vec = { 10, 20, 30 };
	generate_data_ideal(file_move_hole, file_fix_hole_no_rand, tans_vec, R_idle);//利用理想矩阵将产生数据用于分析,

	vector<Point3f>fix_hole, move_hole;

	fix_hole.clear();
	//string fix_hole_file = "fix_hole_big_err.txt";
	string fix_hole_file = "fix_hole.txt";
	res.txt_read(fix_hole_file, fix_hole);

	move_hole.clear();
	string move_hole_file = "move_hole.txt";
	res.txt_read(move_hole_file, move_hole);

	Mat init_r, init_t;//svd 初值传给优化客户端，或记录下来

	res.my_svd(fix_hole, move_hole, init_r, init_t);
	res.xml_write("svd_mat_r.xml", "svd_mat_r", init_r);
	res.xml_write("svd_mat_t.xml", "svd_mat_t", init_t);

	//将svd结果记录，用于优化初值
	float siyunshu[4];

	cout << init_r;

	//计算位姿记录数据
	res.QuaternionFromMatrix(init_r, siyunshu);

	string filneme = "op_res.txt";
	ofstream out(filneme);

	out << siyunshu[0] << " " << siyunshu[1] << " " << siyunshu[2] << " " << siyunshu[3] << endl;

	float ttt[3];
	ttt[0] = (float)init_t.at<float>(0, 0);
	ttt[1] = (float)init_t.at<float>(1, 0);
	ttt[2] = (float)init_t.at<float>(2, 0);

	out << ttt[0] << " " << ttt[1] << " " << ttt[2] << endl;
	out.close();

	system("pause");
	return;
}


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

	readCloudTxt("fix_hole.txt", "fix_hole.pcd", true);
	readCloudTxt("move_hole.txt", "move_hole.pcd", true);

	//加载点云文件
	PointCloud::Ptr cloud_src_o(new PointCloud);//原点云，待配准
	pcl::io::loadPCDFile("move_hole.pcd", *cloud_src_o);
	PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
	pcl::io::loadPCDFile("fix_hole.pcd", *cloud_tgt_o);

	clock_t start = clock();
	//去除NAN点
	std::vector<int> indices_src; //保存去除的点的索引
	pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);
	std::cout << "remove *cloud_src_o nan" << endl;
	//下采样滤波
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
	voxel_grid.setLeafSize(0.012, 0.012, 0.012);
	voxel_grid.setInputCloud(cloud_src_o);
	PointCloud::Ptr cloud_src(new PointCloud);
	voxel_grid.filter(*cloud_src);
	std::cout << "down size *cloud_src_o from " << cloud_src_o->size() << "to" << cloud_src->size() << endl;
	pcl::io::savePCDFileASCII("bunny_src_down.pcd", *cloud_src);
	//计算表面法线
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;
	ne_src.setInputCloud(cloud_src);
	pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
	ne_src.setSearchMethod(tree_src);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
	ne_src.setRadiusSearch(0.02);
	ne_src.compute(*cloud_src_normals);

	std::vector<int> indices_tgt;
	pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);
	std::cout << "remove *cloud_tgt_o nan" << endl;

	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;
	voxel_grid_2.setLeafSize(0.01, 0.01, 0.01);
	voxel_grid_2.setInputCloud(cloud_tgt_o);
	PointCloud::Ptr cloud_tgt(new PointCloud);
	voxel_grid_2.filter(*cloud_tgt);
	std::cout << "down size *cloud_tgt_o.pcd from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl;
	pcl::io::savePCDFileASCII("bunny_tgt_down.pcd", *cloud_tgt);

	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;
	ne_tgt.setInputCloud(cloud_tgt);
	pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
	ne_tgt.setSearchMethod(tree_tgt);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
	//ne_tgt.setKSearch(20);
	ne_tgt.setRadiusSearch(0.02);
	ne_tgt.compute(*cloud_tgt_normals);

	//计算FPFH
	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src;
	fpfh_src.setInputCloud(cloud_src);
	fpfh_src.setInputNormals(cloud_src_normals);
	pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);
	fpfh_src.setSearchMethod(tree_src_fpfh);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
	fpfh_src.setRadiusSearch(0.05);
	fpfh_src.compute(*fpfhs_src);
	std::cout << "compute *cloud_src fpfh" << endl;

	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
	fpfh_tgt.setInputCloud(cloud_tgt);
	fpfh_tgt.setInputNormals(cloud_tgt_normals);
	pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
	fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
	fpfh_tgt.setRadiusSearch(0.05);
	fpfh_tgt.compute(*fpfhs_tgt);
	std::cout << "compute *cloud_tgt fpfh" << endl;

	//SAC配准
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
	scia.setInputSource(cloud_src);
	scia.setInputTarget(cloud_tgt);
	scia.setSourceFeatures(fpfhs_src);
	scia.setTargetFeatures(fpfhs_tgt);
	//scia.setMinSampleDistance(1);
	//scia.setNumberOfSamples(2);
	//scia.setCorrespondenceRandomness(20);
	PointCloud::Ptr sac_result(new PointCloud);
	scia.align(*sac_result);
	std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore() << endl;
	Eigen::Matrix4f sac_trans;
	sac_trans = scia.getFinalTransformation();
	std::cout << sac_trans << endl;
	pcl::io::savePCDFileASCII("bunny_transformed_sac.pcd", *sac_result);
	clock_t sac_time = clock();

	//icp配准
	PointCloud::Ptr icp_result(new PointCloud);
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_src_o);
	icp.setInputTarget(cloud_tgt_o);
	//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance(0.004);
	// 最大迭代次数
	icp.setMaximumIterations(50);
	// 两次变化矩阵之间的差值
	icp.setTransformationEpsilon(1e-10);
	// 均方误差
	icp.setEuclideanFitnessEpsilon(0.2);
	icp.align(*icp_result, sac_trans);

	clock_t end = clock();
	cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
	//我把计算法线和点特征直方图的时间也算在SAC里面了
	cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl;
	cout << "icp time: " << (double)(end - sac_time) / (double)CLOCKS_PER_SEC << " s" << endl;

	//std::cout << "ICP has converged:" << icp.hasConverged()
	//	<< " score: " << icp.getFitnessScore() << std::endl;
	Eigen::Matrix4f icp_trans;
	icp_trans = icp.getFinalTransformation();
	//cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl;
	std::cout << icp_trans << endl;

	//使用创建的变换对未过滤的输入点云进行变换
	pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);
	//保存转换的输入点云
	pcl::io::savePCDFileASCII("bunny_transformed_sac_ndt.pcd", *icp_result);

	//计算误差
	Eigen::Vector3f ANGLE_origin;
	ANGLE_origin << 0, 0, M_PI / 5;
	double error_x, error_y, error_z;
	Eigen::Vector3f ANGLE_result;
	matrix2angle(icp_trans, ANGLE_result);
	error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0));
	error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1));
	error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2));
	cout << "original angle in x y z:\n" << ANGLE_origin << endl;
	cout << "error in aixs_x: " << error_x << "  error in aixs_y: " << error_y << "  error in aixs_z: " << error_z << endl;

	//可视化
	visualize_pcd(cloud_src_o, cloud_tgt_o, icp_result);
	return (0);
}