matlab练习程序（点集配准的SVD法）

超、凢脫俗 2021-12-16 10:13 259阅读 0赞

[上一篇博客][Link 1]中我们使用了四元数法计算ICP。

本篇我们使用SVD计算ICP。

下面是《视觉slam十四讲》中的计算方法：

计算步骤如下：

![340413-20190116144024798-380099834.png][]

我们看到，只要求出了两组点之间的旋转，平移是非常容易得到的，所以我们重点关注R的计算。展开关于R的误差项，得：

![340413-20190116144309364-992382289.png][]

注意到第一项和R无关，第二项由于R'R=I，亦与R无关。因此，实际上优化目标函数变为：

![340413-20190116144430743-1593244788.png][]

接下来，我们介绍怎样通过SVD解出上述问题中最优的R，但关于最优性的证明较为复杂，感兴趣的读者请参考【50,51】，为了解R，先定义矩阵：

![340413-20190116144706788-1169616413.png][]

W是一个3\*3的矩阵，对W进行SVD分解，得：

![340413-20190116144751877-1511243030.png][]

其中，![340413-20190116144824864-1832905843.png][]为奇异值组成的对角矩阵，对角线元素从大到小排列，而U和V为正交矩阵，当W满秩时，R为：

![340413-20190116145023403-421411845.png][]

解得R后，按式7.53求解t即可。

具体证明可以参考：

![340413-20190116145200440-939162277.png][]

代码如下：

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));
    
    [u s v] = svd(sigma);
    RR=u*v';
    
    %计算平移向量
    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');

处理效果和四元数法一致：

原始点集：

![340413-20181214202323377-1790422293.png][]

其中蓝点为原始点集，红点为旋转平移后的点集。

配准后点集：

![340413-20181214202314486-31239577.png][]

计算得到的旋转平移矩阵，通过对蓝点集进行转换得到绿点集，比较红点集与绿点集是否基本一致。

转载于:https://www.cnblogs.com/tiandsp/p/10276994.html

[Link 1]: https://www.cnblogs.com/tiandsp/p/10121587.html
[340413-20190116144024798-380099834.png]: /images/20211214/e83f29c2d7cf42f09b9b1a495904c576.png
[340413-20190116144309364-992382289.png]: /images/20211214/a6834f8a81eb4818bff0c986e1f04285.png
[340413-20190116144430743-1593244788.png]: /images/20211214/84f09efa7202494caadd09abb723012b.png
[340413-20190116144706788-1169616413.png]: /images/20211214/8bc1cf56435d4848a7ef63dbe227f8c1.png
[340413-20190116144751877-1511243030.png]: /images/20211214/a1566a7a13344a3db88b0606eaa6ddf5.png
[340413-20190116144824864-1832905843.png]: /images/20211214/a53a96382cf64ac6a74bd489f8c80a62.png
[340413-20190116145023403-421411845.png]: /images/20211214/c1c2b57d91d549fa9c68b350e2aae659.png
[340413-20190116145200440-939162277.png]: /images/20211214/7bdd9a9ee0064e2eb3a60fd1eb202600.png
[340413-20181214202323377-1790422293.png]: /images/20211214/dbd7cb909bc94710a914911e3068da1e.png
[340413-20181214202314486-31239577.png]: /images/20211214/44ae3c0e24464c30bcb82bdb911db888.png