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

本篇我们使用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. 2.MySQL(二)

    数据之表操作 1.创建表 语法:CREATE TABLE table_name (column_name column_type); create table student( -> id IN ...

  2. Python Selenium 常用方法总结(不断补充)

    还有此篇内容也丰富Selenium常见元素定位方法和操作的学习介绍 selenium Python 总结一些工作中可能会经常使用到的API. 1.获取当前页面的Url 方法:current_url 实 ...

  3. MySQL导入导出实践

    最近一次数据迁移,需要将MySQL的数据导出.处理后导入到新表和ES.这里做个简单记录,方便后续查询. 注: 为了写文章方便及隐私安全,实际内容会有所简化.例如表结构简化.数据库连接部分全部用 xxx ...

  4. SpingBoot 属性加载

    属性加载顺序 配置属性加载的顺序 开发者工具 `Devtools` 全局配置参数: 单元测试上的 `@TestPropertySource` 注解指定的参数: 单元测试上的 `@SpringBootT ...

  5. Java设计模式学习记录-备忘录模式

    前言 这次要介绍的是备忘录模式,也是行为模式的一种 .现在人们的智能手机上都会有备忘录这样一个功能,大家也都会用,就是为了记住某件事情,防止以后自己忘记了.那么备忘录模式又是什么样子的呢?是不是和手机 ...

  6. python bytes和bytearray、编码和解码

    str.bytes和bytearray简介 str是字符数据,bytes和bytearray是字节数据.它们都是序列,可以进行迭代遍历.str和bytes是不可变序列,bytearray是可变序列,可 ...

  7. 分布式系统监视zabbix讲解二之邮件报警通知--技术流ken

    概述 在上一篇博客<分布式系统监视zabbix讲解一技术流ken>中已经详细讲解了如何安装zabbix,本篇博客将详细讲解如何使用zabbix监控另外一台主机,并实现email报警通知机制 ...

  8. NLP入门(四)命名实体识别(NER)

      本文将会简单介绍自然语言处理(NLP)中的命名实体识别(NER).   命名实体识别(Named Entity Recognition,简称NER)是信息提取.问答系统.句法分析.机器翻译等应用领 ...

  9. L1与L2损失函数和正则化的区别

    本文翻译自文章:Differences between L1 and L2 as Loss Function and Regularization,如有翻译不当之处,欢迎拍砖,谢谢~   在机器学习实 ...

  10. 从零开始学安全(九)●OSI参考模型分层

    主要分为7层和网络7层模型一样 物理层主要传输数据比特流  可以理解信号 数据链路层   逻辑层  像是交换机 网络层    又交换机发送到路由器 应用层 应用通信