matlab点集配准,matlab练习程序（对应点集配准的四元数法）

clear all;

close all;

clc;

%生成原始点集

X=[];Y=[];Z=[];

for i=-180:2:180

for j=-90:2:90

x = i * pi / 180.0;

y = j * pi / 180.0;

X =[X,cos(y) * cos(x)];

Y =[Y,sin(y) * cos(x)];

Z =[Z,sin(x)];

end

end

P=[X(1:3000)‘ Y(1:3000)‘ Z(1:3000)‘];

%生成变换后点集

i=0.5;j=0.3;k=0.7;

Rx=[1 0 0;0 cos(i) -sin(i); 0 sin(i) cos(i)];

Ry=[cos(j) 0 sin(j);0 1 0;-sin(j) 0 cos(j)];

Rz=[cos(k) -sin(k) 0;sin(k) cos(k) 0;0 0 1];

R=Rx*Ry*Rz;

X=P*R + [0.2,0.3,0.4];

plot3(P(:,1),P(:,2),P(:,3),‘b.‘);

hold on;

plot3(X(:,1),X(:,2),X(:,3),‘r.‘);

%计算点集均值

up = mean(P);

ux = mean(X);

P1=P-up;

X1=X-ux;

%计算点集协方差

sigma=P1‘*X1/(length(X1));

sigma_mi = sigma - sigma‘;

M=sigma+sigma‘-trace(sigma)*[1,0,0;0,1,0;0,0,1];

%由协方差构造4*4对称矩阵

Q=[trace(sigma) sigma_mi(2,3) sigma_mi(3,1) sigma_mi(1,2);

sigma_mi(2,3) M(1,1) M(1,2) M(1,3);

sigma_mi(3,1) M(2,1) M(2,2) M(2,3);

sigma_mi(1,2) M(3,1) M(3,2) M(3,3)];

%计算特征值与特征向量

[x,y] = eig(Q);

e = diag(y);

%计算最大特征值对应的特征向量

lamda=max(e);

for i=1:length(Q)

if lamda==e(i)

break;

end

end

q=x(:,i);

q0=q(1);q1=q(2);q2=q(3);q3=q(4);

%由四元数构造旋转矩阵

RR=[q0^2+q1^2-q2^2-q3^2 ,2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2);

2*(q1*q2+q0*q3), q0^2-q1^2+q2^2-q3^2, 2*(q2*q3-q0*q1);

2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), q0^2-q1^2-q2^2+q3^2];

%计算平移向量

qr=ux-up*RR‘;

%验证旋转矩阵与平移向量正确性

Pre = P*RR‘+qr;

figure;

plot3(P(:,1),P(:,2),P(:,3),‘b.‘);

hold on;

plot3(X(:,1),X(:,2),X(:,3),‘r.‘);

plot3(Pre(:,1),Pre(:,2),Pre(:,3),‘go‘);

关注公众号： MATLAB基于模型的设计 (ID：xaxymaker) ，每天推送MATLAB学习最常见的问题，每天进步一点点，业精于勤荒于嬉。

打开微信扫一扫哦！