迭代最近点算法实现两个三维模型的配准。
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');