使用matlab实现点云匹配（ICP算法）

最新推荐文章于 2025-04-25 11:32:52 发布

原创

最新推荐文章于 2025-04-25 11:32:52 发布 · 1.2w 阅读

194

标签

#matlab #算法 #计算机视觉

收录于

本文详细介绍并实现了ICP(迭代最近点)算法，一种用于点云配准的经典方法。通过MATLAB代码演示了如何从原始点云数据中找到最佳的旋转和平移参数，以使两个点云达到最优匹配。实验中使用了卫星点云数据，展示了算法的运行过程及结果。

代码主体和数据文件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