SLAM十四讲笔记（1）第二讲到第六讲书本内容梳理

SLAM十四讲笔记

疑问

1. 在4.3中，是只求这一个矩阵T吗？这个矩阵T描述了相机的位姿吗？也就是说相机的位姿是固定不变的吗?

第二讲 初识SLAM

经过第二讲的学习，我们应该明白SLAM各个组成部分及其功能，同时对SLAM问题的数学描述有简单的认识。

2.2 经典SLAM框架

前端视觉里程计

该部分的任务是估算图像间相机的运动，以及局部地图的样子。

后端非线性优化

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

回环检测

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

建图

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

2.3 SLAM问题的数学描述

u_k为输入，W_k为噪声，v_k也为噪声，x_k为k时刻机器人的位置，y_j为第j个路标，z_k,j为机器人在x_k位置观测到的第j个路标产生的观测数据。
该运动方程和观测方程的求解是slam问题的基本问题。

第三讲 三维空间刚体的运动

经过第三讲的学习，我们应该明白旋转矩阵的表达方法，SO(n)群和SE(n)群的性质以及四元数的相关知识，本讲的大部分内容都是为了优化计算方法，比如避免求逆。

3.1 旋转矩阵

反对称矩阵

反对称矩阵满足A^T=-A，（对角线元素为0），通过反对称矩阵我们可以把矩阵的X乘转换为矩阵与向量的的乘法。

SO(n)特殊正交群

根据此性质，当向量a旋转后，我们的计算可以简化为：
a’ = R^-1a = R^Ta

SE(n)特殊欧氏群

仍然方便了矩阵求逆的运算。

3.3 旋转向量和欧拉角

罗德里格斯公式

反过来由R推角和轴：

知转轴n是R特征值为1对应的特征向量

欧拉角

3.4 四元数

四元数既是紧凑的，也没有奇异性（奇异性就是指函数的不连续或导数不存在）

四元数表示旋转 和四元数到旋转矩阵的转换先略

第四讲 李群李代数

本讲主要问题是在求解相机位姿时需要获得R，t，来使误差最小化，但旋转矩阵因为是SO(n)，具有约束，这将使优化问题计算量变大，故引入李群李代数来简化求解。

4.1李群李代数基础

群

李群是指具有连续性质的群，SO(n)和SE(n)就属于李群。

李代数

so(3)

se(3)

4.2 指数映射

SO(3)和SE(3)上的指数映射

4.3 李代数求导与扰动模型

BCH公式与近似模型

SO(3)李代数上的求导

设某个时刻小萝卜的位姿为 T。它观察到了一个世界坐标位于 p 的点，产生了一个观测数据 z。那么，由坐标变换关系知：z=Tp+w,因为w噪声项的存在，则实际数据与观测数据的误差：e=z-Tp,则假设有N个点和观测，将产生N个上式，那么对位姿的估计相当于找到一个最优的T，使整体误差最小化：

求解这个最优问题有两种方法，一种是李代数求导法，另一种是左扰动/右扰动模型。

李导数求导与扰动模型（左乘）

李导数求导

扰动模型

SE(3)上的扰动模型

第五讲 相机与成像

本讲主要需要理解相机的成像过程。

5.1相机模型

像素坐标系

K为内参数矩阵,这里的P是在相机坐标系下的坐标，若转换为世界坐标系下的坐标,则：

其中隐含的归一化处理（不是很理解）：

畸变

畸变的产生由两个原因：1)透镜自身形状对光线传播的影响，2)机械组装时透镜和成像平面不可能完全平行。
前者产生径向畸变，后者产生切向畸变；径向畸变又分为桶型畸变和枕型畸变，其纠正的方法是使用多项式，而切型畸变则用p₁,p₂来纠正，则：

小结单目相机的成像过程

本讲其他内容就略了。

第六讲 非线性优化

6.1 状态估计问题

下面公式描述了，当我们只考虑观测方程带来的数据时，相当于估计P(x|z)的条件概率分布，若忽略时间上的联系，则这是一个SfM问题：

6.2 最小二乘的引出

对于某次观测：z_j,k=h(y_i,x_k)+v_k,j
因为我们假设v是符合正向分布N（0，Q_k,j）的噪声，则容易知道：
P（z_j,k|x_k,y_i） = N(h(y_i,x_k),Q_k,j)
则我们利用高斯分布的性质，使用最小化负对数法求他的最大似然：

则误差平方和：

我们的目标就是优化J(x)到最小。

简单的最小二乘问题

当我们要知道J(x) = 1/2* ||f(x)||₂²时，若f(x)是很简单的函数，则可以通过求极值的方式求出，不然，就使用迭代的方式：

迭代的方式有一个很重要的问题，就是如何确定△x的值，这时，我们可以通过泰勒展开的方式：

该式中的J(x)表示的是fx偏导组成的雅克比矩阵，而H则是二阶导数（海塞矩阵），若只保留一阶梯度，则增量的方向为：

若保留二阶：

保留二阶的方法又叫做牛顿法。第一种方法即最速下降法过于贪心，容易走出锯齿路线，而第二种方法需要计算H矩阵，增大计算量。

高斯牛顿法

高斯牛顿法对f(x)进行了泰勒展开：

展开目标函数的平方项，并且求关于△x的导数并令其为0：

原则上，H矩阵应该是可逆的（而且正定），但一般J^TJ确实半正定的，即该方法可能会出现J^TJ是奇异矩阵或者病态的情况，此时增量 的稳定性较差，导致算法不收敛。

Levenberg-Marquadt法

该方法属于信赖区域方法，即给△x添加了一个信赖区域，范围为：

则新的框架为:

其中ρ和μ的取值范围都是经验值，对于D可以看做是椭圆，当取单位阵时为球。因为D的引入，L-M优化为带不等式的约束问题，引入拉格朗日乘子转换为无约束的优化问题：