视觉 SLAM 十四讲-基础数学篇_一阶梯度法-程序员宅基地

技术标签: # SLAM开发笔记  

1、第1讲  前言

SLAM 是 Simultaneous Localization and Mapping 的缩写,中文译作“同时定位与地图构建”。本书介绍以视觉传感器为主体的视觉 SLAM 技 术,分为两部分:第一部分为数学基础篇,第二部分为 SLAM 技术篇。

本书采用C++ 作为编码语言,所有源代码均托管到 github 上:https://github.com/gaoxiang12/slambook

2、第2讲  初识SLAM

(1)引子:小萝卜的例子

如下图所示我们希望机器人小萝卜具有自主运动能力,够在房间里自由的移动,它至少需要知道两件事:1. 我在什么地方?——定位。2. 周围环境是什么样?——建图。

“定位”和“建图”,可以看成感知的“内外之分”。内就是白自身的状态(即位置),外就是外在的环境(即地图)。

视觉 SLAM 是本书的主题,按SLAM照相机的工作方式,我们把相 机分为单目(Monocular)、双目(Stereo)和深度相机(RGB-D)三个大类,此外还有全景相机和事件相机等特殊或新兴的种类。

(2)经典视觉 SLAM 框架

如下图所示即为经典的视觉 SLAM 框架:

我们把整个视觉 SLAM 流程分为以下几步:

  1. 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果在机 器人中,还可能有码盘、惯性传感器等信息的读取和同步。

  2. 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动, 以及局部地图的样子。VO 又称为前端(Front End)。

  3. 后端优化(Optimization)。后端接受不同时刻视觉里程计测量的相机位姿,以及回 环检测的信息,对它们进行优化,得到全局一致的轨迹和地图。由于接在 VO 之后, 又称为后端(Back End)。

  4. 回环检测(Loop Closing)。回环检测判断机器人是否曾经到达过先前的位置。如果 检测到回环,它会把信息提供给后端进行处理。

  5. 建图(Mapping)。它根据估计的轨迹,建立与任务要求对应的地图。

(3)SLAM 问题的数学表述

运动方程:

观测方程:

参数化运动方程:

参数化观测方程:

 

把运动方程和观测方程取成通用的抽象形式,那么 SLAM 过程可总结为两个基本方程:

我们按照运动和观测方程是否为线性,噪声是否服从高斯分布进行分类,分为线性/非线性和高 斯/非高斯系统。

(4)实践:编程基础

这部分主要讲了安装linux系统和使用g++、cmake、IDE等不同编译方式编译HelloSLAM程序,顺带讲了讲如在编程中何使用库。

3、第3讲  三维空间钢体运动

(1)旋转矩阵

1.点和向量、坐标系

2.坐标系间的欧氏变换

3.变换矩阵与齐次坐标

(2)实践:Eigen

这个例程演示了 Eigen 矩阵的基本操作与运算。比如Matrix定义方式:Eigen::Matrix<数据类型, 行数, 列数> Matrix实例名称,矩阵Amatrix_33的转置、求逆、求和、求迹、行列式等方法以及矩阵方程求解等,详见代码。

(3)旋转向量和欧拉角

旋转向量罗德里格斯公式:

以ZY X 为例的欧拉角:

1. 绕物体的 Z 轴旋转,得到偏航角 yaw;
2. 绕旋转之后的 Y 轴旋转,得到俯仰角 pitch;                                                                                                                                                                                              3. 绕旋转之后的 X 轴旋转,得到滚转角 roll。

(4)四元数

(5)相似、仿射、射影变换

(6)实践:Eigen几何模块

(7)可视化演示

4、第4讲  李群与李代数

(1)李群与李代数基础

1)群

群(Group)是一种集合加上一种运算的代数结构。我们把集合记作 A,运算记作 ·,那么群可以记作 G = (A, ·)。群要求这个运算满足以下几个条件:

1. 封闭性: ∀a1,a2 ∈A, a1 ·a2 ∈A.
2. 结合律: ∀a1,a2,a3 ∈ A, (a1 ·a2)·a3 = a1 ·(a2 ·a3).
3. 幺元: ∃a0 ∈A, s.t. ∀a∈A, a0 ·a=a·a0 =a.                                                                                                                                                                                                  4. 逆: ∀a∈A, ∃a−1 ∈A, s.t. a·a−1 =a0.

矩阵中 常见的群有:一般线性群 GL(n),特殊正交群 SO(n),特殊欧氏群 SE(n), 群结构保证了在群上的运算具有良好的性质,而群论则是研究群的各种结构和性质的理论。李群是指具有连续(光滑)性质的群。 SO(n) 和 SE(n),它们在实数空间上是连续的,都是李群。

2)李代数的引出

考虑任意旋转矩阵 R,我们知道它满足:

(假设t0=0,同时在 t0 附近,设 φ 保持为常数 φ(t0) = φ0)

3)李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定 义如下:

李代数由一个集合 V,一个数域 F 和一个二元运算 [, ] 组成。如果它们满足以下几条 性质,称 (V, F, [, ]) 为一个李代数,记作 g。

1. 封闭性 ∀X,Y ∈V,[X,Y]∈V.
2. 双线性 ∀X,Y,Z ∈V,a,b∈F,有:[aX+bY,Z]=a[X,Z]+b[Y,Z], [Z,aX+bY]=a[Z,X]+b[Z,Y].

3. 自反性x ∀X∈V,[X,X]=0.
4. 雅可比等价 ∀X,Y ,Z ∈ V,[X,[Y ,Z]] + [Z,[Y ,X]] + [Y ,[Z,X]] = 0.

其中二元运算被称为李括号。

4)李代数so(3)

5)李代数se(3)

 

(2)指数与对数映射

1)SO(3)上的指数映射

2)SE(3)上的指数映射

(3)李代数求导与扰动模型

1)BCH公式与近似形式

两个李代数指数映射乘积的完整形式,由 Baker-Campbell-Hausdorff 公式(BCH 公 式)x给出:

特别地,考虑 SO(3) 上的李代数 ln (exp (φ∧1 ) exp (φ∧2 ))∨,当 φ1 或 φ2 为小量时,小量二次以上的项都可以被忽略掉。此时,BCH 拥有线性近似表达:

同样的,对于 SE(3),亦有类似的BCH 近似公式:

2)SO(3)李代数上的求导

设某个时刻小萝卜的位姿为 T。它观察 到了一个世界坐标位于 p 的点,产生了一个观测数据 z。那么,由坐标变换关系知:z = T p + w.                                                                我们通常会计算理想的观测与实际数据的误差: e = z − T p.

求解此问题,需要计算目标函数 J 关于变换矩阵 T 的导数。我们经常会构建与位姿有关的函数,然后讨论该函数关于 位姿的导数,以调整当前的估计值。使用李代数解决求导 问题的思路分为两种:                                                                                                                                                                                                              1. 用李代数表示姿态,然后对根据李代数加法来对李代数求导。                                                                                                                                                              2. 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型。

3)李代数求导

4)扰动模型(左乘)

5)SE(3)上李代数上的求导

(4)实践:Sophus

一个较好的李代数库是 Strasdat 维护的 Sophus 库。Sophus库有模板类和非模板类的版本。你可以输入以下命令获得非模板类的 Sophus:

  1. git clone https://github.com/strasdat/Sophus.git

  2. cd Sophus

  3. git checkout a621ff

(5)相似变换群与李代数

(6)小结

本讲引入了李群 SO(3) 和 SE(3),以及它们对应的李代数 so(3) 和 se(3)。我们介绍 了位姿在它们上面的表达和转换,然后通过 BCH 的线性近似,我们可以对位姿进行求导 和扰动了。

5、第5讲  相机与图像

前面两讲中,我们介绍了“机器人如何表示自身位姿”的问题,部分地解释了 SLAM 经典模型中变量的含义和运动方程部分。本讲,我们要讨论“机器人如何观测外部世界”, 也就是观测方程部分。而在以相机为主的视觉 SLAM 中,观测主要是指相机成像的过程。

(1)相机模型

1)针孔相机模型

针孔相机模型

 

设 P 的坐标为 [X, Y, Z]T ,P ′ 为 [X′, Y ′, Z′]T ,并且设物理成像平面到小孔的距离为 f(焦距)。那么,根据三角形相似关系,有:

 

  

2)畸变

径向畸变的两种类型

切向畸变来源示意图

径向畸变

切向畸变

3)双目相机模型

像素点可能存在的位置

双目相机的成像模型

4)RGB-D相机模型

(2)图像

(3)实践:图像的存取与访问

1)安装 OpenCV

2)操作OpenCV图像

(4)实践:拼接点云

 

6、第6讲  非线性优化

(1)状态估计问题

1)最大后验与最大似然

贝叶斯法则左侧通常称为后验概率。它右侧的 P (z|x) 称为似然,另一部分 P (x) 称 为先验。直接求后验分布是困难的,但是求一个状态最优估计,使得在该状态下,后验概 率最大化(Maximize a Posterior,MAP),则是可行的:

请注意贝叶斯法则的分母部分与待估计的状态 x 无关,因而可以忽略。贝叶斯法则告 诉我们,求解最大后验概率,相当于最大化似然和先验的乘积。进一步,我们当然也可以 说,对不起,我不知道机器人位姿大概在什么地方,此时就没有了先验。那么,可以求解 x 的最大似然估计(Maximize Likelihood Estimation, MLE):

“在什么样的状态下,最可能产生现在观测到的 数据”。这就是最大似然估计的直观意义。

2)最小二乘的引出

(2)非线性最小二乘

1. 给定某个初始值 x0。

2. 对于第 k 次迭代,寻找一个增量 ∆xk,使得 ∥f (xk + ∆xk)∥2 达到极小值。

3. 若 ∆xk 足够小,则停止。

4. 否则,令 xk+1 = xk + ∆xk,返回 2.

1)一阶和二阶梯度法

最速下降法

一阶梯度法(最速下降法): 

 

二阶梯度法(牛顿法):   

2)Gauss-Newton

    

1. 给定初始值 x0。
2. 对于第 k 次迭代,求出当前的雅可比矩阵 J(xk) 和误差 f(xk)。                                                                                                                                                                3. 求解增量方程:H∆xk = g.
4. 若 ∆xk 足够小,则停止。否则,令 xk+1 = xk + ∆xk,返回 2.

3)Levenberg-Marquadt

 

 

1.给定初始值 x0,以及初始优化半径 μ。

2.对于第 k 次迭代,求解:

这里 μ 是信赖区域的半径,D 将在后文说明。

3.计算 ρ。

4.若 ρ > 43 ,则 μ = 2μ;

5.若 ρ < 41 ,则 μ = 0.5μ;

6.如果 ρ 大于某阈值,认为近似可行。令 xk+1 = xk + ∆xk。

7.判断算法是否收敛。如不收敛则返回 2,否则结束。

4)小结

这里只罗列了最常见的两 种非线性优化方案,Gauss Newton 和 Levernberg-Marquardt

(3)实践:Ceres

1)Ceres简介

2)安装Ceres

3)使用Ceres拟合曲线

(4)实践:g2o

1)图优化理论简介

图优化,是把优化问题表现成图(Graph)的一种方式。这里的图是图论意义上的图。 一个图由若干个顶点(Vertex),以及连接着这些节点的边(Edge)组成。进而,用顶点 表示优化变量,用边表示误差项。于是,对任意一个上述形式的非线性最小二乘问题,我们可以构建与之对应的一个图。

2)g2o的编译与安装

3)使用g2o拟合曲线

(5)小结

本节介绍了 SLAM 中经常碰到的一种非线性优化问题:由许多个误差项平方和组成的 最小二乘问题。我们介绍了它的定义和求解,并且讨论了两种主要的梯度下降方式:Gauss- Newton 和 Levenberg-Marquardt。在实践部分中,我们分别使用了 Ceres 和 g2o 两种优 化库求解同一个曲线拟合问题,发现它们给出了相似的结果。

 

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/Bonaventure/article/details/110487058

智能推荐

matlab iir 高通滤波器的设计,基于MATLAB的IIR滤波器的设计.doc-程序员宅基地

文章浏览阅读598次。MACROBUTTON MTEditEquationSection2 SEQ MTEqn \r \h \* MERGEFORMAT SEQ MTSec \r 1 \h \* MERGEFORMAT SEQ MTChap \r 1 \h \* MERGEFORMAT 毕业设计 课程设计 毕业论文 详细资料 联系QQ号;1620812008本科毕业设计(论文)基于MATLAB的IIR滤波..._matlab设计iir滤波器为高通滤波器,截至频率为0.5π

pycharm代码上传到github_git commit为什么需要邮箱和名字、-程序员宅基地

文章浏览阅读6.5k次,点赞11次,收藏26次。一.准备工作Ⅰ.git下载和安装要连接GitHub,首先git是必不可少的,git的安装的基本使用很简单,这里并不是讲git使用的教程,所以只是提一下就略过了.Ⅱ.GitHub设置添加SSH Key这一步算是连接GitHub的最基本的一步了,git是分布式的代码管理工具,远程的代码管理是基于ssh的,所以得先配好SSH key.1.创建一个SSH Key打开终端,windows下面可能叫做git ..._git commit为什么需要邮箱和名字、

B+Tree 在线模拟_在线体验b+tree-程序员宅基地

文章浏览阅读2.4k次,点赞8次,收藏9次。链接地址:https://www.cs.usfca.edu/~galles/visualization/BPlusTree.html数据结构可视化:https://www.cs.usfca.edu/~galles/visualization/Algorithms.html_在线体验b+tree

缺失值的查找与替换_ggto05.net-程序员宅基地

文章浏览阅读5.6k次。X<-c(1,2,3,NA,5) y<-c(236,90,56,NA,7) z<-c(54,6558,5,21,5) x1<-data.frame(X,y,z) r<-c(NA,1,2,NA,NA)ee> ee X y z r1 1 236 54 NA2 2 90 6558 13 3 56 5 24 NA NA 21 NA5 5_ggto05.net

R判别分析_判别分析的应用案例r语言-程序员宅基地

文章浏览阅读377次。常用的判别分析方法是距离判别、贝叶斯判别和Fisher判别等。_判别分析的应用案例r语言

Maven基础(三)—— 依赖管理与生命周期_项目依赖管理生命周期-程序员宅基地

文章浏览阅读124次。1、依赖管理1、依赖配置依赖指当前项目运行所需的jar,一个项目可以设置多个依赖2、依赖传递将project03的坐标复制到project02的依赖中直接依赖:在当前项目中通过依赖配置建立的依赖关系间接依赖∶被资源的资源如果依赖其他资源,当前项目间接依赖其他资源路径优先:当依赖中出现相同的资源时,层级越深,优先级越低,层级越浅,优先级越高声明优先:当资源在相同层级被依赖时,配置顺序靠前的覆盖配置顺序靠后的特殊优先:当同级配置了相同资源的不同_项目依赖管理生命周期

随便推点

current_thread_info在ARM32和ARM64平台的不同点解析_arm64 thread_info-程序员宅基地

文章浏览阅读2.5k次。首先对current_thread_info不熟悉的同学可以先百度一下这是个什么东西?我们这里仅仅简单的提一下,current_thread_info用于获取当前进程的信息。ARM32平台相关定义#define THREAD_SIZE_ORDER 1#define THREAD_SIZE (PAGE_SIZE &lt;&lt; THREAD_SIZE_ORDER)union thr..._arm64 thread_info

阿里P8用这份402页多线程与架构设计笔记,征服你 吊打你_阿里线程框架-程序员宅基地

文章浏览阅读121次。前言随着计算机硬件的不断发展,CPU进入多核时代,并发运算因此也上升到了并行运算的高度,作为Java程序员,如何最大化地“压榨”CPU的资源,利用CPU超高的计算速度编写高效快速运行的程序,如何解决CPU与RAM之间速度不匹配的问题呢?今天分享一份《Java高并发编程详解多线程与架构设计》就会帮你解决这些问题,本书结合作者的实践经验,不仅介绍了高速缓存Cache、Cache Line、Java内存模型,以及现代CPU基本架构等相关知识,还深入讲解了Java跨平台的高并发解决方案。本书实._阿里线程框架

宝塔php伪静态规则,使用宝塔管理nginx伪静态规则配置方法技巧-程序员宅基地

文章浏览阅读1.8k次。我们网站使用的服务器环境一般有IIS、Apache、Tomcat、Nginx 等,他们各有优劣。一般虚拟空间使用IIS 和 Apache居多,这两个的配置都比较简单,IIS一般都安装了ISAPI_Rewrite 模块,可以直接使用和Apache一样的.htaccess文件来配置,直接将.htaccess文件上传到网站根目录即可。.htaccess 文件代码RewriteEngine OnRewri..._宝塔怎么添加htaccess规则

2023 Zblog对接微信小程序源码 附教程_博客小程序源码-程序员宅基地

文章浏览阅读189次。在HbuilderX中打开前端项目,依次点击 运行 =》 运行到小程序模拟器 =》微信开发者工具。必要工具:HbuilderX,微信开发者工具。_博客小程序源码

mysql组合索引与单列索引_假设有一张名为orders的订单表-程序员宅基地

文章浏览阅读1.1k次。1、组合索引能够避免回表查询:假设有一张订单表(orders),包含order_id和product_id二个字段。一共有31条数据。符合下面语句的数据有5条。执行下面的sql语句:select product_id from orderswhere order_id in (123, 312, 223, 132, 224);这条语句要mysql去根据order_id进行搜索,然后返回_假设有一张名为orders的订单表

python+jenkins持续集成输出html测试报告_jenkins + python + publish html reports-程序员宅基地

文章浏览阅读7k次。前言在jenkins上展示html的报告,需要添加一个HTML Publisher plugin插件,把生成的html报告放到指定文件夹,这样就能用jenkins去读出指定文件夹的报告了。一、构建后操作1.前面执行完测试用例后,可以用“添加构建后操作步骤”,读出html报告文件2.如果你的展开后有Publish THML reports这_jenkins + python + publish html reports

推荐文章

热门文章

相关标签