ICP实现代码

本代码由opencv实现，每一步骤都附有详细的说明，文件包中除了实现代码之外还包含一篇ICP的经典文章，注释中的公式指的就是文章中的公式

#include “ICP.h“

void ReadPoint3D（const char *filename vector &P）
{
	FILE *fp = fopen（filename “r“）;
	char str[10];

	while（fscanf（fp “%s“ str） != EOF）
	{
		if（strcmp（str “v“） == 0）
		{
			Point3D p;
			fscanf（fp “%lf%lf%lf%d%d%d“ &p.x &p.y &p.z &p.r &p.g &p.b）;

			P.push_back（p）;
		}
	}

	fclose（fp）;
}

void SelectPoint3D（const char *filename vector &P int ratio）
{
	FILE *fp = fopen（filename “r“）;
	char str[10];
	int i=0;

	while（fscanf（fp “%s“ str） != EOF）
	{
		if（strcmp（str “v“） == 0）
		{
			Point3D p;
			fscanf（fp “%lf%lf%lf%d%d%d“ &p.x &p.y &p.z &p.r &p.g &p.b）;

			if（i%ratio == 0）
				P.push_back（p）;

			i++;
		}
	}

	fclose（fp）;
}

void ICP（vector &model vector &data double *R double *T double e）
{
	vector Y P;
	vector::iterator it1 it2;
	double pre_d = 0.0 d = 0.0;
	int round = 0;
	P = data;

	do 
	{
		pre_d = d;

		double R1[9] T1[3];

		FindClosestPointSet（model P Y）;//寻求model到data中的对应点，并把它存在Y中

		Point3D _mean_P _mean_Y;

		CalculateMeanPoint3D（P _mean_P）;//求质心坐标
		CalculateMeanPoint3D（Y _mean_Y）;

		double m[9] A[9] delta[3];

		for（int i=0; i<9; i++）
		{
			m[i] = 0;
		}

		it1 = P.begin（）;
		it2 = Y.begin（）;
		for（; it1!=P.end（）; it1++ it2++）
		{
			double p[3] y[3] mul[9];

			p[0] = it1->x;
			p[1] = it1->y;
			p[2] = it1->z;

			y[0] = it2->x;
			y[1] = it2->y;
			y[2] = it2->z;

			MatrixMul（p y mul 3 1 13）;//mul= p*y
			MatrixAdd（m mul 3 3）;//m=m+mul
		}

		for（int i=0; i<9; i++）
		{
			m[i] = m[i]/（double）P.size（）;///求均值
		}

		double mean_P[3] mean_Y[3] mul[9];

		mean_P[0] = _mean_P.x;
		mean_P[1] = _mean_P.y;
		mean_P[2] = _mean_P.z;

		mean_Y[0] = _mean_Y.x;
		mean_Y[1] = _mean_Y.y;
		mean_Y[2] = _mean_Y.z;

		MatrixMul（mean_P mean_Y mul 3 1 1 3）;//mul=mean_P*mean_Y
		MatrixDiv（m mul 3 3）;//m=m-mul，
		//从75行到107行相当于单位四元素中公式（22），（23）的求解过程
		//接下来求反对称矩阵及其特征值特征向量

		double m_T[9];
		MatrixTran（m m_T 3 3）;///求m的转置

		for（int i=0; i<9; i++）
		{
			A[i] = m[i];
		}//A=m
		//公式（25）中delta的求解过程
		MatrixDiv（A m_T 3 3）;//A=A-m_T

		delta[0] = A[5];
		delta[1] = A[6];
		delta[2] = A[1];

		//公式（25）中主对角线上元素的求解
		double tr = MatrixTR（m 3）;//求m的迹
		MatrixAdd（m m_T 3 3）;///m=m+m_T

		double I3[9] = {1 0 0 0 1 0 0 0 1};
		I3[0] = tr;
		I3[4] = tr;
		I3[8] = tr;

		MatrixDiv（m I3 3 3）;//m=m-I3

		double Q[16];
		Q[0] = tr;

		for（int i=1; i<=3; i++）
			Q[i] = delta[i-1];

		for（int i=1; i<=3; i++）
			Q[i*4] = delta[i-1];

		for（int i=1; i<=3; i++）
		{
			Q[i*4 + 1] = m[4*（i-1） + 0];
			Q[i*4 + 2] = m[4*（i-1） + 1];
			Q[i*4 + 3] = m[4*（i-1） + 2];
		}
		//求公式（25）中反对称矩阵的最大特征值及其特征向量
		double eigen qr[4];
		MatrixEigen（Q &eigen qr 4）;
		//根据特征向量求旋转矩阵，公式（21）
		CalculateRotation（qr R1）;

		//158到167行为求平移向量的过程，公式（26）
		double qt[3];
		for（int i=0; i<

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2013-11-05 21:34  ICP1\
     文件     1716108  2013-04-15 16:23  ICP1\A method for regitration of 3D modle.pdf
     目录           0  2013-11-01 14:53  ICP1\Debug\
     文件      557568  2013-11-01 15:19  ICP1\Debug\ICP1.exe
     文件     1265108  2013-11-01 15:19  ICP1\Debug\ICP1.ilk
     文件     2788352  2013-11-01 15:19  ICP1\Debug\ICP1.pdb
     文件        8636  2013-11-04 14:49  ICP1\ICP.cpp
     文件        1226  2012-04-29 20:41  ICP1\ICP.h
     目录           0  2013-10-30 21:06  ICP1\ICP1\
     目录           0  2013-11-01 15:19  ICP1\ICP1\Debug\
     文件       34908  2013-11-01 14:53  ICP1\ICP1\Debug\CL.read.1.tlog
     文件        1020  2013-11-01 14:53  ICP1\ICP1\Debug\CL.write.1.tlog
     文件      236258  2013-11-01 15:19  ICP1\ICP1\Debug\ICP.obj
     文件        2360  2013-11-01 14:53  ICP1\ICP1\Debug\ICP1.Build.CppClean.log
     文件         406  2013-11-01 14:53  ICP1\ICP1\Debug\ICP1.exe.embed.manifest
     文件         472  2013-11-01 14:53  ICP1\ICP1\Debug\ICP1.exe.embed.manifest.res
     文件         381  2013-11-01 15:19  ICP1\ICP1\Debug\ICP1.exe.intermediate.manifest
     文件          79  2013-11-01 15:19  ICP1\ICP1\Debug\ICP1.lastbuildstate
     文件         812  2013-11-01 15:19  ICP1\ICP1\Debug\ICP1.log
     文件         198  2013-11-01 14:53  ICP1\ICP1\Debug\ICP1_manifest.rc
     文件        1910  2013-11-01 14:53  ICP1\ICP1\Debug\cl.command.1.tlog
     文件           2  2013-11-01 14:53  ICP1\ICP1\Debug\link-cvtres.read.1.tlog
     文件           2  2013-11-01 14:53  ICP1\ICP1\Debug\link-cvtres.write.1.tlog
     文件           2  2013-11-01 14:53  ICP1\ICP1\Debug\link.12136-cvtres.read.1.tlog
     文件           2  2013-11-01 14:53  ICP1\ICP1\Debug\link.12136-cvtres.write.1.tlog
     文件           2  2013-11-01 14:53  ICP1\ICP1\Debug\link.12136.read.1.tlog
     文件           2  2013-11-01 14:53  ICP1\ICP1\Debug\link.12136.write.1.tlog
     文件        1820  2013-11-01 14:53  ICP1\ICP1\Debug\link.command.1.tlog
     文件        3588  2013-11-01 14:53  ICP1\ICP1\Debug\link.read.1.tlog
     文件        1016  2013-11-01 14:53  ICP1\ICP1\Debug\link.write.1.tlog
     文件       91065  2013-11-01 15:19  ICP1\ICP1\Debug\main.obj
............此处省略15个文件信息