/*=================================================================
?*
?*?ICP?algorithm?implemented?in?c++
?*
?*?written?by
?*
?*?Per?Bergstr鰉?2007-10-09
?*
?*?email:?per.bergstrom@ltu.se
?*
?*?Uses?kd-tree?written?by?Guy?Shechter
?*?http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=4586&objectType=file
?*
?*=================================================================*/

#include?
#include?“mex.h“
#include?

#include?“kdtree_common.h“
#include?“kdtree_common.cc“


/*?Input?Arguments?*/

#define	model	prhs[0]
#define	data	prhs[1]
#define	weights	prhs[2]
#define	randvec	prhs[3]
#define	sizerand	prhs[4]
#define	treeptr	prhs[5]
#define	iter	prhs[6]


/*?Output?Arguments?*/

#define	TR	plhs[0]
#define	TT?????plhs[1]

#if?!defined（MAX）
#define	MAX（A?B）	（（A）?>?（B）???（A）?:?（B））
#endif

#if?!defined（MIN）
#define	MIN（A?B）	（（A）?#endif

double?pwr2（double?a）{
????return?a*a;
}

void?icp（
double?*trpr
double?*ttpr
double?*modelz
unsigned?int?nmodelz
double?*dataz
double?*qltyz
unsigned?int?ndataz
unsigned?int?*randvecz
unsigned?int?nrandvecz
unsigned?int?nrandz
unsigned?int?iimax
double?*pointer_to_tree
）
{
????
????unsigned?int?iitempjkiicou=0;
????double?R[9]T[3]distcc;
????double?datam[3]modelm[3]MM[3]Spx[9]quasum;
????unsigned?short?int?bol=1;
????double?SIGMA[3];
????double?SpxTSpx[6];
????double?ABC;
????double?sqrtA23B;
????double?x0f0;
????double?SIpdifl;
????double?invmat[6];
????double?V[9];
????double?U[9];
????Tree?*tree;
????
????tree?=?（Tree?*）?（（long）?pointer_to_tree[0]）;
????
????trpr[0]=1.0;
????trpr[4]=1.0;
????trpr[8]=1.0;
????
????if（（nrandz+1）>ndataz）{
????????bol=0;
????}
????
????for（ii=0;ii????????
????????for（i=0;i<9;i++）{
????????????Spx[i]=0.0;
????????}
????????for（i=0;i<3;i++）{
????????????modelm[i]=0.0;
????????????datam[i]=0.0;
????????}
????????
????????quasum=0.0;
????????for（itemp=0;itemp????????????
????????????if（bol）{
????????????????if（itemp????????????????????i=randvecz[cou];
????????????????????
????????????????????if（cou<（nrandvecz-1））{
????????????????????????cou++;
????????????????????}
????????????????????else{
????????????????????????cou=0;
????????????????????}
????????????????}
????????????????else{
????????????????????break;
????????????????}
????????????}
????????????else{
????????????????i=itemp;
????????????}
????????????
????????????if（qltyz[i]>0.0）{
????????????????
????????????????T[0]=trpr[0]*dataz[3*i]+trpr[3]*dataz[3*i+1]+trpr[6]*dataz[3*i+2]+ttpr[0];
????????????????T[1]=trpr[1]*dataz[3*i]+trpr[4]*dataz[3*i+1]+trpr[7]*dataz[3*i+2]+ttpr[1];
????????????????T[2]=trpr[2]*dataz[3*i]+trpr[5]*dataz[3*i+1]+trpr[8]*dataz[3*i+2]+ttpr[2];
????????????????
????????????????distcc=0.0;
????????????????A=0.0;
????????????????
????????????????run_queries（

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件???????17752??2008-04-24?15:40??icpCpp.cpp
?????文件???????20480??2008-04-24?15:41??icpCpp.dll
?????文件????????1275??2008-04-24?15:37??icpCpp.m
?????文件????????1822??2008-04-24?15:37??icp_demo.m
?????文件????????5441??2008-04-24?15:37??kdtree.cc
?????文件???????10240??2008-04-24?15:41??kdtree.dll
?????文件????????2098??2008-04-24?15:37??kdtree.m
?????文件????????8798??2008-04-24?15:37??kdtree_common.cc
?????文件????????2391??2008-04-24?15:37??kdtree_common.h
?????文件????????1335??2009-05-28?14:10??license.txt
?????文件?????????101??2008-04-24?15:37??make.m
?????文件?????????660??2008-04-24?15:37??readme.txt