基于PCA的点云位姿估计与粗拼接附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

⛄ 内容介绍

三维点云配准是三维重建过程中的重要环节,ICP配准算法无法处理初始位姿相差较大的点云。

⛄ 部分代码

clc
clear
close all
%------读取点云数据------%
% mydir=uigetdir('d:','选择一个目录');
mydir='.\bunny';
DIRS=dir(fullfile(mydir,'*.ply'));
n=length(DIRS);
name={DIRS.name}';
p=1;q=2;
[~,P] = ply_read(fullfile(mydir,DIRS(p).name),'tri');
[~,Q] = ply_read(fullfile(mydir,DIRS(q).name),'tri');
figure;
plot3(P(1,:),P(2,:),P(3,:),'r.');
hold on
plot3(Q(1,:),Q(2,:),Q(3,:),'b.')
hold off
title(name([p,q]));
view(2)
point1=size(P,2);
point2=size(Q,2);
%------分别求解P Q的每一行的均值------%
pc = mean(P,2);%  xc:3*1的矩阵
qc = mean(Q,2);   %mean(A,2)求各行的均值 

%----分别求解两个数据集合的协方差矩阵----%
x1 = P - repmat(pc,1,point1); %repmat(A,m,n)将矩阵A以m行n列复制摆好拼起来
Mx =x1 * x1';
y1 = Q - repmat(qc,1,point2);
My = y1 * y1';

%------求解Mx My的主元和主方向------%
[Vx,Dx] = eig(Mx,'nobalance'); %Vx特征向量,Dx特征值
[Vy,Dy] = eig(My,'nobalance');

[~,index]=max(sum(x1.*x1));
xm=x1(:,index);
xm(3,1)=-abs(xm(3,1));
p3 = Vx(:,3);
if dot(xm,p3)<0
    p3=-p3;
end
p2 = Vx(:,2);
if dot(xm,p2)<0
    p2=-p2;
end
p1=cross(p3,p2);

[~,index]=max(sum(y1.*y1));
ym=y1(:,index);
ym(3,1)=-abs(ym(3,1));
q3 = Vy(:,3);
if dot(ym,q3)<0
    q3=-q3;
end
q2 = Vy(:,2);
if dot(ym,q2)<0
    q2=-q2;
end
q1=cross(q3,q2);
%------求解旋转矩阵R和平移向量T------%

R = [q1,q2,q3]/[p1,p2,p3];%R=(q1,q2,q3)(p1,p2,p3)-1：逆矩阵

xc2 = R*pc;
T = (qc - xc2);
P1=R*P+repmat(T,1,size(P,2));
figure;
plot3(P1(1,:),P1(2,:),P1(3,:),'r.');
hold on
plot3(Q(1,:),Q(2,:),Q(3,:),'b.')
hold off
title(name([p,q]));
view(2)

⛄ 运行结果

⛄ 参考文献

[1] 刘哲, 周天, 彭东东,等. 一种改进的基于PCA的ICP点云配准算法研究[J]. 黑龙江大学自然科学学报, 2019, 36(4):6.

[2] 刘向东. 基于PCA算法人脸识别的MATLAB实现[J]. 电脑知识与技术：学术版, 2016(4X):2.

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料