ICP算法源代码拿走不谢

ICP算法的C++源代码。迭代最近点法（Iterative Closest Points Algorithm）。基本思想是：根据某种几何特性对数据进行匹配，并设这些匹配点为假想的对应点，然后根据这种对应关系求解运动参数。再利用这些运动参数对数据进行变换。并利用同一几何特征，确定新的对应关系，重复上述过程。

/*=================================================================
 *
 * 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） < （B） ? （A） : （B））
#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