matlab实现ICP（3D迭代最近点算法）

迭代最近点算法实现两个三维模型的配准。

function1：

读取三维模型，函数输入为 ：

loadoff('源文件名')
sourceF=ans.TRIV;
sourceV=ones(n,3);%n为三维模型点数
sourceV(:,1)=ans.X;
sourceV(:,2)=ans.Y;
sourceV(:,3)=ans.Z;

loadoff('目标文件名')
targetF=ans.TRIV;
targetV=ones(n,3);
targetV(:,1)=ans.X;
targetV(:,2)=ans.Y;
targetV(:,3)=ans.Z;

function shape = loadoff(filename)

 

shape = [];

 

f = fopen(filename, 'rt');

 

n = '';

while isempty(n)

    fgetl(f);

    n= sscanf(fgetl(f), '%d %d %d');

end

   

nv = n(1);

nt = n(2);

data = fscanf(f, '%f');

 

if(length(data) == nv*3 + nt*3)

   numsInTri = 3;

else

   if(length(data) == nv*3 + nt*4)

       numsInTri = 4;

   else

       error('file format not supported');

   end

end

 

 

shape.TRIV =reshape(data(end-numsInTri*nt+1:end), [4 nt])';

if(numsInTri ==4)

   shape.TRIV = shape.TRIV(:,2:4);

end

if(shape.TRIV(1) == 0)

   shape.TRIV = shape.TRIV + 1;

end

 

 

data = data(1:end-numsInTri*nt);

 

data = reshape(data, [length(data)/nv nv]);

 

shape.X = data(1,:)';

shape.Y = data(2,:)';

shape.Z = data(3,:)';

 

fclose(f);

function2：

function[Prealligned_source,Prealligned_target,transformtarget ]=Preall(target,source)

 

% This function performs a first and roughpre-alligment of the data as starting position for the iterative allignment andscaling procedure

 

% Initial positioning of the data is basedon alligning the coordinates of the objects -which are assumed to beclose/similar in shape- following principal component analysis

 

[COEFF,Prealligned_source] =princomp(source);

 

[COEFF,Prealligned_target] =princomp(target);

 

% the direction of the axes is thanevaluated and corrected if necesarry.

Maxtarget=max(Prealligned_source)-min(Prealligned_source);

Maxsource=max(Prealligned_target)-min(Prealligned_target);

D=Maxtarget./Maxsource;

D=[D(1,1) 0 0;0 D(1,2) 0; 0 0 D(1,3)];

RTY=Prealligned_source*D;

 

load R

for i=1:8

   T=R{1,i};

   T=RTY*T;

   [bb DD]=knnsearch(T,Prealligned_target);

   MM(i,1)=sum(DD);

end

 

[M I]=min(MM);

 T=R{1,I};

 Prealligned_source=Prealligned_source*T;

 

 [d,Z,transformtarget] =procrustes(target,Prealligned_target);

 

function[error,Reallignedsource]=ICPmanu_allign2(target,source,Indices_edgesS,Indices_edgesT)

 

[IDX1(:,1),IDX1(:,2)]=knnsearch(target,source);

[IDX2(:,1),IDX2(:,2)]=knnsearch(source,target);

IDX1(:,3)=1:length(source(:,1));

IDX2(:,3)=1:length(target(:,1));

 

[C,ia]=setdiff(IDX2(:,1),Indices_edgesS);

IDX2=IDX2(ia,:);

 

[C,ia]=setdiff(IDX1(:,1),Indices_edgesT);

IDX1=IDX1(ia,:);

 

m1=mean(IDX1(:,2));

s1=std(IDX1(:,2));

IDX2=IDX2(IDX2(:,2)<(m1+1.96*s1),:);

 

Datasetsource=vertcat(source(IDX1(:,3),:),source(IDX2(:,1),:));

Datasettarget=vertcat(target(IDX1(:,1),:),target(IDX2(:,3),:));

 

[error,Reallignedsource,transform] = procrustes(Datasettarget,Datasetsource);

Reallignedsource=transform.b*source*transform.T+repmat(transform.c(1,1:3),size(source,1),1);

function3：

输入为[Indices_edgesS]=detectedges(sourceV,sourceF) 和[Indices_edgesT]=detectedges(targetV,targetF)

function[Indices_edges]=detectedges(V,F)

fk1 = F(:,1);

fk2 = F(:,2);

fk3 = F(:,3);

 

ed1=sort([fk1 fk2 ]')';

ed2=sort([fk1 fk3 ]')';

ed3=sort([fk2 fk3 ]')';

 

%single edges

ed=[ed1 ;ed2 ;ed3];

[etemp1,ia,ic]=unique(ed,'rows','stable');

esingle=ed(ia,:);

 

%dubbles

edouble=removerows(ed,ia);

 

C = setdiff(esingle,edouble,'rows');

 

Indices_edges=reshape(C,size(C,1)*2,1);

function4：

配准并显示，函数输入为：[error,Reallignedsource,transform]=rigidICP(targetV,sourceV,flag,Indices_edgesS,Indices_edgesT)

function[error,Reallignedsource,transform]=rigidICP(target,source,flag,Indices_edgesS,Indices_edgesT)

 

% This function rotates, translates andscales a 3D pointcloud "source" of N*3 size (N points in N rows, 3collumns for XYZ)

% to fit a similar shaped point cloud"target" again of N by 3 size

%

% The output shows the minimized value ofdissimilarity measure in "error", the transformed source data set andthe

% transformation, rotation, scaling andtranslation in transform.T, transform.b and transform.c such that

% Reallignedsource = b*source*T + c;

 

if flag==0

[Prealligned_source,Prealligned_target,transformtarget]=Preall(target,source);

else

   Prealligned_source=source;

   Prealligned_target=target;

end

 

display ('error')

errortemp(1,:)=100000;

index=2;

[errortemp(index,:),Reallignedsourcetemp]=ICPmanu_allign2(Prealligned_target,Prealligned_source,Indices_edgesS,Indices_edgesT);

 

while(errortemp(index-1,:)-errortemp(index,:))>0.000001

[errortemp(index+1,:),Reallignedsourcetemp]=ICPmanu_allign2(Prealligned_target,Reallignedsourcetemp,Indices_edgesS,Indices_edgesT);

index=index+1;

d=errortemp(index,:)

 

end

 

error=errortemp(index,:);

 

if flag==0

Reallignedsource=Reallignedsourcetemp*transformtarget.T+repmat(transformtarget.c(1,1:3),length(Reallignedsourcetemp(:,1)),1);

[d,Reallignedsource,transform] =procrustes(Reallignedsource,source);

else

  [d,Reallignedsource,transform] =procrustes(Reallignedsourcetemp,source);

end

 %显示配准结果

figure(1)

 %load ftarget

 load sourceF

 load targetF

% [error,Reallignedsource]=ICPmanu_allign2(target,source,Indices_edgesS,Indices_edgesT);

 trisurf(targetF,target(:,1),target(:,2),target(:,3),'facecolor','y','Edgecolor','none');

 hold

 light

 lighting phong;

 set(gca, 'visible', 'off')

 set(gcf,'Color',[1 1 0.88])

 view(90,90)

 set(gca,'DataAspectRatio',[1 11],'PlotBoxAspectRatio',[1 1 1]);

 %trisurf(fsource,source(:,1),source(:,2),source(:,3),'facecolor','m','Edgecolor','none');

 trisurf(sourceF,Reallignedsource(:,1),Reallignedsource(:,2),Reallignedsource(:,3),'facecolor','b','Edgecolor','none');