zoukankan      html  css  js  c++  java
  • matlab练习程序(点集配准的SVD法)

    上一篇博客中我们使用了四元数法计算点集配准。

    本篇我们使用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即可。

    具体证明可以参考:

    代码如下:

    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');

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

    原始点集:

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

    配准后点集:

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

  • 相关阅读:
    leetcode 268. Missing Number
    DBSCAN
    python二维数组初始化
    leetcode 661. Image Smoother
    leetcode 599. Minimum Index Sum of Two Lists
    Python中的sort() key含义
    leetcode 447. Number of Boomerangs
    leetcode 697. Degree of an Array
    滴滴快车奖励政策,高峰奖励,翻倍奖励,按成交率,指派单数分级(1月3日)
    北京Uber优步司机奖励政策(1月2日)
  • 原文地址:https://www.cnblogs.com/tiandsp/p/10276994.html
Copyright © 2011-2022 走看看