ICP算法实现(MATLAB)
来源:互联网 发布:淘宝上怎么买到真蜂蜜 编辑:程序博客网 时间:2024/06/06 16:26
ICP原理
ICP(Iterative closet point method)迭代最近点法,用于两组数据之间的配准,其实现的具体步骤如下
对于两组点云:
P 、Q step1:选择控制点
pi→∈P 、设置T 的初始值T0=T0 step2:重复执行以下步骤,直至满足收敛条件
step2-1:对各控制点,
pi→ 在Q 中求其最近点qj→ ,并将其作为pi→ 的假想对应点 step2-2:对于确定的对应的关系,求解
Tk ,对并且求解loss function
Ek=∑Tk|pi→−qj→|2> step2-3:重新计算控制点
pi→ 在经过Tk 变换之后的点,并将其重新赋值给pi→ 算法的收敛条件是
δ=Ek−Ek−1M<ε
数据采集
把采集的数据集1:
数据2:
经过初始配准之后的数据集:
Registration
在初始时,通过手动选取3000个点作为配准点;平移通过质心之间的距离计算,旋转通过
则变换
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%%%对于确定的关系,求解RTcentroid1=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/controldatanumdelta=abs(E-last_E)/controldatanum%中间迭代的误差delta_Intermediate(iter,1)=delta;if(delta<0.001) break;endlast_E=E;end
由于控制点手工选取的较好,所以算法收敛的很快,基本上经过三次迭代即收敛,算法的loss函数定义为控制点与其match之间的平均差距,即为
配准的结果如下图:
配准结果好坏的衡量标准为观察点云是否融合在一起,仔细观察上述结果是彼此融合在一起。
不足之处
用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%%%对于确定的关系,求解RTcentroid1=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/controldatanumdelta=abs(E-last_E)/controldatanum%中间迭代的误差delta_Intermediate(iter,1)=delta;if(delta<0.001) break;endlast_E=E;endfigure(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与Ttemp_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);endR_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;
0 0
- ICP算法实现(MATLAB)
- matlab实现ICP(3D迭代最近点算法)
- ICP算法实现
- PCL 之 ICP 算法实现
- ICP算法(Iterative Closest Point)及VTK实现(改)
- ICP算法
- ICP算法
- ICP算法
- ICP算法
- ICP算法
- PCL中的点云配准(Registration)ICP算法
- ICP定位算法学习笔记(一)
- 基于PCL的三维重建——点云配准(一)ICP算法实现
- ICP算法(Iterative Closest Point)及VTK实现
- ICP算法(Iterative Closest Point)及VTK实现
- ICP算法(Iterative Closest Point)及VTK实现
- 迭代最近点算法流程详解(ICP算法)
- ICP算法解析
- jQuery跳转链接
- Android retrofit历程(一)
- makefile-规则的命令
- Rails中的ajax运用1---评论
- 关于使用PulltoRefreshListView来展示新闻的内容以及多级评论
- ICP算法实现(MATLAB)
- [HNOI2002][poj1091]跳蚤
- 百度地图的基本应用
- python里面的encode和decode
- websocket
- Linux进程间通信——使用信号量
- 课程实践:电子词典
- Hibernate 开发实例全步骤
- Electron初探