ICP配准MATLAB实现

ICP配准MATLAB实现

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

---

---

一、ICP算法

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

---

1.算法简介

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

式中：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),T(3,3),t(3);
    0 0 0 1];  % 复合

rows=size(data,2);
rows_one=ones(1,rows);
data=[data;rows_one];    %化为齐次坐标

data_q=T*data;
data_q=data_q(1:3,:);    %返回三维坐标

2.ICP算法代码

代码如下：

% ICP 算法
clear;
close all;
clc;

data_source=load('16-3.txt');
data_source=data_source';
%旋转角度
alpha = 20;
theta = 5;
grama = 5;

t=[1,1,30];   %平移向量

[data_target,T0]=rotate(data_source,alpha ,theta,grama,t);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%绘制两幅原始图像
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure(1);
title('初始位置');
scatter3(x1,y1,z1,'b*');
hold on;
scatter3(x2,y2,z2,'r*');
hold off;


T_final=eye(4,4);   %旋转矩阵初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %初次更新点集（代表粗配准结果）
err=1;
while(err>0.0001)
    iteration=iteration+1;    %迭代次数
    disp(['迭代次数ieration=',num2str(iteration)]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %利用欧式距离找出对应点集 搜索每个点
    k=size(data_target,2);
    for i = 1:k
        data_q1(1,:) = data_source(1,:) - data_target(1,i);    % 两个点集中的点x坐标之差
        data_q1(2,:) = data_source(2,:) - data_target(2,i);    % 两个点集中的点y坐标之差
        data_q1(3,:) = data_source(3,:) - data_target(3,i);    % 两个点集中的点z坐标之差
        distance = data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2;  % 欧氏距离
        [min_dis, min_index] = min(distance);   % 找到距离最小的那个点
        data_mid(:,i) = data_source(:,min_index);   % 将那个点保存为对应点
        error(i) = min_dis;     % 保存距离差值
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %去中心化
    % 质心
    data_target_mean=mean(data_target,2);
    data_mid_mean=mean(data_mid,2);
    
    data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));
    data_mid_c=data_mid-data_mid_mean*ones(1,size(data_mid,2));
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %SVD分解
    W=zeros(3,3);
    % 计算协方差矩阵
    for j=1:size(data_target_c,2)
        W=W+data_mid_c(:,j)*data_target_c(:,j)';
    end
    [U,S,V]=svd(W);
    Rf=U*V';
    % 计算质心平移矢量
    Tf=data_mid_mean-Rf*data_target_mean;
    
    err=mean(error);
    
    T_t=[Rf,Tf];
    T_t=[T_t;0,0,0,1];
    T_final=T_t*T_final;   %更新变换矩阵
    disp(['误差err=',num2str(err)]);
    disp('变换矩阵T=');
    disp(inv(T_final));
    
    data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %更新点集
    if iteration >= 200
        break
    end
end

disp('真值');
disp(T0);  %旋转矩阵真值,对矩阵求逆

x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure(2);
title('配准后');
scatter3(x1,y1,z1,'r*');
hold on;
scatter3(x2,y2,z2,'b*');
hold off;

注：代码中的导入的文件是零件的点云数据。

---

参考文献

---

1. 戴静兰, 陈志杨, 叶修梓. ICP算法在点云配准中的应用[J]. 中国图象图形学报, 2007(03):517-521. ↩︎