ICP算法实现(MATLAB)

原创 2017年01月03日 17:00:37

ICP原理

ICP(Iterative closet point method)迭代最近点法,用于两组数据之间的配准,其实现的具体步骤如下

对于两组点云:PQ

step1:选择控制点piP、设置T的初始值T0=T0

step2:重复执行以下步骤,直至满足收敛条件

​ step2-1:对各控制点,piQ中求其最近点qj,并将其作为pi的假想对应点

​ step2-2:对于确定的对应的关系,求解Tk,对并且求解loss function

Ek=Tk|piqj|2>

​ step2-3:重新计算控制点pi在经过Tk变换之后的点,并将其重新赋值给pi

算法的收敛条件是δ=EkEk1M<ε

数据采集

把采集的数据集1:
这里写图片描述

数据2:
这里写图片描述

经过初始配准之后的数据集:

这里写图片描述

Registration

在初始时,通过手动选取3000个点作为配准点;平移通过质心之间的距离计算,旋转通过svd分解进行计算。具体实现方法为,假设两组去质心的数据点为qiqi,通过计算

Hi=1nqiqti

则变换qi=Rqi,对H进行SVD分解,即H=USVt,则旋转矩阵可表示为R=VUt,该方法已经集成在PCL库中用于registration,具体证明方法可以参照”Least-Squares Fitting of Two 3-D Points Sets”这篇文章。算法迭代的流程如下:

for iter=1:iteration
%寻找控制点的对应点
for i=1:controldatanum
    temp_data1=repmat(controldata1(i,:),m,1);
    diff=sqrt(sum((temp_data1-data2).^2,2));
    [minvalue,index(i,1)]=min(diff);
    controldata2(i,:)=data2(index(i,1),:);
end
%%
%对于确定的关系,求解RT
centroid1=mean(controldata1);
centroid2=mean(controldata2);
demeancontroldata1=controldata1-repmat(centroid1,controldatanum,1);
demeancontroldata2=controldata2-repmat(centroid2,controldatanum,1);
H=demeancontroldata1'*demeancontroldata2;
[U,S,V]=svd(H);
R=V*U';
T=(centroid2-centroid1)';
R_Intermediate(:,:,iter)=R;
T_Intermediate(:,:,iter)=T;
%%
%利用求解得到的RT计算变换之后的点
controldata1=R*controldata1'+repmat(T,1,controldatanum);
controldata1=controldata1';%新的控制点
E=norm(controldata1-controldata2,2);
e_Intermediate(iter,1)=E/controldatanum
delta=abs(E-last_E)/controldatanum%中间迭代的误差
delta_Intermediate(iter,1)=delta;
if(delta<0.001)
    break;
end
last_E=E;
end

由于控制点手工选取的较好,所以算法收敛的很快,基本上经过三次迭代即收敛,算法的loss函数定义为控制点与其match之间的平均差距,即为Ek/M,得到如下图像

这里写图片描述
配准的结果如下图:

这里写图片描述

配准结果好坏的衡量标准为观察点云是否融合在一起,仔细观察上述结果是彼此融合在一起。

不足之处

用matlab实现起来速度比较慢,尤其是在寻找控制点的match时,需要对另外一组数据进行遍历匹配,一种比较快速的方法是用C++并且通过K-d tree进行匹配点搜索,这样耗时应该比较少。

全部代码

%%
%寻找的变换关系data2=Rdata1+T

%%
%加载数据选取控制点
data1=load('3.asc');
data2=load('4.asc');
figure(1);
plot3(data1(:,1),data1(:,2),data1(:,3),'r.');
hold on;
plot3(data2(:,1),data2(:,2),data2(:,3),'b.');
title('原始数据');
axis tight equal;
hold off;


[m,n]=size(data2);
controldata1=load('controldata.asc');%选取控制点
[controldatanum,~]=size(controldata1);
controldata2=zeros(controldatanum,3);
%%
%初始化
R=[1,0,0;0,1,0;0,0,1];
T=[0,0,0];
last_E=0;
iteration=20;
R_Intermediate=zeros(3,3,iteration);
T_Intermediate=zeros(3,1,iteration);
delta_Intermediate=zeros(iteration,1);
index=zeros(controldatanum,1);
e_Intermediate=zeros(iteration,1);
%%
%迭代
for iter=1:iteration
%寻找控制点的对应点
for i=1:controldatanum
    temp_data1=repmat(controldata1(i,:),m,1);
    diff=sqrt(sum((temp_data1-data2).^2,2));
    [minvalue,index(i,1)]=min(diff);
    controldata2(i,:)=data2(index(i,1),:);
end
%%
%对于确定的关系,求解RT
centroid1=mean(controldata1);
centroid2=mean(controldata2);
demeancontroldata1=controldata1-repmat(centroid1,controldatanum,1);
demeancontroldata2=controldata2-repmat(centroid2,controldatanum,1);
H=demeancontroldata1'*demeancontroldata2;
[U,S,V]=svd(H);
R=V*U';
T=(centroid2-centroid1)';
R_Intermediate(:,:,iter)=R;
T_Intermediate(:,:,iter)=T;
%%
%利用求解得到的RT计算变换之后的点
controldata1=R*controldata1'+repmat(T,1,controldatanum);
controldata1=controldata1';%新的控制点
E=norm(controldata1-controldata2,2);
e_Intermediate(iter,1)=E/controldatanum
delta=abs(E-last_E)/controldatanum%中间迭代的误差
delta_Intermediate(iter,1)=delta;
if(delta<0.001)
    break;
end
last_E=E;
end

figure(2);
plot(1:iter,delta_Intermediate(1:iter,1)');
xlabel('迭代次数');ylabel('delta');
figure(3);
plot(1:iter,e_Intermediate(1:iter)');
xlabel('迭代次数');ylabel('loss');
%%
%计算最终的R与T
temp_R=eye(3);
temp_T=zeros(3,1);
for i=1:iter
   temp_R=R_Intermediate(:,:,i)*temp_R; 
   temp_T=R_Intermediate(:,:,i)*temp_T+T_Intermediate(:,:,i);
end
R_final=temp_R;
T_final=temp_T;

data1_transformed=R_final*data1'+repmat(T_final,1,size(data1,1));
data1_transformed=data1_transformed';
figure(4);
plot3(data1_transformed(:,1),data1_transformed(:,2),data1_transformed(:,3),'r.')
hold on;
plot3(data2(:,1),data2(:,2),data2(:,3),'b.')
title('ICP results')
axis equal tight;
hold off;
 save data3.asc -ascii data1_transformed;

相关文章推荐

点云匹配和ICP算法概述

【原文:http://www.cnblogs.com/yhlx125/p/4955337.html】 Iterative Closest Point (ICP) [1][2][3] is ...

Delphi7高级应用开发随书源码

  • 2003年04月30日 00:00
  • 676KB
  • 下载

ICP算法进行点云匹配

【原文:http://www.cnblogs.com/yhlx125/p/5234156.html】 上一篇:http://www.cnblogs.com/yhlx125/p/4924283...

Delphi7高级应用开发随书源码

  • 2003年04月30日 00:00
  • 676KB
  • 下载

Delphi7高级应用开发随书源码

  • 2003年04月30日 00:00
  • 676KB
  • 下载

MATLAB点云处理:读取、展示、最近邻、ICP算法求取转移矩阵、旋转

MATLAB中关于点云的几个函数的简单应用。作者使用的是MATLAB R2015b,这几个函数应该是在Computer Vison包里。 全文都是作者自己结合MATLAB文档的理解,欢迎指教。 1...

Delphi7高级应用开发随书源码

  • 2003年04月30日 00:00
  • 676KB
  • 下载

Delphi7高级应用开发随书源码

  • 2003年04月30日 00:00
  • 676KB
  • 下载

PCL点云库:ICP算法

ICP(Iterative Closest Point迭代最近点)算法是一种点集对点集配准方法。在VTK、PCL、MRPT、MeshLab等C++库或软件中都有实现,可以参见维基百科中的ICP Alg...

Delphi7高级应用开发随书源码

  • 2003年04月30日 00:00
  • 676KB
  • 下载
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:ICP算法实现(MATLAB)
举报原因:
原因补充:

(最多只允许输入30个字)