matlab实现icp算法,基于icp算法的图像配准的matlab实现.docx

基于icp算法的图像配准的matlab实现.docx

function[TR,TT]=icp(model,data,max_iter,min_iter,fitting,thres,init_flag,tes_flag,refpnt)%ICPIterativeClosestPointAlgorithm.Takesuseof%Delaunaytesselationofpointsinmodel.%%Ordinaryusage:%%[R,T]=icp(model,data)%%ICPfitpointsindatatothepointsinmodel.%Fitwithrespecttominimizethesumofsquare%errorswiththeclosestmodelpointsanddatapoints.%%:%%model-matrixwithmodelpoints,[Pm_1Pm_2.Pm_nmod]%data-matrixwithdatapoints,[Pd_1Pd_2.Pd_ndat]%%OUTPUT:%%R-rotationmatrixand%T-translationvectoraccordinglyso%%newdata=R*data+T.%%newdataaretranseddatapointstofitmodel%%%Specialusage:%%icp(model)oricp(model,tes_flag)%%ICPcreatesaDelaunaytessellationofpointsin%modelandsaveitasglobalvariableTes.ICPalso%savestwoglobalvariablesirandjcfortes_flag=1(default)or%TesindandTesverfortes_flag=2,which%makesiteasytofindinthetesselation.Tousetheglobalvariables%inicp,puttes_flagto0.%%%Otherusage:%%[R,T]=icp(model,data,max_iter,min_iter,.%fitting,thres,init_flag,tes_flag)%%:%%max_iter-maximumnumberofiterations.Default=104%%min_iter-minimumnumberofiterations.Default=4%%fitting-=2Fitwithrespecttominimizethesumofsquareerrors.(default)%alt.=[2,w],wherewisaweightvectorcorrespondingtodata.%wisavectorofsamelengthasdata.%Fitwithrespecttominimizetheweightedsumofsquareerrors.%=3Fitwithrespecttominimizethesumtotheamount0.95%oftheclosestsquareerrors.%alt.=[3,lambda],0.09warning( Toomanyarguments! );endifisempty(tes_flag)tes_flag=1;elseifnot(tes_flag==0|tes_flag==1|tes_flag==2|tes_flag==3)init_flag=1;warning( init_flaghasbeenchangedto1 );endifand((size(MODEL,2)==size(MODEL,1)),tes_flag~=0)error( Thisicpdemandsthenumberofmodelpointstobegreaterthenthedimension. );endifisempty(min_iter)min_iter=4;endifisempty(max_iter)max_iter=100+min_iter;endifmax_iter1iffi11iffitting(2)>floor(fitting(2))fitting(2)=floor(fitting(2));warning([ lambdahasbeenchangedto ,num2str(fitting(2)), ! ]);endiffitting(2)>size(data,2)fitting(2)=size(data,2);warning([ lambdahasbeenchangedto ,num2str(fitting(2)), ! ]);endelseiffitting(2)>0iffitting(2)size(refpnt,1))refpnt=refpnt ;endif(size(refpnt,1)~=size(DATA,1))ifnot(isempty(refpnt))refpnt=[];warning( Dimensionofrefpntdismatch.refpntisputto[](emptymatrix). );endendif(size(refpnt,2)>1)refpnt=refpnt(:,1);warning( Onlythefirstpointinrefpntisused. );end%StarttheICPalgorithmN=size(DATA,2);icp_init(init_flag,fitting);%initiateastartingrotationmatrixandstartingtranslationvectortes_flag=icp_struct(tes_flag);%constructaDelaunaytesselationandtwovectorsmakeiteasytofindinTesERROR=icp_closest_start(tes_flag,fitting);%initiateavectorwithindicesofclosestMODELpointsicp_transation(fitting,[]);%findtransationDATA=TR*data;%applytransationDATA=DATA+repmat(TT,1,N);%foriter=1:max_iterolderror=ERROR;ERROR=icp_closest(tes_flag,fitting);%findindicesofclosestMODELpointsandtotalerrorifiter=min_iterifabs(olderror-ERROR)1e-8) MODELred=onbasT*MODEL;ifsize(MODELred,1)==1[so1,ind1]=sort(MODELred);Tes=zeros(n,2);Tes(1:(n-1),1)=ind1(2:n) ;Tes(2:n,2)=ind1(1:(n