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

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

    原始点集:

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

    配准后点集:

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

  • 相关阅读:
    CSS学习(五)
    1. Git-2.12.0-64-bit .exe下载
    90.bower解决js的依赖管理
    89.[NodeJS] Express 模板传值对象app.locals、res.locals
    88.NODE.JS加密模块CRYPTO常用方法介绍
    87.node.js操作mongoDB数据库示例分享
    50.AngularJs directive详解及示例代码
    49.AngularJs 指令directive之controller,link,compile
    48.AngularJS ng-src 指令
    86.express里面的app.configure作用
  • 原文地址:https://www.cnblogs.com/tiandsp/p/10276994.html
Copyright © 2011-2022 走看看