使用MATLAB实现点云迭代最近点(Iterative Closest Point, ICP)
提示:使用前需先下载置顶博客《自定义MATLAB点云函数集》,并将文件夹添加到MATLAB搜索路径中。
实验数据:
链接:https://pan.baidu.com/s/17JSVD40pSt6eqMrqkUx8qA?pwd=4zvo
提取码:4zvo
--来自百度网盘超级会员V5的分享
clear;
close all;
clc;
pc1=pcread("teapot1.pcd");
pc1=pcRemoveDuplicate(pc1);
pt1=pc1.Location;
count1=pc1.Count;
pc2=pcread("teapot2.pcd");
pc2=pcRemoveDuplicate(pc2);
pt2=pc2.Location;
count2=pc2.Count;
%%
numIter=0;
rmse=1;
while(rmse>0.001)
numIter=numIter+1;
[idx,dis]=knnsearch(pt2,pt1,K=1);
KNNpt=pt2(idx,:);
[rot,trans]=RigidTformSVD(pt1,KNNpt);
pt1=pt1*rot+ones(count1,1)*trans;
rmse=sqrt(mean(dis.^2));
fprintf('均方根误差为%f,第%d次迭代\n',rmse,numIter);
if numIter>=100
break
end
end
%%
figure
pcshowpair(pc1,pc2,BackgroundColor=[1 1 1])
figure
pcshowpair(pointCloud(pt1),pointCloud(pt2),BackgroundColor=[1 1 1])
% figure;
% x1=pt1(1,:);
% y1=pt1(2,:);
% z1=pt1(3,:);
% x2=pt2(1,:);
% y2=pt2(2,:);
% z2=pt2(3,:);
% scatter3(x1,y1,z1,'b');
% hold on;
% scatter3(x2,y2,z2,'r');
% hold off;
function [rot,trans]=RigidTformSVD(P,X)
up=mean(P);
ux=mean(X);
P1=P-up;%去中心
X1=X-ux;%去中心
sigma=P1'*X1/(length(X1));%3×3
[u,s,v]=svd(sigma);
%% 变换矩阵
rot=u*v';%旋转矩阵
trans=ux-up*rot;%平移向量
end