基于特征匹配和RANSAC的三维点云拼接配准方法，matlab实现

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