最近点迭代ICP（Iterative Closest Point）算法多平台实现

最新推荐文章于 2024-08-12 09:50:11 发布

老牛拉破车

最新推荐文章于 2024-08-12 09:50:11 发布

阅读量2.3k

点赞数 2

分类专栏：机器视觉文章标签：视觉三维 ICP MATLAB PCL

本文链接：https://blog.csdn.net/qq_34839823/article/details/94399375

版权

本文介绍了ICP（Iterative Closest Point）算法的基本原理及其在MATLAB、PCL和HALCON平台上的实现。在MATLAB中，通过迭代寻找最佳匹配并计算旋转和平移矩阵，但速度较慢。PCL库提供了基于SVD的ICP实现，可以通过设置参数控制迭代过程。同时，文章提到了ICP的变种，如point-to-plain，以适应不同应用场景。

摘要由CSDN通过智能技术生成

1、ICP算法

**
最近点迭代算法，是最为经典的数据配准算法。其特征在于，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阀值，则迭代进行上述运算直到满足给定的误差要求.

ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。

*2、MATLAB实现

2.1算法原理
对于两组点云：PP、QQ
step1：选择控制点pi→∈Ppi→∈P、设置TT的初始值T0=T0T0=T0
step2：重复执行以下步骤，直至满足收敛条件
step2-1：对各控制点，pi→pi→在QQ中求其最近点qj→qj→，并将其作为pi→pi→的假想对应点
 step2-2：对于确定的对应的关系，求解TkTk，对并且求解loss function
Ek=∑Tk|pi→−qj→|2>
Ek=∑Tk|pi→−qj→|2>
step2-3：重新计算控制点pi→pi→在经过TkTk变换之后的点，并将其重新赋值给pi→pi→
算法的收敛条件是δ=Ek−Ek−1M<ε

2.2 Registration
在初始时，通过手动选取3000个点作为配准点；平移通过质心之间的距离计算，旋转通过svdsvd分解进行计算。具体实现方法为，假设两组去质心的数据点为qiqi与q′iqi′，通过计算
H≜∑i=1nqiq′ti
H≜∑i=1nqiqi′t

则变换q′i=Rqiqi′=Rqi，对HH进行SVDSVD分解，即H=USVtH=USVt，则旋转矩阵可表示为R=VUtR=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

由于控制点手工选取的较好，所以算法收敛的很快