#include <iostream>
#include <sstream>
#include <fstream>
#include <cassert>
#include <math.h>
#include <time.h>
#include <newmat.h>
#include <newmatap.h>
#include "ICP_H.h"

ICP::ICP()
{
}

ICP::ICP(int controlnum, double thre, int iter)
{
	conNum = controlnum;
	threshold = thre;
	iterate = iter;

	contP = new Vertex[conNum];
	assert(contP != NULL);
	contQ = new Vertex[conNum];
	assert(contQ != NULL);
	rmcoP = new Vertex[conNum];
	assert(rmcoP != NULL);
	rmcoQ = new Vertex[conNum];
	assert(rmcoQ != NULL);
	index = new int[conNum];
	assert(index != NULL);
}

ICP::~ICP()
{
	delete[] contP;
	delete[] contQ;
	delete[] rmcoP;
	delete[] rmcoQ;
	delete[] index;
}

void ICP::readfile(std::string firstname, std::string secondname)
{
	std::cout << "读取两个点云文件！！" << std::endl;
	ifstream in;
	in.open(firstname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" << std::endl;
		system("pause");
	}
	Vertex v;
	while (in >> v.coord[0] >> v.coord[1] >> v.coord[2])
	{
		VarrP.push_back(v);
	}
	std::cout << "点云A的大小：" << VarrP.size() << std::endl;
	in.close();

	//
	in.open(secondname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		cout << "error open!" << endl;
		system("pause");
	}

	//Vertex v;
	while (in >> v.coord[0] >> v.coord[1] >> v.coord[2])
	{
		VarrQ.push_back(v);
	}
	std::cout << "点云B的大小：" << VarrQ.size() << std::endl;
	in.close();
}
void ICP::run()
{
	initransmat();
	sample();
	//
	double err = closest();
	std::cout << "初始误差：error = " << err << std::endl;
	//
	for (int i = 0; i < iterate; i++)
	{
		getcenter();
		rmcontcenter();
		transform();
		uprotate();
		uptranslate();
		updata();
		double newerr = closest();
		std::cout << "迭代次数 times = " << i << std::endl;
		std::cout << "error = " << newerr << std::endl;
		double delta = fabs(err - newerr) / conNum;
		std::cout << "delta = " << delta << std::endl;
		if (delta < threshold)
			break;
		err = newerr;

	}
	printTR();
	printTT();
	applyall();
}
void ICP::writefile(std::string name)
{
	ofstream outobj;
	outobj.open(name.c_str());
	//outobj << "# Geomagic Studio" << endl;
	int num = 1;
	for (vector<Vertex>::const_iterator p = VarrP.begin(); p != VarrP.end(); p++)
	{
		Vertex v;
		v = *p;
		outobj << v.coord[0] << " " << v.coord[1] << " " << v.coord[2] << endl;
		//outobj << "p " << num++ << endl;

		//outobj << "v " << v.coord[0] << " " << v.coord[1] << " " << v.coord[2] << endl;
		//outobj << "p " << num++ << endl;
	}
	//
	outobj.close();
}

//初始化变换矩阵
//    -
//    | 1.0  0.0 0.0 | 0.0  |
//	  |	0.0  1.0 0.0 | 0.0  |
//	  | 0.0  0.0 1.0 | 0.0  |
//    | -------------|----- |
//    | 0.0 0.0 0.0  | 1.0  |
void ICP::initransmat()//初始化变换矩阵
{
	std::cout << "初始化变换矩阵" << endl;
	for (int i = 0; i < 3; i++)
		TT[i] = 0;
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			if (i != j)
				TR[i][j] = 0.0;
			else
				TR[i][j] = 1.0;
		}
	}
}


//随机选取控制点,并存储在contP中

void ICP::sample()
{
	std::cout << "随机选取控制点,并存储在contP中" << std::endl;
	int N = VarrP.size();
	bool* flag = new bool[N];
	assert(flag != NULL);
	for (int i = 0; i < N; i++)
		flag[i] = false;
	//随机选择一个控制点，并记录其索引
	srand((unsigned)time(NULL));
	for (int i = 0; i < conNum; i++)
	{
		while (true)
		{
			int sam = rand() % N;
			if (!flag[sam])
			{
				index[i] = sam;
				flag[sam] = true;
				break;
			}
		}
	}
	//cout<<"store control points into contP"<<endl;
	for (int i = 0; i < conNum; i++)
	{
		Vertex v = VarrP[index[i]];//
		for (int j = 0; j < 3; j++)
		{
			contP[i].coord[j] = v.coord[j];
		}
	}
	delete[] flag;
}

//找出最近点并计算误差之和
double ICP::closest()
{
	//find closest points and error
	double error = 0.0;
	for (int i = 0; i < conNum; i++)
	{
		double mindist = 100.0;
		index[i] = 0;
		for (unsigned int j = 0; j < VarrQ.size(); j++)
		{
			double dist = distance(contP[i], VarrQ[j]);
			if (dist < mindist)
			{
				mindist = dist;
				index[i] = j;
			}
		}
		Vertex v = VarrQ[index[i]];
		for (int j = 0; j < 3; j++)
		{
			contQ[i].coord[j] = v.coord[j];
		}
		error += mindist;
	}
	return error;
}

//求取两个点云控制点的重心
void ICP::getcenter()
{
	//首先初始化重心坐标0，0，0
	for (int i = 0; i < 3; i++)
		meanP.coord[i] = 0.0;
	//求取每个分量之和
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			meanP.coord[j] += contP[i].coord[j];
		}
	}
	//求取平均值
	for (int i = 0; i < 3; i++)
		meanP.coord[i] = meanP.coord[i] / conNum;

	//
	for (int i = 0; i < 3; i++)
		meanQ.coord[i] = 0.0;
	//求取每个分量之和
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			meanQ.coord[j] += contQ[i].coord[j];
		}
	}
	//求取平均值
	for (int i = 0; i < 3; i++)
		meanQ.coord[i] = meanQ.coord[i] / conNum;
}

//点集中心化，生成新的点云数据
void ICP::rmcontcenter()
{
	std::cout << "点集中心化，生成新的点云数据" << std::endl;
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			rmcoP[i].coord[j] = contP[i].coord[j] - meanP.coord[j];
			rmcoQ[i].coord[j] = contQ[i].coord[j] - meanQ.coord[j];
		}
	}
}

void ICP::transform()
{
	std::cout << "获取变换矩阵" << std::endl;
	Matrix B(4, 4);
	B = 0;
	double u[3];//di+di'
	double d[3];//di-di'
	//计算协方差
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			u[j] = rmcoP[i].coord[j] + rmcoQ[i].coord[j];
			d[j] = rmcoP[i].coord[j] - rmcoQ[i].coord[j];
		}
		double uM[16] = {
			   0, -d[0], -d[1], -d[2],
			d[0],     0, -u[2], -u[1],
			d[1], -u[2],     0,  u[0],
			d[2],  u[1], -u[0],    0 };

		Matrix Ai(4, 4);
		Ai << uM;
		B += Ai * Ai.t();
	}

	Matrix U;
	Matrix V;
	DiagonalMatrix D;
	SVD(B, D, U, V);

	for (int i = 0; i < 4; i++)
	{
		quad[i] = V.element(i, 3);
	}
	B.Release();
	U.Release();
	V.Release();
	D.Release();
}

void ICP::uprotate()
{
	//根据四元数求解选择矩阵
	Rw[0][0] = quad[0] * quad[0] + quad[1] * quad[1] - quad[2] * quad[2] - quad[3] * quad[3];
	Rw[0][1] = 2 * (-quad[0] * quad[3] + quad[1] * quad[2]);
	Rw[0][2] = 2 * (quad[0] * quad[2] + quad[1] * quad[3]);
	Rw[1][0] = 2 * (quad[0] * quad[3] + quad[1] * quad[2]);
	Rw[1][1] = quad[0] * quad[0] - quad[1] * quad[1] + quad[2] * quad[2] - quad[3] * quad[3];
	Rw[1][2] = 2 * (-quad[0] * quad[1] + quad[2] * quad[3]);
	Rw[2][0] = 2 * (-quad[0] * quad[2] + quad[1] * quad[3]);
	Rw[2][1] = 2 * (quad[0] * quad[1] + quad[2] * quad[3]);
	Rw[2][2] = quad[0] * quad[0] - quad[1] * quad[1] - quad[2] * quad[2] + quad[3] * quad[3];

	//Rn+1 = R * Rn
	double tmp[3][3];
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			tmp[i][j] = 0;
		}
	}

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			for (int k = 0; k < 3; k++)
			{
				tmp[i][j] += Rw[i][k] * TR[k][j];
			}
		}
	}

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
			TR[i][j] = tmp[i][j];
	}
}

void ICP::uptranslate()
{
	//Tw = P'-Rw * P
	double tmp[3] = { 0, 0, 0 };
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			tmp[i] += Rw[i][j] * meanP.coord[j];
		}
	}
	for (int i = 0; i < 3; i++)
	{
		Tw[i] = meanQ.coord[i] - tmp[i];
	}

	double temp[3] = { 0, 0, 0 };
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			temp[i] += Rw[i][j] * TT[j];
		}
	}
	for (int i = 0; i < 3; i++)
	{
		TT[i] = temp[i] + Tw[i];
	}
}

void ICP::updata()
{
	for (int i = 0; i < conNum; i++)
	{
		double tmp[3] = { 0, 0, 0 };
		for (int j = 0; j < 3; j++)
		{
			for (int k = 0; k < 3; k++)
			{
				tmp[j] += Rw[j][k] * contP[i].coord[k];
			}
		}
		for (int j = 0; j < 3; j++)
			contP[i].coord[j] = tmp[j] + Tw[j];
	}
}

void ICP::applyall()
{
	for (vector<Vertex>::iterator p = VarrP.begin(); p != VarrP.end(); p++)
	{
		Vertex v = *p;
		double tmp[3] = { 0, 0, 0 };
		for (int i = 0; i < 3; i++)
		{
			for (int k = 0; k < 3; k++)
			{
				tmp[i] += TR[i][k] * v.coord[k];
			}
		}
		for (int i = 0; i < 3; i++)
		{
			v.coord[i] = tmp[i] + TT[i];
		}
		*p = v;
	}
}

double ICP::distance(Vertex a, Vertex b)
{
	double dist = 0.0;
	for (int i = 0; i < 3; i++)
	{
		dist += (a.coord[i] - b.coord[i]) * (a.coord[i] - b.coord[i]);
	}
	return dist;
}

void ICP::printTT()
{
	std::cout << "Translate Matrix = " << std::endl;
	for (int i = 0; i < 3; i++)
	{
		std::cout << TT[i] << " ";
	}
	std::cout << std::endl;
}

void ICP::printTR()
{
	std::cout << "Rotate Matrix = " << std::endl;
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			std::cout << TR[i][j] << " ";
		}
		std::cout << std::endl;
	}
}

void ICP::showcloud(std::string firstname, std::string secondname, std::string thirdname)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Target(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Source(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);

	std::cout << "显示两个点云：" << std::endl;
	//第一个点云数据
	ifstream in;
	in.open(firstname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" << std::endl;
		system("pause");
	}
	vector<Point> points;
	points.clear();
	Point tmp;
	while (in >> tmp.x >> tmp.y >> tmp.z)
	{
		points.push_back(tmp);
	}
	pcl::PointXYZ cltmp;

	for (size_t i = 0; i != points.size(); i++)
	{
		cltmp.x = points[i].x;
		cltmp.y = points[i].y;
		cltmp.z = points[i].z;
		cloud_Target->points.push_back(cltmp);
	}
	std::cout << "点云A的大小：" << cloud_Target->size() << std::endl;
	in.close();

	//第二个点云数据
	in.open(secondname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" << std::endl;
		system("pause");
	}
	points.clear();
	while (in >> tmp.x >> tmp.y >> tmp.z)
	{
		points.push_back(tmp);
	}
	//pcl::PointXYZ cltmp;

	for (size_t i = 0; i != points.size(); i++)
	{
		cltmp.x = points[i].x;
		cltmp.y = points[i].y;
		cltmp.z = points[i].z;
		cloud_Source->points.push_back(cltmp);
	}
	std::cout << "点云B的大小：" << cloud_Source->size() << std::endl;
	in.close();

	//第三个点云数据
	in.open(thirdname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" << std::endl;
		system("pause");
	}
	points.clear();
	while (in >> tmp.x >> tmp.y >> tmp.z)
	{
		points.push_back(tmp);
	}
	//pcl::PointXYZ cltmp;

	for (size_t i = 0; i != points.size(); i++)
	{
		cltmp.x = points[i].x;
		cltmp.y = points[i].y;
		cltmp.z = points[i].z;
		cloudOut->points.push_back(cltmp);
	}
	std::cout << "点云C的大小：" << cloudOut->size() << std::endl;
	in.close();


	//可视化初始化
	pcl::visualization::PCLVisualizer viewer;
	viewer.setCameraFieldOfView(0.785398);//fov 45°  视场角
	viewer.setBackgroundColor(0.2, 0.2, 0.2);
	viewer.setCameraPosition(
		0, 0, 0,
		0, 0, -1,
		0, 0, 0);
	//点云可视化
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> TargetHandler(cloud_Target, 255, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> SourceHandler(cloud_Source, 0, 0, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> OutHandler(cloudOut, 0, 255, 0);
	viewer.addPointCloud(cloud_Target, TargetHandler, "cloud_Target");
	viewer.addPointCloud(cloud_Source, SourceHandler, "cloud_Source");
	viewer.addCoordinateSystem(0.1, "cloud", 0);


	int v2 = 1;
	viewer.createViewPort(0.5, 0, 1, 1, v2);
	viewer.createViewPortCamera(v2);
	viewer.setCameraFieldOfView(0.785398, v2);//fov 45°
	viewer.setBackgroundColor(0.2, 0.2, 0.2, v2);
	viewer.setCameraPosition(
		0, 0, 0,
		0, 0, -1,
		0, 0, 0, v2);
	//点云可视化
	viewer.addPointCloud(cloud_Target, TargetHandler, "cloud222", v2);
	viewer.addPointCloud(cloudOut, OutHandler, "cloudOut", v2);
	viewer.addCoordinateSystem(0.1, "cloud1", v2);
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
}