高翔orbslam_高翔《视觉SLAM十四讲》从理论到实践

目录php

第1讲 前言：本书讲什么；如何使用本书；html

第2讲 初始SLAM：引子-小萝卜的例子；经典视觉SLAM框架；SLAM问题的数学表述；实践-编程基础；前端

第3讲 三维空间刚体运动 旋转矩阵；实践-Eigen；旋转向量和欧拉角；四元数；类似、仿射、射影变换；实践-Eigen几何模块；可视化演示；算法

第4讲 李群与李代数 李群李代数基础；指数与对数映射；李代数求导与扰动模型；实践-Sophus；类似变换群与李代数；小结；编程

第5讲 相机与图像 相机模型；图像；实践-图像的存取与访问；实践-拼接点云；windows

第6讲 非线性优化 状态估计问题；非线性最小二乘；实践-Ceres；实践-g2o；小结；后端

第7讲 视觉里程计1 特征点法；特征提取和匹配；2D-2D：对极几何；实践-对极约束求解相机运动；三角测量；实践-三角测量；3D-2D：Pnp；实践-求解PnP；3D-3D：ICP；实践-求解ICP；小结；前端框架

第8讲 视觉里程计2 直接法的引出；光流(Optical Flow)；实践-LK光流；直接法(Direct Methods)；实践-RGBD的直接法；服务器

第9讲 实践章：设计前端 搭建VO前端；基本的VO-特征提取和匹配；改进-优化PnP的结果；改进-局部地图；小结数据结构

第10讲 后端1 概述；BA与图优化；实践-g2o；实践-Ceres；小结

第11讲 后端2 位姿图(Pose Graph)；实践-位姿图优化；因子图优化初步；实践-gtsam；

第12讲 回环检测 回环检测概述；词袋模型；字典；类似度计算；实验分析与评述；

第13讲 建图 概述；单目稠密重建；实践-单目稠密重建；实验分析与讨论；RGBD稠密建图；TSDF地图和Fusion系列；小结；

第14讲 SLAM：如今与将来 当前的开源方案；将来的SLAM话题；

附录A 高斯分布的性质

附录B ROS入门

第1讲 前言

1.1 本书讲什么

讲的是SLAM(Simultaneous Localization And Mapping，同步定位与成图)，即利用传感器来进行机器人的自身定位以及对周围环境的成图。根据传感器来划分主要分为雷达SLAM和视觉SLAM。这里固然主要讲的是视觉SLAM。

从定义上能够看出SLAM主要解决的是“自身定位”和“周围环境的成图”。

这里主要把SLAM系统分红几个模块：视觉里程计、后端优化、建图以及回环检测。

这是一个很复杂的过程，共分十四讲来说述，每一讲都有一个主题，而后介绍该主题的算法和实践，即从理论到实践。

ok，这讲就到这里了。

第2讲 初始SLAM

这讲主要是讲SLAM的基本框架，以及开发前期准备工做。

如上图所述，SLAM基本框架就是这样。一个机器人，靠一个摄像头来感知周围的世界并评估自身的位置，它须要利用Visual Odometry视觉里程计来经过相邻两张相片来计算姿态参数估算距离角度这些用来计算自身位置xyz以及恢复图像上各点的位置pxpypz，因为有上一步有累积偏差须要Optimization后端优化，对于周围环境的感知须要Mapping地图构建并配准，最后须要Loop Closure回环检测计算闭合偏差并修正。

前期准备工做有：

准备一台电脑和一个Turtlebot机器人(没有也没关系，只要有一台Kinect相机就能够了，你能够用手拿着它模拟机器人的走动，它会还原你的位置信息)。这台电脑和Turtlebot机器人都是安装的Linux操做系统，没错，机器人身上有一台电脑做为控制，电脑就是一个高级点的单片机，这是它的大脑控制着它发射信息行走以及计算汇总，而另外一台电脑做为咱们的服务器它接收机器人发送的信息如图片等以及给机器人发送指令。但愿安装的是Ubuntu这款Linux操做系统，由于咱们之后将在该操做系统下下载安装软件。ROS软件就不用说了，这是机器人操做系统，是必备的，能够利用它与机器人完成交互传递信息发送指令等。另外咱们还须要配置开发环境，由于咱们须要开发咱们本身的程序，调节参数。这是ROS所不具有的。

配置开发环境: Kdevelop。

编写实例程序: HelloSLAM。

第3讲 三维空间刚体运动

这讲主要是讲三维几何空间中刚体的运动方程。

a'=Ra+t

其中R为旋转矩阵，t为平移矩阵

将R和t融合到一个矩阵中，将上式变为线性方程

那么编程如何实现上式呢？用Eigen开源库。

Eigen使用方法：添加头文件，编写程序。

旋转向量与旋转矩阵之间的变换：

，其中n为旋转向量，R为旋转矩阵

旋转向量只须要三个变量，比旋转矩阵三维矩阵的9个变量要简洁，节省运算成本。

由旋转向量到四元数：因为旋转矩阵的冗余，加上旋转向量和角度的奇异性，引入四元数，即用复数来表示旋转从而避免了奇异性。

用四元数来表示旋转：设空间点p=[x,y,z]，以及一个由旋转轴和角度指定的旋转，那么旋转后的点p'用四元数怎么表示呢？咱们知道使用矩阵描述的话p'=Rp。而用四元数表达：

1. 把三维空间点用一个虚四元数描述：

p=[0, x, y, z]=[0, v]

这至关于咱们把四元数的三个虚部与空间中的三个轴相对应。用四元数q表示这个旋转：

q=[cos(θ/2), ncos(θ/2)]

那么旋转后的点p'便可表示为这样的乘积：

p'=qpq^(-1)

2. 四元数到旋转矩阵的转换

那么用Eigen演示如何进行各类旋转变换。在Eigen中使用四元数、欧拉角和旋转矩阵，演示它们三者之间的变换关系。

Eigen中各个数据类型总结以下：

旋转矩阵(3×3)：Eigen::Matrix3d。

旋转向量(3×1)：Eigen::AngleAxisd。

欧拉角(3×1)：Eigen::Vector3d。

四元数(4×1)：Eigen::Quaterniond。

欧氏变换矩阵(4×4)：Eigen::Isometry3d。

仿射变换(4×4)：Eigen::Affine3d。

射影变换(4×4)：Eigen::Perspective3d。

本讲结束。

第4讲 李群与李代数

三维旋转矩阵构成了特殊正交群SO(3)，而变换矩阵构成了特殊欧氏群SE(3)

但不管SO(3)，仍是SE(3)，它们都不符合加法封闭性，即加以后再也不符合旋转矩阵的定义，可是乘法却知足，将这样的矩阵称为群。即只有一种运算的集合叫作群。

群记做G=(A, .)，其中A为集合，.表示运算。群要求运算知足如下几个条件：

(1)封闭性。

(2)结合律。

(3)幺元。一种集合里特殊的数集。

(4)逆。

能够证实，旋转矩阵集合和矩阵乘法构成群，而变换矩阵和矩阵乘法也构成群。

介绍了群的概念以后，那么，什么叫李群呢？

李群就是连续(光滑)的群。一个刚体的运动是连续的，因此它是李群。

每一个李群都有对应的李代数。那么什么叫李代数呢？

李代数就是李群对应的代数关系式。

李群和李代数之间的代数关系以下：

可见二者之间是指数与对数关系。

那么exp(φ^)是如何计算的呢？它是一个矩阵的指数，在李群和李代数中，它称为指数映射。任意矩阵的指数映射能够写成一个泰勒展开式，可是只有在收敛的状况下才会有结果，它的结果仍然是一个矩阵。

一样对任意一元素φ，咱们亦可按此方式定义它的指数映射：

因为φ是三维向量，咱们能够定义它的模长θ和方向向量a知足使φ=θa。那么，对于a^，能够推导出如下两个公式：

设a=(cosα, cosβ, cosγ),可知(cosα)^2+(cosβ)^2+(cosγ)^2=1

(1)a^a^=aaT-I

(2)a^a^a^=-a^

上面两个公式说明了a^的二次方和a^的三次方的对应变换，从而可得：

exp(φ^)=exp(θa^)=∑(1/n!(θa^)n)=...=a^a^+I+sinθa^-cosθa^a^=(1-cosθ)a^a^+I+sinθa^=cosθI+(1-cosθ)aaT+sinθa^.

回忆前一讲内容，它和罗德里格斯公式一模一样。这代表，so(3)实际上就是由旋转向量组成的空间，而指数映射即罗德里格斯公式。经过它们咱们把so(3)中任意一个向量对应到了一个位于SO(3)中的旋转矩阵。反之，若是定义对数映射，咱们也能把SO(3)中的元素对应到so(3)中：

但一般咱们会经过迹的性质分别求解转角和转轴，那种方式会更加省事一些。

OK，讲了李群和李代数的对应转换关系以后，有什么用呢？

主要是经过李代数来对李群进行优化。好比说，对李群中的两个数进行运算，对应的他们的李代数会有什么变化？

首先是，两个李群中的数进行乘积时，对应的李代数是怎么样的变化，是否是指数变化呢？可是注意，李群里的数是矩阵，不是常数，因此不知足ln(exp(A+B))=A+B，由于A,B是矩阵，不是常数，那么是怎么的对应关系呢？

是

第5讲 相机与图像

以前介绍了机器人的运动方程，那么如今来说相机的原理。相机最先是根据小孔成像原理产生的最先相机。后面又有了加凸透镜的相机。

相机模型：

这个公式表示了世界坐标Pw到像素坐标Puv的转换。表示了从世界坐标(Pw)到相机坐标(经过外参R,t)再到像素坐标(经过K内方位元素)的转换。

对于凸透镜的镜头畸变则采用：

进行径向畸变纠正和切向畸变纠正。其中k1,k2,k3为径向畸变参数，而p1,p2为切向畸变参数。

而对于相机标定，即求解内方位参数K，能够采用OpenCV, Matlab, ROS三种方法求解。参照：连接。

接下来是使用OpenCV处理图像的示例。OpenCV是处理图像的类库，是很是重要的。

那么接下来呢，固然是将上一步求的相机内参数应用到实际的图像点云数据处理上。

有了相机内参数和相片，就能够恢复像点的像点X,Y,Z坐标吗？不能，由

可知，要求像点X,Y,Z除了内参数矩阵外，还须要像点的(x,y,w)，而从像片上只能获得x,y，那么w是距离，怎么得到呢？经过RGD-D相机能够得到深度数据，即w。(经过Kinect获取彩色和深度图像，保存 显示)

假设你已经经过Kinect得到彩色和深度图像了，那么就能够经过上式恢复像素的相机坐标点了(即经过彩色图像(x,y)和深度图像(w)就能够获得点云了)。而后就能够(经过IMU获得的位姿参数)当前点的R,t这些旋转矩阵和平移矩阵(外参)恢复到它的世界坐标Pw(Xw, Yw, Zw)。

以上两个文件夹分别为彩色图像和深度图像，一一对应。

接下来来写一个经过两张彩色图像和对应的两张深度图像获得点云数据，而且将两个点云数据融合生成地图的程序：