ICP配准MATLAB实现

ICP配准MATLAB实现

本文介绍了ICP算法及其MATLAB实现



一、ICP算法

经典的ICP(iterative closet point)算法是由McKay和Besl1提出的,它的核心思想是迭代调姿使距离偏差最小化。原始ICP算法的基本描述是:对于点云P的每个点,在另一个点云Q中求取距离它最近的点(理想状态下本应重合的点),计算对应点的欧式距离平方的平均值,然后通过迭代算法,最小化平均值,这样不断更新点云片间的相对位置,达到点云片之间配准对齐的效果,如图6所示。


1.算法简介

扫描点云与CAD模型最佳拟合对齐是利用奇异值分解法、四元组法找出扫描点云与模型表面对应点的变换矩阵,多次迭代直至目标函数满足一定的精度为止。其目标函数为:

1
式中:pi(i=0,1,…,n) 为待配准的扫描点; qi(i=0,1,…,n)为pi在CAD模型表面上的匹配点; R,T分别为待求的旋转矩阵与平移向量。
具体计算步骤如下:
Step1. 计算两个待配准点集的质心:

Step2. 计算两个待配准点集相对于各自质心的位移:

Step3. 计算协方差矩阵:

Step4. 构造对称矩阵:

Step5. 计算旋转矩阵

Step6. 计算平移变换矢量

2. 算法局限性

ICP有以下局限性:
(1)初始位置的依赖性
初始位置对它的影响很大,ICP算法的收敛速度及收敛的最终位置直接受制于求取的匹配点(最近点)是否精确,而只有在扫面点云与CAD模型有较好的初始位置关系时,匹配点才能精准地获取到。
(2)局部最优性
ICP算法得到的是局部最优解,从本质上来讲,该算法每次迭代总是选择距离差的最小值,并不能得到全局最优解。
(3)不区分正负性
ICP算法是以最小二乘的方式最小化距离差,其收敛结果趋向于使得各配对点的差值均匀化,并且该距离差不具有正负性和方向性。
(4)计算匹配点复杂度较高
ICP算法的时间复杂度非常高:计算扫描点的匹配点(求取最近点)时,对于点云片P中的每个点,需要遍历CAD模型的曲面集合 中每个面,计算匹配点集 时间复杂度为 ,并且点到曲面最近点的计算也是较为耗时的。为了提高最佳拟合对齐的效率,点云数据需要精简,计算最近点的方式需要改进。

二、代码实现

1.平移旋转计算

代码如下:

function [data_q,T] = rotate(data,x,y,z,t)

%欧拉角转旋转矩阵

x = x/180*pi;
y = y/180*pi;
z = z/180*pi;

Rx = [1      0      0;
    0 cos(x) -sin(x);
    0 sin(x) cos(x)];
Ry = [cos(y)  0 sin(y);
    0       1      0;
    -sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;
    sin(z) cos(z)  0;
    0      0       1];
T = Rz*Ry*Rx;        %旋转矩阵

T = [T(1,1),T(1,2),T(1,3),t(1);
    T(2,1),T(2,2),T(2,3),t(2);
    T(3,1),T(3,2),
  • 5
    点赞
  • 52
    收藏
    觉得还不错? 一键收藏
  • 4
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值