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

本篇我们使用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=-::
for j=-::
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(:)' Y(1:3000)' Z(:)']; %生成变换后点集
i=0.5;j=0.3;k=0.7;
Rx=[ ; cos(i) -sin(i); sin(i) cos(i)];
Ry=[cos(j) sin(j); ;-sin(j) cos(j)];
Rz=[cos(k) -sin(k) ;sin(k) cos(k) ; ];
R=Rx*Ry*Rz;
X=P*R + [0.2,0.3,0.4]; plot3(P(:,),P(:,),P(:,),'b.');
hold on;
plot3(X(:,),X(:,),X(:,),'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(:,),P(:,),P(:,),'b.');
hold on;
plot3(X(:,),X(:,),X(:,),'r.');
plot3(Pre(:,),Pre(:,),Pre(:,),'go');

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

原始点集:

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

配准后点集:

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

matlab练习程序(点集配准的SVD法)的更多相关文章

  1. matlab练习程序(对应点集配准的四元数法)

    这个算是ICP算法中的一个关键步骤,单独拿出来看一下. 算法流程如下: 1.首先得到同名点集P和X. 2.计算P和X的均值up和ux. 3.由P和X构造协方差矩阵sigma. 4.由协方差矩阵sigm ...

  2. 点集配准技术(ICP、RPM、KC、CPD)

    在计算机视觉和模式识别中,点集配准技术是查找将两个点集对齐的空间变换过程.寻找这种变换的目的主要包括:1.将多个数据集合并为一个全局统一的模型:2.将未知的数据集映射到已知的数据集上以识别其特征或估计 ...

  3. matlab练习程序(SUSAN检测)

    matlab练习程序(SUSAN检测) SUSAN算子既可以检测角点也可以检测边缘,不过角点似乎比不过harris,边缘似乎比不过Canny.不过思想还是有点意思的. 主要思想就是:首先做一个和原图像 ...

  4. (转)matlab练习程序(HOG方向梯度直方图)

    matlab练习程序(HOG方向梯度直方图)http://www.cnblogs.com/tiandsp/archive/2013/05/24/3097503.html HOG(Histogram o ...

  5. matlab练习程序(射线法判断点与多边形关系)

    依然是计算几何. 射线法判断点与多边形关系原理如下: 从待判断点引出一条射线,射线与多边形相交,如果交点为偶数,则点不在多边形内,如果交点为奇数,则点在多边形内. 原理虽是这样,有些细节还是要注意一下 ...

  6. matlab练习程序(PCA<SVD>)

    clear all;close all;clc;img1=imread('Corner.png');img2=imread('Corner1.png');img3=imread('Corner2.pn ...

  7. matlab练习程序(Levenberg-Marquardt法最优化)

    上一篇博客中介绍的高斯牛顿算法可能会有J'*J为奇异矩阵的情况,这时高斯牛顿法稳定性较差,可能导致算法不收敛.比如当系数都为7或更大的时候,算法无法给出正确的结果. Levenberg-Marquar ...

  8. matlab示例程序--Motion-Based Multiple Object Tracking--卡尔曼多目标跟踪程序--解读

    静止背景下的卡尔曼多目标跟踪 最近学习了一下多目标跟踪,看了看MathWorks的关于Motion-Based Multiple Object Tracking的Documention. 官网链接:h ...

  9. matlab练习程序(透视投影,把lena贴到billboard上)

    本练习程序是受到了这个老外博文的启发,感觉挺有意思,就尝试了一下.他用的是opencv,我这里用的是matlab. 过去写过透视投影,当时是用来做倾斜校正的,这次同样用到了透视投影,不过更有意思,是将 ...

随机推荐

  1. Ocelot简易教程(二)之快速开始2

    为什么这篇的标题叫"Ocelot简易教程(二)之快速开始2"呢,因为很多朋友跟我说上一篇" Ocelot简易教程(二)之快速开始1"内容太少了,只是简单介绍Oc ...

  2. [Java 泥水匠] Java Components 之一:Java String (肯定有你不懂的)

    作者:泥沙砖瓦浆木匠网站:http://blog.csdn.net/jeffli1993个人签名:打算起手不凡写出鸿篇巨作的人,往往坚持不了完成第一章节. 1.1 前言 说起String,大家最熟悉不 ...

  3. Netty实现简单HTTP代理服务器

    自上次使用Openresty+Lua+Nginx的来加速自己的网站,用上了比较时髦的技术,感觉算是让自己的网站响应速度达到极限了,直到看到了Netty,公司就是打算用Netty来替代Openresty ...

  4. leetcode — combination-sum-ii

    import java.util.ArrayList; import java.util.Arrays; import java.util.List; /** *Source : https://oj ...

  5. Elasticsearch 学习总结 - 相关配置补充说明

    一.   Elasticsearch的基本概念 term索引词,在elasticsearch中索引词(term)是一个能够被索引的精确值.foo,Foo Foo几个单词是不相同的索引词.索引词(ter ...

  6. man exportfs(exportfs命令中文手册)

    本人译作集合:http://www.cnblogs.com/f-ck-need-u/p/7048359.html exportfs() System Manager's Manual exportfs ...

  7. 第一册:lesson forty five。

    原文: The boss's letter. A:Can you come here a minute please,Bob? B:Yes,sir. A:Where is C? B:She is ne ...

  8. JQuery官方学习资料(译):类型

    类型     JavaScript提供了多个内置数据类型.除了这些,这份文档还将介绍一些虚拟类型,例如选择器.伪类.事件等. String 在JavaScript中字符串是一个不可变的对象,它包含无. ...

  9. C#-变量类型(值类型、引用类型)

    第一次发这样的笔记呢! 这个是在再读基础的时候感觉自己理解的东西吧 变量的类型差异在数据的存储方式不一样,值类型是变量本身直接存储数据,另一个则是存储实际变量的引用, 值类型:都是存储在栈中的,都是直 ...

  10. [PHP]算法-队列结构的PHP实现

    题目描述 用两个栈来实现一个队列,完成队列的Push和Pop操作. 队列中的元素为int类型. 思路: 1.php数组完全就能实现 2.array_push 从尾部往里压入元素 3.array_shi ...