视觉SLAM学习路线

导师让我了解SLAM，SLAM原本是比较小众的方向，最近自动驾驶火起来，做这个SLAM的人也多了，反过来也会推动机器人感知的发展。希望未来学成的时候，能赶上机器人大规模普及，就业一片蓝海。

关于视觉SLAM我感觉学习成本高、科研选题难和行业壁垒高，但是充满挑战不是很有意思嘛。

学SLAM方向跟motion planning科研都不好做，而且都很吃数学基础。学习难度的话，planning可能教程更多一点，SLAM相较于planning要小众一些，教程也相对较少，所以在学习《视觉SLAM十四讲》整理一些自己的理解，记录一下学习路线。

一、初识SLAM

1、SLAM是什么？

SLAM (同时定位与地图构建)，是指搭载特定传感器的主体，在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自己的运动。如果这里的传感器是相机，那就称为视觉SLAM
如何使用《视觉SLAM十四讲》
注重理论和实践的结合、一讲一个主题(理论部分+实践部分)

全书由两部分组成:

(1)数学基础篇(前六讲)

①预备知识与SLAM概述(教材第1，2讲)
②三维空间的刚体运动(教材第3讲)
③李群与李代数(教材第4讲)
④相机模型与非线性优化(教材第5，6讲)

(2)实践应用篇(后八讲)

⑤特征点法视觉里程计(教材第7讲)
⑥直接法视觉里程计(教材第8讲)
⑦后端优化(教材第9，10讲)
⑧回环检测(教材第11讲)
⑨地图构建( 教材第12讲)
⑩工程实践(教材第13讲)
⑪SLAM的目前与未来(教材第14讲)

注：
学习本书会接触到–些必要的数学理论和许多编程知识,会用到Eigen、OpenCV、 PCL、 g2o、Ceres等库， 需要掌握他们在Linux操作系统中的使用方法。

学习本书最好具备以下基础:
1.高等数学（积分、求导）、线性代数（矩阵的运算）、概率论（极大似然估计）
2.C++语言基础，经典的slam框架都是用c++写的
3. Linux基础（代码都是在Linux系统上运行的）

2、小萝卜的例子

(1)自主运动的两大基本问题:

①我在什么地方?——定位
②周围环境是什么样? ——建图

(2)定位与建图=内外兼修

定位侧重对自身的了解，建图侧重对外在的了解

(3)相互关联

• 准确的定位需要精确的地图
• 精确的地图来自准确的定位

(4)怎样完成定位和建图？

传感器：机器人感知外界环境的手段
传感器主要分为以下两类:
①携带于机器人本体上的传感器
例：机器人的轮式编码器、相机、激光传感器、惯性测量单元(IMU)等
②安装于环境之中的传感器
例：导轨、二维码标志等

(5)环境传感器的限制

例：GPS：需要能接收到卫星信号的环境
Marker、导轨：需要环境允许安装

(6)本体上传感器优点

• 相比之下，激光、相机等携带式传感器测量的通常都是一些间接的物理量而不是直接的位置数据，所以更加自由使用携带式传感器来完成SLAM也是我们重点关注的问题
• 使用携带式传感器来完成SLAM也是我们重点关注的问题

(7)视觉SLAM是本书的主题，所以我们非常关心小萝卜的眼睛能够做些什么事。即如何用相机解决定位和建图的问题

• SLAM中使用的相机更加简单，以一定速率采集图像、形成视频

(8)相机的特点

• 以二维投影形式记录了三维世界的信息
• 该过程丢掉了一一个维度:距离(或深度)

(9)相机分类

• 单目相机Monocular
• 双目相机(立体相机) Stereo
• 深度相机RGB-D

其他 全景相机（可以观察周围360°的环境）、事件相机Event Camera（科研热点：只要有一个像素值变化，就会回传一个事件，响应速度非常快）、红外相机（科研热点：红外SLAM，传感器融合的SLAM）

①单目相机(只使用一个摄像头的相机)

通过相机的运动形成视差，可以测量物体相对深度。
优点： 结构简单，成本低，便于标定和识别
缺点： 在单张图片里，无法确定一个物体的真实大小。
它可能是一个很大但很远的物体，也可能是一个很近很小的物体。即单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是 尺度(scale) ，单凭图像无法确定这个真实尺度，所以称 尺度不确定性

②双目相机(由两个单目相机组成)

通过基线来估计每个像素的空间位置。** (类似于人眼) **
优点： 基线距离越大，能够测量的距离就越远;并且可以运用到室内和室外。
缺点： 配置与标定较为复杂，深度量程和精度受到双目基线与分辨率限制，计算非常消耗计算资源，需要GPU (图形处理器) /FPGA设备(现场可编程门阵列)加速用两部相机来定位。
深度相机(RGB-D)
通过红外结构光或ToF(time of fly) 的物理方法测量物体深度信息。

③深度相机(RGB-D)

通过红外结构光或ToF(time of fly) 的物理方法测量物体深度信息。
优点： 相比于双目相机可节省大量的计算资源。
缺点： 是测量范围窄，噪声大，视野小，易受日光干扰，无法测量透射材质等问题，主要用在室内，室外很难应用。深度相机主要用来三维成像，和距离的测量。

相机综合的特点：
优点：没有累计误差、直接测量旋转平移、便宜
缺点：要求光照环境相对稳定、不适于高速运动场景、无法应对纹理缺失、单目丢失深度信息、双目计算量大

标定：

• 对象
  [1] 内参：内参数矩阵，将相机坐标系3D空间点映射到2D成像平面上（3D->2D）
  [2] 外参：外参数矩阵，将相机坐标系映射到世界坐标系（3D->3D）
• 方法
  [1] 张正友标定法
  [2] OpenCV
  [3] kalibr
  [4] WiKi/CSDN

(10)IMU

①微机电系统MEMS

[1] 加速度计（f=ma）
[2] 陀螺仪 （科氏力）
[3] 磁力计 （磁场朝向来辨别自己的方向）

②分类

• 按照测量参数
  [1] 三轴
  [2] 六轴
  [3] 九轴
• 按照测量精度

③特点

[1] 优点

• 能够捕捉剧烈变化的运动
• 效果稳定

[2] 缺点

• 噪声大
• 存在bias

[3]am = a - g

④标定

[1] 对象
[2] 方法
[3] 科研：纯IMU的odom（DIDO）

3、经典视觉SLAM框架

流程步骤如下:

(1)传感器信息读取

在视觉SLAM中主要为相机图像信息的读取和预处理。

(2)前端视觉里程计(Visual Odometry, V0)

估算帧间运动，建立局部地图

视觉里程计的任务是估计相邻图像间相机的运动，以及局部地图的样子。(V0又称为前端)

视觉里程计通过相邻帧间的图像估计相机运动，并恢复场景的空间结构，但只计算相邻时刻的运动，不关心再往前的信息。但前端过程中必然存在误差，误差会不断累积，形成累积漂移 (会发现原本直的走廊变成了斜的，而原本90°的直角变成了歪的)。为消除漂移，我们需要回环检测和后端优化。

(3)回环检测(Loop Closure Detection)用于判断机器人是否到达过先前的位置。

判断是否之前来过

• 回环检测的作用:主要解决位置估计随时间漂移的问题(通俗的理解就是，假设机器人经过一段时间又回到了原点(事实)，但是我们的位置估计值没有回到原点，怎么解决)

• 回环检测要达到的目标:通过某种手段，让机器人知道“回到原点”这件事情，让机器人具有识别到过的场景的能力。再把位置估计值“拉”过去。

• 相机检测手段:判断与之前位置的差异，计算图像间相似性。

• 回环检测后:可将所得的信息告诉后端优化算法，把轨迹和地图调整到符合回环检测结果的样子。

(4)后端(非线性)优化(optimization)

修正前端造成的一些误差

对不同时刻的视觉里程计测量的相机位姿及回环检测的信息进行优化，得到全局一致的轨迹和地图。

• 定义:如何处理前端所传噪声的数据，从带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——称为最大后验概率估计

• 通常来说，前端（图像的特征提取与匹配）给后端提供待优化的数据，以及这些数据的初始值。后端（滤波和非线性优化）负责整体的优化过程，它往往面对的就只有数据。

• 反映了SLAM问题的本质:对运动主体自身和周围环境空间不确定性的估计。（状态估计理论一估计状态的均值和不确定性）

(5)建图(Mapping)

根据估计的轨迹，建立任务要求对应的地图

地图大体.上可分为以下两类：
①度量地图(强调精确的表示地图中的位置关系)
常用稀疏与稠密进行分类

• 稀疏地图:即由路标组成的地图（）
• 稠密地图:着重于建模所有看到的东西(可用于导航) (耗费大量的储存空间)

②拓扑地图
拓扑地图(更加强调元素之间的关系)
是一个图:由节点和边组成

• 例:只关注A、B点是连通的，而不考虑如何从A点到达B点
• 不适用于表达较为复杂结构的地图

4、SLAM问题的数学表达

假设小萝卜正携带着某种传感器在位置环境里运动。如何用数学语言描述这件事呢?
我们知道，相机通常是在某些时刻采集数据的，所以我们也关心这些时刻的位置和地图。
.段连续时间的运动变成了离散时刻t=1,2,3,…,k当中发生的事

• x表示小萝卜的位置，x1,x2,…xk表示个时刻的位置
• 假设地图是由许多个路标组成，每个时刻，传感器会测量到一 部分路标点，得到他们的观测数据，设路标点有N个，用y1,y2…yn表示

这样，我们需要考虑以下两件事情:

(1)什么是运动?

从k-1时刻到k时刻，小萝卜的位置是如何变化的?

(2)什么是观测?

假设小萝卜在k时刻xk处观测到了某个路标yj，我们如何用数学语描述呢?

(3)关于运动模型

机器人会携带一个测量自身运动的传感器，这合传感器可以测量有关运动的读数，但不一-定直接就是位置之差，还可能是加速度、角速度这些信息。
我们可以用一个抽象的数学模型来描述:
数学模型：

(4)关于观测模型

机器人在xk位置上看到某个路标点yj产生了一个观测数据zk,j同样可以用一个抽象的数学模型描述：

数学模型：

事实上，根据机器人的真实运动和传感器的种类，存在着若干种参数化形式。而考虑视觉SLAM时，传感器是相机，则观测方程就是“对路标点拍摄后，得到图像中的像素”的过程(第五讲)

(5)举个例子解释:

假设机器人在平面运动，那么位姿(位置+姿态)由两个位置的坐标和一个转角来描述xk=(x, y, Θ)，同时，运动传感器能够测量到机器人在任意两个时间间隔位置和转角的变化量云(Δx, Δy, ΔΘ)T
于是此时的运动方程就可以写成：

(6)这两个方程描述了最基本的SLAM问题

当知道运动测量的读数u以及传感器的读数z时，如何求解定位问题(估计x)和建图问题(估计y)

第三讲《三位空间刚体运动》是讲机器人在k时刻的位姿的表示，第五讲《相机与图像》是讲相机的成像，成像函数h，第六讲《非线性优化》是讲当知道建立这两个方程和里面的一部分未知量之后，怎么把里面的姿态Xk和路标Yj表示出来，用到非线性优化，那么学习第四讲《李群和李代数》是为了在求解非线性优化的时候要涉及梯度，梯度就是求导，对位置可以求导，但对姿态不行，因为姿态是以旋转形成表示的（欧拉角、旋转矩阵）不遵循加减法，只能使用乘法，无法构成求导的格式，为了让他能够求导，引入李代数，李代数是三维空间旋转的一种代数方面的映射，这样就能对方程求导。

(7)环境搭建（Ubuntu18.04）

①虚拟机
VMware或VirtualBox
②ISO系统镜像
国内的镜像源下载速度会比Ubuntu官网快一些
清华大学开源软件镜像站

如果直接git clone高翔博士的slambook2的话，引用部分是无法下载的，这个时候需要一个递归命令，git下载子模块命令

git clone --recursive https://github.com/gaoxiang12/slambook2.git

③常用工具

• htop工具，类似Windows资源管理器，跑SLAM框架，观察每个节点程序的内存占用情况，给你提供一个优化程序的方向
• vim工具，i插入模式，ESC退出插入模式，:wq保存并退出
• terminator工具，超级终端，可以分栏，支持同时操作，在ROS下需要同时在一个工作空间打开（比如source ./devel/stepup.bash把当前工作环境加入到执行的序列里面）
• vscode工具，打开命令code .当前路径打开vscode

sudo snap install --classic code

• ROS工具，机器人操作系统，相当于管理各种功能的一个框架，ROS的安装非常复杂，推荐小鱼的一键安装
• locate命令，查找文件，sudo apt install mlocate安装，查找locate xx | grep xx

wget http://fishros.com/install -O fishros && bash fishros

SLAM在ROS下跑和非ROS下跑的区别：可以把ROS理解成Linux下集成了许多好用的第三方和多进程编程的一个库集。在ROS下跑SLAM可以使用ROS的许多成熟的方法库。比如进程间通信可以默认使用ROSMaster建立的本地socket通信。不在ROS下跑，很多进程间通信、同步、回调之类的就需要自己去设计

二、三维空间刚体运动

1、旋转矩阵与变换矩阵

(1)点、向量和坐标系，旋转矩阵

坐标系的三根坐标轴的方向向量就是基，坐标系能用它的基来表示，如果坐标系是两两垂直的，那么就是标准正交基，标准正交基组成的矩阵是正交矩阵

三维空间由3个轴组成，则一个空间点的位置可以由3个坐标指定，对于一个刚体(在运动和受力作用后，形状和大小不变，而且内部各点的相对位置不变的物体)，不光有位置，还有自身的姿态一相机可以看成三维空间中的刚体，则位置就是说相机在空间中的哪个地方，姿态是指相机的朝向(相机处于空间(0，0，0)处，朝向正前方)

• 点
  点没有长度，没有体积
  点和点可以组成向量

• 向量
  带指向性的箭头(方向性)
  可以进行加法、减法等运算
  向量是一种矩阵，点乘按照矩阵运算，记得要转置（T）
  向量叉乘表示不方便，可以写成矩阵与向量相乘的形式，记住这个运算符（^）

• 坐标
  坐标加坐标系可以表示向量
  当我们指定坐标系后，才可以谈论该向量在此坐标系下的坐标
  

• 坐标系
  构成线性空间的一组基
  左手系和右手系

• 向量的运算可以由坐标运算来表达

• 加减法

• 内积
  

• 外积
  
  
  

(2)世界坐标系和相机坐标系

• 两个不同的坐标系

• 如何描述左侧到右侧的变化?

• 直观来看由两个部分组成
  三个轴的旋转
  原点之间的平移

• 平移是一个向量

• 旋转是什么?
  

• 旋转

• 设某坐标系(e1,e2,e3) 经过一次旋转变成了(e1’,e2’, e3’)

• 对于某个固定的向量a，它的坐标是如何变化的?

• 坐标关系

• 左乘

• 中间的矩阵R称为旋转矩阵
• 可以证明
  R是一个正交矩阵
  R的行列式为±1

证明过程：

• 取行列式为1的正交矩阵称为旋转矩阵
  
• SΟ(n)是特殊正交群

R12表示把姿态由2系对其到1系的旋转矩阵
t表示平移量

(3) 齐次坐标和变换矩阵

上述已经完整的表达了欧式空间的旋转和平移，但不是一个线性关系

但是从a到c的变换为：

很显然，这样叠加起来过去复杂
则我们改写形式

为方便嵌套，写成齐次式子
这样多次变换就可以写成

• 这种用四个数表达玉维向量的做法称为 齐次坐标

• 引入齐次坐标后，旋转和平移可以放在同一个矩阵中，称为变换矩阵
  
  SO3 就是三维空间的旋转
  R属于SO3也就是旋转，t属于三维实数空间，集合T可表示为3X3的R、3X1的t、1X3的零和数1

• 称为特殊欧氏群

• 类似的，可定义反向的变换
  
  我们知道T是从a到b的变换，T的逆就是从b到a的变换。
  旋转矩阵：从a到b的旋转是R，从b到a的旋转是R的逆；从a到b的平移是t，但是从b到a的平移，不是-t，而是 -R的转置t

注意平移并非是简单的取原平移量的相反数，因为是先旋转后平移，所以平移量是受到旋转量的影响的

SO(3)的旋转矩阵有九个量，但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理，变换矩阵用十六个量表达了六自由度的变换。那么，是否有更紧凑的表示呢?

旋转矩阵自身带有约束:它必须是个正交矩阵，且行列式为1.变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得点解变得更困难。

2、旋转向量

(1)除了旋转矩阵之外的旋转表示

旋转矩阵表示旋转是冗杂的（旋转矩阵有9个量，但一次旋转只有3个自由度并且旋转矩阵自身带有约束）

旋转向量：

(2)旋转向量和旋转矩阵的不同

• 旋转矩阵： 9个量，有正交性约束和行列式值约束
• 旋转向量：3个量，没有约束

这两个只是表达方式不同，但是表达的东西是同一个

(3)转换关系

罗德里格斯公式推导：

位姿变换分为：旋转变换和欧式变换
旋转变换就是坐标原点还是重合的，姿态发生变换

欧式变换就是除了坐标发生变换，还有坐标原点的平移
即有旋转又有平移

(4)旋转矩阵与旋转向量

对于转角Θ，取两边的迹，有

对于转轴n，旋转轴上的向量在旋转后不发生改变，说明：

Rn = n

转轴是矩阵R特征值1对应的特征向量

无论是旋转矩阵、旋转向量，虽然它们能描述旋转，但对我们人类是非常不直观的。当我们看到一个旋转矩阵或旋转向量时，很难想象出来这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是向哪个方向在转动。

3、欧拉角

旋转矩阵和旋转向量是不直观的表示，欧拉角提供了一种非常直观的方式
将一个一步到位的旋转分成三次进行，每一次所绕的轴都是坐标轴，这样会很直观，方便人理解

(1)欧拉角

欧拉角不适用于增量式调整，欧拉角会出现死锁现象

• 将旋转分解为三次不同轴上的转动，以便理解
• 例如按照Z—Y—X转动
• 轴顺序亦可不同，因此存在许多种定义方式不同的欧拉角
  

旋转次序问题
选择的旋转顺序不同，欧拉角对同一旋转的描述也不同

• 因为后旋转的轴受先旋转轴的影响，所以旋转顺序和欧拉角数值密切相关
• 要定义欧拉角，先说明旋转轴顺序

(2)万向锁

欧拉角有一个缺点就是万向锁问题

• 在ZYX顺序中，若第二次旋转得到的为正负90°，则第三次和第一次绕同一个轴，使得系统丢失了一个自由度，存在奇异性问题
• 由于万向锁，欧拉角不适合插值和迭代，往往用于人机交互中
• 可以证明，用三个实数来表达三维旋转时，会不可避免地碰到奇异性问题
• SLAM程序中很少直接用欧拉角表示姿态

X轴本来在平行“地面”的平面上，第二次绕y轴旋转土90的话，会变成垂直与地面，这时的x轴与之前的z轴重合

旋转矩阵用九个量描述三自由度的旋转，具有冗余性;欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。

4、四元数

三维的方式都是有奇异的，那么四维就没有奇异了

一种扩展的复数，既是紧凑的也没有奇异性

四元数一般都用q来表示，在定义的时候做初始化，有参初始化

Quaterniond q = Quaterniond(rotation_vector);

(1)四元数有三个虛部，可以表达三维空间中的旋转

$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$
3D空间的旋转也可以用负数表示，只不过虚部有三个维度组成，ijk类似于四元数的旋转操作
$$q=[s,v],s=q_0\in \mathbb{R}，v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$
四元数也能写成向量的形式,zyzw or wxyz，具体看定义为编程准备的形式

• 虚部之间的关系

$$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k，ji=-k\\ jk=i，kj=-i\\ ki=j，ik=-j\end{array}$$

• 单位四元数满足四元数的模长为1

$$q_0^2+q_1^2+q_2^2+q_3^2=1$$
（单位四元数可以表达三维空间的一次旋转）

(2)四元数的一些运算与性质

$$q_a=s_a+x_{a}i+y_{a}j+z_{a}k，q_b=s_a+x_{b}i+y_{b}j+z_{b}k$$

• 乘法
  
  四元数没有交换律！
• 逆

$$q_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b]$$

• 取模
  用来描述姿态的四元数是单位四元数，如果在运算中改变了四元数的模长，记得将其变为单位长度
• 共轭

(3)四元数表示旋转

任意单位向量v,沿着以单位向量定义的旋转轴u旋转度之后的v’可以使用四元数乘法来获得。令v=[0, v]，p=[cos(1/2)Θ，sin((1/2)Θ)u]那么
$$v^{{}^{\prime }}=pv{p}^{\ast }=pv{p}^{-1}$$
并证明变换之后的v实部为0，虛部是罗德里格斯公式结果

$$q_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b]$$

$$v^{{}^{\prime }}=pv{p}^{\ast }=pv{p}^{-1}$$

• 四元数q的性质

• 证明实部为0

虚部是罗德里格斯公式结果


任意单位向量v，沿着以单位向量定义的旋转轴u旋转Θ度之后的v可以使用四元数乘法来获得。
$$令v=[0,v]，p=2[\cos \frac{1}{2}(\theta ),\sin \frac{1}{2}(\theta )u]$$
$$那么v^{{}^{\prime }}=pv{p}^{\ast }=pv{p}^{-1}$$

并证明变换之后的v实部为0，虚部是罗德里格斯公式结果

使用上，
$$p=[0,x,y,z{]}^T=[0,v{]}^T，把三维点扩展到四维$$

$$p^{{}^{\prime }}=qpq{p}^{-1}，使用四元数q对点p进行变换，得到四维点p^{\prime }，将后三维去除即为坐标$$

虽然数学实质如上所述，但是Eigen已经将四元数的旋转运算重载，平时只需要这么算就行了p'=q*p

四元数相比旋转向量的优势：紧凑，无奇异性

四元数好用难理解，欧拉角难用好理解

旋转左乘和右成是有区别的

四种变换之间的转换：

• 对同一旋转/姿态的不同描述方式是等价的
• 描述之间可以相互转化

矩阵微积分 https://en.wikipedia.org/wiki/Matrix_calculus
可用于推到数学公式

三、李群与李代数

误差函数是位姿的函数
Localization就是找使得误差最小的位姿

设某个时刻机器人的位姿为Tcw，它观察到了一个世界坐标位于P的点，产生了一个观测数据Z，根据坐标变换

那我们实际要做的事情是求一个欧氏变换Tcw，使得Tcw满足上式

然而，由于观测噪声w的存在，z往往不可能精确地满足z=Tp的关系。所以，我们通常会计算理想的观测与实际数据的误差：

假设一共有N个这样的路标点和观测，则就有N个上式，则对于机器人的位姿估计，相当于寻找一个最优的T，使得整体误差最小化：

求解此问题，需要解决以下两个问题：
①如何求解目标函数相对于T的导数。
②T对于乘法封闭，对于加法不封闭。

引入李代数

SO(3)是特殊正交群，是一种群，而且还是李群
每种李群都有与之对应的 李代数

1、李群与李代数基础

(1)群的定义

群是一种集合加上一种运算的代数结构
记集合为A，运算为·，那么当运算满足以下性质时，称（A,·）成群

(2)可以验证构成的群

旋转矩阵集合和矩阵乘法构成群 =====> 特殊正交群/旋转矩阵群SO(3)
变换矩阵集合和矩阵乘法构成群 =====> 特殊欧式群/变换矩阵群SE(3)

(3)其他常见的群

①一般线性群GL(n)指n×n的可逆矩阵，它们对矩阵乘法成群。
②特殊正交群SO(n)也就是所谓的旋转矩阵群，其中 SO(2)和SO(3)最为常见。
③特殊欧氏群SE(n)也就是前面提到的n维欧氏变换，如SE(2)和 SE(3)。

(4)证明

• 李群
  具有连续（光滑）性质的群
  直观上看，一个刚体能够连续地在空间中运动，故SO(3)和SE(3)都是李群
  但是，SO(3)和SE(3)只有定义良好的乘法，没有加法，所以难以进行取极限，求导等操作。

• 李代数
  李代数对应李群的正切空间。它描述了李群局部的导数。
  记作so(3)和se(3)

李代数是处在线性空间里，满足加法性闭合，而李群不处于线性空间，高维空间的流型，所以只满足乘法不满足加法

2、李代数的引出1

在零时刻，物体没有移动，位姿旋转的是单位阵
问题：
so(3)的定义和性质
这个指数映射如何求

3、李代数的定义

(1)每个李群都有与之对应的李代数，李代数描述了李群单位元数的正切空间性质

集合V + 数域F + 二元预算[，]

(2)二元运算被称为李括号

例子： $$三维空间向量+R^3+叉积运算构成李代数$$

[，] 二元运算符，李代数定义为李括号

(3)李代数证明SO3

so(3)可以说是三维向量，也可以说是三维矩阵

综上，把李代数理解成向量形式或者矩阵形式都是可以的，向量形式更加自然一些

(4)李代数证明SE3

综上，把李代数理解成向量形式或者矩阵形式都是可以的，向量形式更加自然一些

(5)李代数证明se(3)

se(3)可以说是三维向量，也可以说是三维矩阵

4、指数与对数映射

指数映射反映了从李代数到李群的对应关系
$$R=exp({\Phi }^{\wedge })$$

$$由于{\Phi }^{\wedge }是一个向量，可以定义模长(\theta )和单位方向向量a$$
$$\Phi =\theta a$$
旋转向量

(1)so(3)→SO(3)

指数映射
so(3)是特殊正交群，里面的向量都是正交矩阵，里面的R都是和时间相关的量，所描述的旋转是随时间在变化的



①so(3)的物理意义就是旋转向量，即so(3)的李代数空间就是由旋转向量组成的空间。
②如果李群(旋转矩阵，R(t)，类似一个函数)代表一个球面，那么球上所有点的切线(单位元处李群的切空间李代数，旋转向量），也会组成一个球面，而且这个球面和原来的球面一样。

李代数其实是这种李群在幺元出的切空间，曲线上有一个切线，这个切线就是李代数

(2)SO(3)→so(3)

对数映射

反之，给定旋转矩阵时，亦能求李代数
（把So(3)中的元素对应到so(3)）

但在实际当中没有必要这样求，之前已经介绍了旋转矩阵到旋转向量的转换关系

notes：

BCH公式告诉了我们，指数是李代数^时，除了A+B，还有很多李括号余项

在其中一个为小量时，在李群上做乘法，相当于李代数做加法，只不过根据BCH公式，并不是完全相等，这个加法在so3小量左边乘一个雅克比知阵，而这个雅克比矩阵是Jr还是J要根据对应的SO3相乘的时候，小量代表的SO3在左边还是在右边决定的，在左边就是左雅克比逆，在右边就是右雅克比逆

直观上看，可以这么写

但我们更喜欢这么写;如果写的时候直接带上J，这样李代数相加的时候就没有雅克比逆了，另外，我们还不用想到底是左雅克比还是右雅克比了，因为我们如果把小量指数映射乘在左边，那么你直接就用小的形式乘方边，如果把小量指数映射写右边，那么你就直接用Jr的形式右乘，很好!

两个李群相乘并不能对应的李代数相加，前提是如果有一个量是小量，他们可以近似的，小量前面加一个修正量，可以先乘上小量，变成干干净净的Φ1+Φ2

(3)se(3)→SE(3)

R就是我们熟知的SO(3)上的元素可以理解成李代数的平移部分到李群的平移部分差一个线性变换(雅可比矩阵)

(4)SE(3)→se(3)

可以类比推得其对数映射
但是根据变换矩阵T，求se(3)上的对应向量也有更简单的办法:
从R计算出旋转向量Φ
由t =Jρ(J可由旋转向量中得到，ρ可通过求解线性方程求出)

5、李代数求导与扰动模型

在估计和优化过程中，求导数是非常必要的。

$$但是李群元素只有乘法，无法定义导数R_1+R_2\notin SO(3)$$

已知SO(3)和SE(3)上的李群和李代数关系，但是当SO(3)中完成两个矩阵乘法时，李代数so(3)上发生什么变化?换言之，当so(3)上做两个李代数相加时，SO(3)上是否对应着两个矩阵的乘积?

$$矩阵的指数函数\ln (exp(A)exp(B))=A+B?$$

两个李代数指数映射乘积的完整形式，由BCH公式给出，由于完整形式复杂，只给出展开的前几项：

当其中一个量很小时，忽略其高阶项，BCH具有线性近似形式

so(3)上的BCH近似

se(3)上的BCH近似

建立 李代数加法 与 李群乘法 之间的关系（求导前最后的理论准备），李群是没有办法定义加法的，对加法不封闭，加完之后就不能表示旋转了，但是它的切空间是线性空间，线性空间是支持加法的，所以说我们的思路：`能不能把李群的乘法R·△R这种表示小位姿变换的扰动量来变成李代数的加法，也就是让李代数加一个小的李代数，因为它对加法封闭，这样就可以定义导数了。