本篇我们使用SVD计算点集配准。
下面是《视觉slam十四讲》中的计算方法：
计算步骤如下：
我们看到，只要求出了两组点之间的旋转，平移是非常容易得到的，所以我们重点关注R的计算。展开关于R的误差项，得：
注意到第一项和R无关，第二项由于R'R=I，亦与R无关。因此，实际上优化目标函数变为：
接下来，我们介绍怎样通过SVD解出上述问题中最优的R，但关于最优性的证明较为复杂，感兴趣的读者请参考【50,51】，为了解R，先定义矩阵：
W是一个3*3的矩阵，对W进行SVD分解，得：
其中，
为奇异值组成的对角矩阵，对角线元素从大到小排列，而U和V为正交矩阵，当W满秩时，R为：
解得R后，按式7.53求解t即可。
具体证明可以参考：
代码如下：
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
40 [u s v] = svd(sigma);
41 RR=u*v';
42
43 %计算平移向量
44 qr=ux-up*RR;
45
46 %验证旋转矩阵与平移向量正确性
47 Pre = P*RR+qr;
48
49 figure;
50 plot3(P(:,1),P(:,2),P(:,3),'b.');
51 hold on;
52 plot3(X(:,1),X(:,2),X(:,3),'r.');
53 plot3(Pre(:,1),Pre(:,2),Pre(:,3),'go');
处理效果和四元数法一致：
原始点集：
其中蓝点为原始点集，红点为旋转平移后的点集。
配准后点集：
计算得到的旋转平移矩阵，通过对蓝点集进行转换得到绿点集，比较红点集与绿点集是否基本一致。
