使用matlab实现点云匹配(ICP算法)

代码主体和数据文件satellite.txt来自JusticeZQ的博客
加入了自己的修改,参数设置在代码的最前面,可以选择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,-100,200];   %平移向量

[data_target,T0]=rotate(data_source,theta_x,theta_y,theta_z,t);

% 只取其中一部分点,打乱点的顺序,添加噪声,添加离群点
data_source = data_source(:,1:300);
data_source = data_source(:,randperm(size(data_source,2)));
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(4,4);   %旋转矩阵初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_source=Rf*data_source+Tf*ones(1,size(data_source,2));    %初次更新点集(代表粗配准结果)
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_source,2);
        for i = 1:k
            data_q1(1,:) = data_target(1,:) - data_source(1,i);    % 两个点集中的点x坐标之差
            data_q1(2,:) = data_target(2,:) - data_source(2,i);    % 两个点集中的点y坐标之差
            data_q1(3,:) = data_target(3,:) - data_source(3,i);    % 两个点集中的点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_source,2)*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(1,size(data_source,2));
    data_source=Rf*data_source_old+Tf*ones(1,size(data_source_old,2));
    
    % 显示中间结果
    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('————————————————————————————');
        disp('情况1:优化结果已经达到目标,结束优化');
        break
    end
    if iteration > 1 && err_rec(iteration-1) - err_rec(iteration) < step_Tolerance
        disp('————————————————————————————');
        disp('情况2:迭代每一步带来的优化到极限值,结束优化');
        break
    end
    if iteration>=max_iteration
        disp('————————————————————————————');
        disp('情况3:迭代已经达到最大次数,结束优化');
        break
    end
end

%% 计算最后结果的误差
if kd == 1
    %利用Kd-tree找出对应点集
    kd_tree = KDTreeSearcher(data_target','BucketSize',10);
    [index, dist] = knnsearch(kd_tree, data_source');
else
    %利用欧式距离找出对应点集
    k=size(data_source,2);
    for i = 1:k
        data_q1(1,:) = data_target(1,:) - data_source(1,i);    % 两个点集中的点x坐标之差
        data_q1(2,:) = data_target(2,:) - data_source(2,i);    % 两个点集中的点y坐标之差
        data_q1(3,:) = data_target(3,:) - data_source(3,i);    % 两个点集中的点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))]);
err_rec(iteration+1) = mean(dist);

%% 迭代优化过程中误差变化曲线
figure;
plot(0:iteration,err_rec);
grid on

%% 最后点云匹配的结果
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]);

disp('旋转矩阵的真值:');
disp(T0);  %旋转矩阵真值
disp('计算出的旋转矩阵:');
T_final = [Rf,Tf];
T_final=[T_final;0,0,0,1];
disp(T_final);


%% 计算两个点集p,q的刚性变换参数,p和q的大小要一致
function [R, t] = rigidTransform3D(p, q)
n = cast(size(p, 1), 'like', p);
m = cast(size(q, 1), 'like', q);
% 去中心化
pmean = sum(p,1)/n;
p2 = bsxfun(@minus, p, pmean);
qmean = sum(q,1)/m;
q2 = bsxfun(@minus, q, qmean);
% 对协方差矩阵进行SVD分解
C = p2'*q2;
[U,~,V] = svd(C);
R = V*diag([1 1 sign(det(U*V'))])*U';
t = qmean' - R*pmean';
end

%% 对点云进行旋转与平移
function [data_q,T] = rotate(data,theta_x, theta_y, theta_z, t)
theta_x = theta_x/180*pi;
rot_x = [1 0 0;0 cos(theta_x) sin(theta_x);0 -sin(theta_x) cos(theta_x)];
theta_y = theta_y/180*pi;
rot_y = [cos(theta_y) 0 -sin(theta_y);0 1 0;sin(theta_y) 0 cos(theta_y)];
theta_z = theta_z/180*pi;
rot_z = [cos(theta_z) sin(theta_z) 0;-sin(theta_z) cos(theta_z) 0;0 0 1];
% 变换矩阵
T = rot_x*rot_y*rot_z;
T = [T,t'];
T=[T;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,:);
end
  • 13
    点赞
  • 181
    收藏
    觉得还不错? 一键收藏
  • 32
    评论

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值