这个算是ICP算法中的一个关键步骤，单独拿出来看一下。
算法流程如下：
1.首先得到同名点集P和X。
2.计算P和X的均值up和ux。
3.由P和X构造协方差矩阵sigma。
4.由协方差矩阵sigma构造4*4对称矩阵Q。
5.计算Q的特征值与特征向量。其中Q最大特征值对应的特征向量即为最佳旋转向量q。
6.通过四元数q得到旋转矩阵R。
7.根据R计算最佳平移向量qr。
具体公式我就不贴图了，可以参考这篇“ICP算法在点云配准中的应用”论文的3.1节。
处理效果如下：
原始点集：
其中蓝点为原始点集，红点为旋转平移后的点集。
配准后点集：
计算得到的旋转平移矩阵，通过对蓝点集进行转换得到绿点集，比较红点集与绿点集是否基本一致。
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学习最常见的问题，每天进步一点点，业精于勤荒于嬉。
打开微信扫一扫哦！