使用matlab实现ICP点云匹配

使用matlab实现点云匹配（ICP算法）。参数设置在代码的最前面，可以选择kd-tree或者暴力计算最近邻点。

% 程序说明：输入data_source和data_target两个点云，找寻将data_source映射到data_targe的旋转和平移参数
clear;
close all;
clc;
%% 参数配置
kd = 1;
inlier_ratio = 0.9;
Tolerance = 0.001;
step_Tolerance = 0.0001;
max_iteration = 200;
show = 1;

%% 生成数据
data_source=load（‘satellite.txt‘）;
data_source=data_source‘;

theta_x = 50;  %x旋转角度
theta_y = 30;  %y旋转角度
theta_z = 20;  %z旋转角度

t=[0-100200];   %平移向量

[data_targetT0]=rotate（data_sourcetheta_xtheta_ytheta_zt）;

% 只取其中一部分点，打乱点的顺序，添加噪声，添加离群点
data_source = data_source（:1:300）;
data_source = data_source（:randperm（size（data_source2）））;
data_source = data_source + （rand（size（data_source））-0.5）*10;
data_source（:end+1） = [1000;2000;500];

%% 绘制原始点与旋转后的点图像
figure;
scatter3（data_source（1:）data_source（2:）data_source（3:）‘b.‘）;
hold on;
scatter3（data_target（1:）data_target（2:）data_target（3:）‘r.‘）;
hold off;
daspect（[1 1 1]）;

%% 开始ICP
T_final=eye（44）;   %旋转矩阵初始值
iteration=0;
Rf=T_final（1:31:3）;
Tf=T_final（1:34）;
data_source=Rf*data_source+Tf*ones（1size（data_source2））;    %初次更新点集（代表粗配准结果）
err=1;
data_source_old = data_source;
%% 迭代优化
while（1）
    iteration=iteration+1;
    if kd == 1
        %利用Kd-tree找出对应点集
        kd_tree = KDTreeSearcher（data_target‘‘BucketSize‘10）;
        [index dist] = knnsearch（kd_tree data_source‘）;
    else
        %利用欧式距离找出对应点集
        k=size（data_source2）;
        for i = 1:k
            data_q1（1:） = data_target（1:） - data_source（1i）;    % 两个点集中的点x坐标之差
            data_q1（2:） = data_target（2:） - data_source（2i）;    % 两个点集中的点y坐标之差
            data_q1（3:） = data_target（3:） - data_source（3i）;    % 两个点集中的点z坐标之差
            distance = sqrt（data_q1（1:）.^2 + data_q1（2:）.^2 + data_q1（3:）.^2）;  % 欧氏距离
            [dist（i） index（i）] = min（distance）;   % 找到距离最小的那个点
        end
    end
    
    disp（[‘误差err=‘num2str（mean（dist））]）;
    disp（[‘迭代次数ieration=‘num2str（iteration）]）;
    err_rec（iteration） = mean（dist）;
    
    % 按距离排序，只取前面占比为inlierratio内的点以应对外点
    [~ idx] = sort（dist）;
    inlier_num = round（size（data_source2）*inlier_ratio）;
    idx = idx（1:inlier_num）;
    data_source_temp = data_source（:idx）;
    dist = dist（idx）;
    index = index（idx）;
    data_mid = data_target（:index）;
    
    % 去中心化后SVD分解求解旋转矩阵与平移向量
    [R_new t_new] = rigidTransform3D（data_source_temp‘ data_mid‘）;
    
    % 计算累计的旋转矩阵与平移向量
    Rf = R_new * Rf;
    Tf = R_new * Tf + t_new;
    
    %     更新点集
    %     data_source=R_new*data_source+t_new*ones（1size（data_source2））;
    data_source=Rf*data_source_old+Tf*ones（1size（data_source_old2））;
    
    % 显示中间结果
    if show == 1
        h = figure（2）;
        scatter3（data_source（1:）data_source（2:）data_source（3:）‘b.‘）;
        hold on;
        scatter3（data_target（1:）data_target（2:）data_target（3:）‘r.‘）;
        hold off;
        daspect（[1 1 1]）;
        pause（0.1）;
        drawnow
    end
    
    if err < Tolerance
        disp（‘———————

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       36652  2020-07-21 10:37  satellite.txt
     文件        6073  2020-07-22 17:18  my_ICP_final.m