clc
clear
close all
file1='bun045.asc';
file2='bun000.asc';
%后续--处理下文件名的约束
data1 = ascread(file1);%40097points,{1}为点数,{2}为3*点数的坐标矩阵
data2 = ascread(file2);%40256points
P = data1{2};
Q = data2{2};
k=16;
%建立法向量
pn = lsqnormest(P, k);
qn = lsqnormest(Q, k);
figure;
plot3(P(1,:),P(2,:),P(3,:),'r.');
hold on
plot3(Q(1,:),Q(2,:),Q(3,:),'b.');
title('原始数据')
view(2)
%计算特征度
pt=zeros(1,data1{1});
qt=zeros(1,data2{1});
[n1,d1] = knnsearch(transpose(P), transpose(P), 'k', 400);
n1=transpose(n1);
d1=transpose(d1);
for i=1:data1{1}
pt(1,i)=1/k*sum(abs(transpose(pn(:,n1(2:k+1,i)))*pn(:,i)));
end
[n2,d2] = knnsearch(transpose(Q), transpose(Q), 'k', 400);
n2=transpose(n2);
d2=transpose(d2);
for i=1:data2{1}
qt(1,i)=1/k*sum(abs(transpose(qn(:,n2(2:k+1,i)))*qn(:,i)));
end
%选取特征点
ptt=find(pt<mean(pt));
qtt=find(qt<mean(qt));
p0=P(:,ptt);
q0=Q(:,qtt);
figure;
plot3(p0(1,:),p0(2,:),p0(3,:),'r.');
hold on
plot3(q0(1,:),q0(2,:),q0(3,:),'b.');
title('特征度在均值以上的点')
view(2)
fep=[];feq=[];
for i=ptt
[~,I]=min(pt(1,n1(1:k+1,i)));
if I==1
fep=[fep,i];
end
end
for i=qtt
[~,I]=min(qt(1,n2(1:k+1,i)));
if I==1
feq=[feq,i];
end
end
feq0=feq;
p0=P(:,fep);
q0=Q(:,feq);
figure;
plot3(p0(1,:),p0(2,:),p0(3,:),'r.');
hold on
plot3(q0(1,:),q0(2,:),q0(3,:),'b.');
title('选取的特征点集')
view(2)
%建立特征直方图
vep=zeros(16,length(fep));
veq=zeros(16,length(feq));
r=0.005;
a=0;
for i=fep
a=a+1;
for j=2:400
nor1=pn(:,i);
nor2=pn(:,n1(j,i));
dis=P(:,i)-P(:,n1(j,i));
f1=abs(nor1'*nor2);
f2=nor1'*dis;
f3=norm(dis);
if f1>=cos(pi/9)
k1=1;
elseif f1>=cos(2*pi/9)
k1=2;
elseif f1>=cos(pi/3)
k1=3;
else k1=4;
end
if f2>0
k2=0;
else
k2=1;
end
if f3>r/2
k3=1;
else
k3=0;
end
fi=k1+k2*4+k3*8;
vep(fi,a)=vep(fi,a)+1;
if d1(j,i)>r
break
end
end
end
a=0;
for i=feq
a=a+1;
for j=2:400
nor1=qn(:,i);
nor2=qn(:,n2(j,i));
dis=Q(:,i)-Q(:,n2(j,i));
f1=abs(nor1'*nor2);
f2=nor1'*dis;
f3=norm(dis);
if f1>=cos(pi/9)
k1=1;
elseif f1>=cos(2*pi/9)
k1=2;
elseif f1>=cos(pi/3)
k1=3;
else k1=4;
end
if f2>0
k2=0;
else
k2=1;
end
if f3>r/2
k3=1;
else
k3=0;
end
fi=k1+k2*4+k3*8;
veq(fi,a)=veq(fi,a)+1;
if d2(j,i)>r
break
end
end
end
%根据直方图建立特征点匹配关系并建立刚性不变约束
[nv,d]=knnsearch(vep',veq');
nv=nv';
d=d';
p0=P(:,fep);
q0=Q(:,feq);
figure;
plot3(p0(1,:),p0(2,:),p0(3,:),'r.');
hold on
plot3(q0(1,:),q0(2,:),q0(3,:),'b.');
for i=1:length(nv)
hold on
x=[Q(1,feq(i)),P(1,fep(nv(1,i)))];
y=[Q(2,feq(i)),P(2,fep(nv(1,i)))];
z=[Q(3,feq(i)),P(3,fep(nv(1,i)))];
plot3(x,y,z,'g-');
end
title('初始的特征点匹配')
view(2)
tic
dist=zeros(size(nv));
for i=1:length(nv)
dist(1,i)=norm(Q(:,feq(i))-P(:,fep(nv(1,i))));
end
feq(dist>0.05)=[];
nv(dist>0.05)=[];
p0=P(:,fep);
q0=Q(:,feq0);
figure;
plot3(p0(1,:),p0(2,:),p0(3,:),'r.');
hold on
plot3(q0(1,:),q0(2,:),q0(3,:),'b.');
for i=1:length(nv)
hold on
x=[Q(1,feq(i)),P(1,fep(nv(1,i)))];
y=[Q(2,feq(i)),P(2,fep(nv(1,i)))];
z=[Q(3,feq(i)),P(3,fep(nv(1,i)))];
plot3(x,y,z,'g-');
end
num=zeros(size(feq));
for i=1:length(nv)
a=0;
for j=1:length(nv)
if i==j
continue
end
dq=norm(Q(:,feq(i))-Q(:,feq(j)));
dp=norm(P(:,fep(nv(i)))-P(:,fep(nv(j))));
if abs(dp-dq)/(dp+dq)<0.02
a=a+1;
end
end
num(1,i)=a;
end
num1=sort(num,'descend');
feq(num<num1(20))=[];
nv(num<num1(20))=[];
p0=P(:,fep);
q0=Q(:,feq0);
figure;
plot3(p0(1,:),p0(2,:),p0(3,:),'r.');
hold on
plot3(q0(1,:),q0(2,:),q0(3,:),'b.');
for i=1:length(nv)
hold on
x=[Q(1,feq(i)),P(1,fep(nv(1,i)))];
y=[Q(2,feq(i)),P(2,fep(nv(1,i)))];
z=[Q(3,feq(i)),P(3,fep(nv(1,i)))];
plot3(x,y,z,'g-');
end
title('通过刚性不变约束筛选后的特征匹配情况')
view(2)
%使用随机抽样一致性算法RANSAC确定匹配关系
aa=200;
b0=0;
c0=zeros(size(feq));
while aa
n=length(nv);
a=randperm(n);
feq1=feq(a(1:3));
nv1=nv(a(1:3));
[R,T]=Quater_Registration(Q(:,feq1)',P(:,fep(nv1))');
Q0=R*Q(:,feq)+repmat(T,1,n);
dist=Q0-P(:,fep(nv));
b=0;
c=zeros(1,n);
for i=1:n
% if norm(dist(:,i))<0.0012
if norm(dist(:,i))<0.002
b=b+1;
c(i)=1;
end
end
if b>b0
b0=b;
c0=c;
end
aa=aa-1;
end
feq(c0<1)=[];
nv(c0<1)=[];
p0=P(:,fep);
q0=Q(:,feq0);
figure;
plot3(p0(1,:),p0(2,:),p0(3,:),'r.');
hold on
plot3(q0(1,:),q0(2,:),q0(3,:),'b.');
for i=1:length(nv)
hold on
x=[Q(1,feq(i)),P(1,fep(nv(1,i)))];
y=[Q(2,feq(i)),P(2,fep(nv(1,i)))];
z=[Q(3,feq(i)),P(3,fep(nv(1,i)))];
plot3(x,y,z,'g-');
end
title('经过RANSAC算法处理后的匹配点对')
view(2)
%四元数法配准
[R,T]=Quater_Registration(Q(:,feq)',P(:,fep(nv))');
Q1=R*Q+repmat(T,1,data2{1});
toc
figure;
plot3(P(1,:),P(2,:),P(3,:),'r.');
hold on
plot3(Q1(1,:),Q1(2,:),Q1(3,:),'b.');
% hold on
% plot3(Q(1,:),Q(2,:),Q(3,:),'g.');
title('拼接结果')
view(2)
% save fuyuan_registion.mat P Q1
- 1
- 2
前往页