matlab练习程序(点集配准的SVD法)

栏目: 编程工具 · 发布时间: 5年前

内容简介:上一篇博客中我们使用了四元数法计算ICP。本篇我们使用SVD计算ICP。下面是《视觉slam十四讲》中的计算方法:

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

本篇我们使用SVD计算ICP。

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

计算步骤如下:

matlab练习程序(点集配准的SVD法)

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

matlab练习程序(点集配准的SVD法)

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

matlab练习程序(点集配准的SVD法)

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

matlab练习程序(点集配准的SVD法)

W是一个3*3的矩阵,对W进行SVD分解,得:

matlab练习程序(点集配准的SVD法)

其中,为奇异值组成的对角矩阵,对角线元素从大到小排列,而U和V为正交矩阵,当W满秩时,R为:

matlab练习程序(点集配准的SVD法)

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

具体证明可以参考:

matlab练习程序(点集配准的SVD法)

代码如下:

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

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

原始点集:

matlab练习程序(点集配准的SVD法)

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

配准后点集:

matlab练习程序(点集配准的SVD法)

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


以上所述就是小编给大家介绍的《matlab练习程序(点集配准的SVD法)》,希望对大家有所帮助,如果大家有任何疑问请给我留言,小编会及时回复大家的。在此也非常感谢大家对 码农网 的支持!

查看所有标签

猜你喜欢:

本站部分资源来源于网络,本站转载出于传递更多信息之目的,版权归原作者或者来源机构所有,如转载稿涉及版权问题,请联系我们

Unity 3D脚本编程

Unity 3D脚本编程

陈嘉栋 / 电子工业出版社 / 2016-9-1 / 79

《Unity 3D脚本编程——使用C#语言开发跨平台游戏》以Unity 3D 的跨平台基础Mono,以及其游戏脚本语言C#为基础进行讲解。全面系统地剖析了Unity 3D 的跨平台原理以及游戏脚本开发的特点。 第1 章主要介绍了Unity 3D 引擎的历史以及编辑器的基本知识;第2 章主要介绍了Mono,以及Unity3D 利用Mono 实现跨平台的原理,并且分析了C#语言为什么更适合Uni......一起来看看 《Unity 3D脚本编程》 这本书的介绍吧!

URL 编码/解码
URL 编码/解码

URL 编码/解码

XML 在线格式化
XML 在线格式化

在线 XML 格式化压缩工具

html转js在线工具
html转js在线工具

html转js在线工具