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

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

function1：函数

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

loadoff('源文件名')

sourceF=ans.TRIV;

sourceV=ones(n,3);%n为三维模型点数

sourceV(:,1)=ans.X;

sourceV(:,2)=ans.Y;

sourceV(:,3)=ans.Z;spa

loadoff('目标文件名')

targetF=ans.TRIV;

targetV=ones(n,3);

targetV(:,1)=ans.X;

targetV(:,2)=ans.Y;

targetV(:,3)=ans.Z;

component

function shape = loadoff(filename)orm

shape = [];ip

f = fopen(filename, 'rt');ci

n = '';rem

while isempty(n)get

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)

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');