点云配准-ICP迭代最近点（MATLAB代码）

ZDhlx123456

已于 2024-07-15 21:12:03 修改

阅读量280

点赞数 1

分类专栏：点云配准文章标签： matlab

于 2024-05-08 08:05:28 首次发布

本文链接：https://blog.csdn.net/ZDhlx123456/article/details/138556019

版权

点云配准专栏收录该内容

2 篇文章 0 订阅

订阅专栏

使用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