MATLAB 對應點集配準的四元數法
阿新 • • 發佈:2020-09-10
這個算是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程式碼如下:
1 clear all;
2 close all;
3 clc;
4
5 %生成原始點集
6 X=[];Y=[];Z=[];
7 for i=-180:2:180
8 for j=-90:2:90
9 x = i * pi / 180.0;
10 y = j * pi / 180.0;
11 X =[X,cos(y) * cos(x)];
12 Y =[Y,sin(y) * cos(x)];
13 Z =[Z,sin(x)];
14 end
15 end
16 P=[X(1 :3000)' Y(1:3000)' Z(1:3000)'];
17
18 %生成變換後點集
19 i=0.5;j=0.3;k=0.7;
20 Rx=[1 0 0;0 cos(i) -sin(i); 0 sin(i) cos(i)];
21 Ry=[cos(j) 0 sin(j);0 1 0;-sin(j) 0 cos(j)];
22 Rz=[cos(k) -sin(k) 0;sin(k) cos(k) 0;0 0 1];
23 R=Rx*Ry*Rz;
24 X=P*R + [0.2,0.3,0.4];
25
26 plot3(P(:,1),P(:,2),P(:,3),'b.');
27 hold on;
28 plot3(X(:,1),X(:,2 ),X(:,3),'r.');
29
30 %計算點集均值
31 up = mean(P);
32 ux = mean(X);
33
34 P1=P-up;
35 X1=X-ux;
36
37 %計算點集協方差
38 sigma=P1'*X1/(length(X1));
39 sigma_mi = sigma - sigma';
40 M=sigma+sigma'-trace(sigma)*[1,0,0;0,1,0;0,0,1];
41
42 %由協方差構造4*4對稱矩陣
43 Q=[trace(sigma) sigma_mi(2,3) sigma_mi(3,1) sigma_mi(1,2);
44 sigma_mi(2,3) M(1,1) M(1,2) M(1,3);
45 sigma_mi(3,1) M(2,1) M(2,2) M(2,3);
46 sigma_mi(1,2) M(3,1) M(3,2) M(3,3)];
47
48 %計算特徵值與特徵向量
49 [x,y] = eig(Q);
50 e = diag(y);
51
52 %計算最大特徵值對應的特徵向量
53 lamda=max(e);
54 for i=1:length(Q)
55 if lamda==e(i)
56 break;
57 end
58 end
59 q=x(:,i);
60
61 q0=q(1);q1=q(2);q2=q(3);q3=q(4);
62
63 %由四元數構造旋轉矩陣
64 RR=[q0^2+q1^2-q2^2-q3^2 ,2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2);
65 2*(q1*q2+q0*q3), q0^2-q1^2+q2^2-q3^2, 2*(q2*q3-q0*q1);
66 2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), q0^2-q1^2-q2^2+q3^2];
67
68 %計算平移向量
69 qr=ux-up*RR';
70
71 %驗證旋轉矩陣與平移向量正確性
72 Pre = P*RR'+qr;
73
74 figure;
75 plot3(P(:,1),P(:,2),P(:,3),'b.');
76 hold on;
77 plot3(X(:,1),X(:,2),X(:,3),'r.');
78
79 plot3(Pre(:,1),Pre(:,2),Pre(:,3),'go');