简单来说，slam最主要的内容就是优化理论，利用最小二乘法（牛顿高斯，LM等），在g2o或者ceres库的基础上做BA优化或者位姿图。

1.三维空间的刚体运动

三维空间中刚体的运动可以通过旋转矩阵，旋转向量，四元数，欧拉角（不怎用）来进行描述，旋转矩阵和旋转向量之间可以通过欧德里格斯公式进行相互转换。但是旋转矩阵用9个量描述3个自由度的旋转，具有冗余性，欧拉角存在万向锁的问题，旋转向量具有周期性，因为引入了一个紧凑且不具有奇异性的概念：四元数，一种类似于复数的代数。

自我感觉这一部分数学公式之间的推导不用过于花费心思，都可以通过Eigen库实现相互之间的转换。因此，Eigen的使用就是核心。

//定义一个2*3的float类型的矩阵，前三个参数：数据类型，行，列
Eigen：：Matrix<float,2,3>matrix_23;
//Eigen::Vector3d实质上是Eigen::Matrix<double,3,1>; Matrix3d、MatrixXd等等；
//定义一个三维的向量
Eigen::Vector3d v_3d
//注意矩阵的维度和类型
Eigen：：Matrix<double,2,1>result=matrix_23.cast<double>()*v_3d;//cast->强制类型转换
 matrix_33 = Eigen::Matrix3d::Random();      // 随机数矩阵
 cout << matrix_33 << endl << endl;
 
 cout << matrix_33.transpose() << endl;      // 转置
 cout << matrix_33.sum() << endl;            // 各元素和
 cout << matrix_33.trace() << endl;          // 迹
 cout << 10*matrix_33 << endl;               // 数乘
 cout << matrix_33.inverse() << endl;        // 逆
 cout << matrix_33.determinant() << endl;    // 行列式
// 通常用矩阵分解来求逆，例如QR分解
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
//将旋转矩阵转化为欧拉角， ZYX顺序，即roll pitch yaw顺序
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); 
//可以直接把旋转向量和旋转矩阵赋值给四元数：
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );

2.李群和李代数

李群是指具有连续性质的群。旋转矩阵和变换矩阵都与矩阵乘法构成群，每个李群都有与之对应的李代数。李代数需要满足相关性质。旋转矩阵是自身带有约束的（正交且行列式为0），优化时引入额外的约束，优化困难，转化为李代数之后把位姿转为无约束的优化问题。

在这种情况下，SO3和so3之间实就是旋转矩阵空间和旋转向量空间之间的关系。变换矩阵（SE3）本身就是由R+t构成组合形式。此外李代数对应李群的正切空间，它描述了李群局部的导数。

相机在三维空间中是连续的旋转或者变换，SLAM目的就是优化求解相机的这个最佳的位姿T（变换矩阵），优化方法一般都采用迭代优化的方法，每次迭代都更新一个位姿的增量delta，使得目标函数最小。这个delta就是通过误差函数对T微分得到的。也就是说我们需要对变换矩阵T求微分（导数），我们先以SO(3)空间中的旋转矩阵 R为例来说说吧，你觉得如何对R求微分呢？

李代数求导和扰动模型

一般都用第二种，就是对李群进行左乘或者右乘微小的扰动，然后对该扰动求导。

对R的一次扰动量为△R。设左扰动△R对应的李代数为φ，对φ求导即为问题解：

$\frac{\partial\mathbf{R\vec{p}}}{\partial\varphi}=\lim\limits_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}.$

$\begin{aligned} \frac{\partial\mathbf{R\vec{p}}}{\partial\varphi} &=\lim\limits_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}\\ &\approx \lim\limits_{\varphi\rightarrow 0} \frac{ (1 + \varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}\\ &= \lim\limits_{\varphi\rightarrow 0} \frac{\varphi^\wedge \exp(\Phi^\wedge)\vec{p} }{\varphi} = \lim\limits_{\varphi\rightarrow 0} \frac{\varphi^\wedge \mathbf{R}\vec{p} }{\varphi}\\ &= \lim\limits_{\varphi\rightarrow 0} \frac{ - (\mathbf{R}\vec{p})^\wedge \varphi }{\varphi}=-(\mathbf{R}\vec{p})^\wedge. \end{aligned}$

详细求导网址：http://www.mathsword.com/diff_se3_so3/

核心结论有两个：

第一个结论：

通过学习，了解到旋转矩阵的微分是一个反对称(也叫斜对称)矩阵（A^T = -A）左乘它本身，这就印证了矩阵是可以微分的。对于某个时刻的R(t)（李群空间），存在一个三维向量φ=（φ1，φ2，φ3）（李代数空间），用来描述R在t时刻的局部的导数。对于反对称矩阵，举个例子，对于等式A^T = -A，左边第2行第1列位置的元素，是矩阵A元素a12转置后到了位置a21，等式右边原来a21变成了 -a21，所以其实对于矩阵A，元素a12 = -a21，所以用一个元素及其负数就可以表示矩阵中这两个元素，同理，其他4个元素也是这样。所以，其实矩阵A中非对角线元素只用3个元素就可以表示。也就是说反对称矩阵A只有3个自由度。反对称矩阵只有3个自由度很重要啊，这样我们就可以把一个三维向量和一个三维矩阵建立对应关系。

即：

$a=[a_1,a_2,a_3]^T$

$A=\begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 &-a_1 \\ -a_2 & a1 & 0\end{bmatrix}\quad$

那么 $a^\wedge = A ,A^\vee =a$ ,通过这个符号，我们把向量和矩阵建立了对应关系。

第二个结论：

通过书中一系列辛苦的计算，得到下面式子，它的前提是R在原点附近的一阶泰勒展开，我们看到这个向量φ=（φ1，φ2，φ3）反应了R的导数性质，故称它在SO(3)上的原点 φ0 附近的正切空间上。这个φ正是李群大SO(3)对应的李代数小so(3)。

李代数so(3)是三维向量φ的集合，每个向量φi的反对称矩阵都可以表达李群(大O(3))上旋转矩阵R的导数，而R和φ是一个指数映射关系。也就是说，李群空间的任意一个旋转矩阵R都可以用李代数空间的一个向量的反对称矩阵指数来近似。

为什么需要李群和李代数？

下面举个例子说明。比如你拿着相机一边移动一边拍，假设某个时刻相机的位姿是T，它观察到一个在世界坐标系中的一个空间点p，并在相机上产生了一个观测数据z，那么：

$z=Tp+noise$

观测误差就是：

$error=z-Tp$

假设我们总共有N个这样的三维点p和观测值z，那么我们的目标就是寻找一个最佳的位姿T，使得整体误差最小化，也就是:

$min_{T} J(T) = \sum_{n=1}^N\left \| z_i - Tp \right \|_2^2$

求解此问题，就是求目标函数J对于变换矩阵T的导数。首先转化为李代数之后把位姿转为无约束的优化问题。其次T所在的SE(3)空间，对加法计算并不封闭，也就是说任意两个变换矩阵相加后并不是一个变换矩阵，这主要是因为旋转矩阵对加法是不封闭造成的，它是有约束的。李代数就是解决这个问题的。我们把大写SE(3)空间的T映射为一种叫做李代数的东西，映射后的李代数我们叫做小se(3)好了。它是由向量组成的，我们知道向量是对加法封闭的。这样我们就可以通过对李代数求导来间接的对变换矩阵求导了。

3.相机模型

世界坐标系： $（x_w,y_w,z_w）$ $(x_w,y_w,z_w)$

相机坐标系： $(x_c,y_c,z_c)$

像素坐标： $（x,y）$ $(u,v)$

$x_c=f\frac{x_w}{z_w},y_c=f\frac{y_w}{z_w}$

$u=\alpha x_c+u_0,v=\beta y_c+v_0$

另 $\alpha f=f_x,\beta f=f_y$ ,得到：

$u=f_x\frac{x_w}{z_w}+c_x,v=f_y\frac{y_w}{z_w}+c_y$

坐标系转换

1 世界坐标系到相机坐标系（旋转矩阵）

2 相机坐标系到图像坐标系（透视投影矩阵 3X4矩阵）

3 图像坐标系到像素坐标系（K 内参矩阵）

$K=\begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{bmatrix}$

$Z P_{uv}=Z\begin{bmatrix}u\\v\\1\end{bmatrix}=K(RP_w+t)=KTP_w$

4.状态估计的最小二乘

运动方程（相机位姿） $x_k = f(x_{k-1},u_k)+w_k$

观测方程（针孔模型） $z_{k,j}=h(y_j,x_k)+v_{k,j}$

$x_k = T_k = \exp({\xi_k}^\wedge)$ ，是一个六自由度的位姿，可以由变换矩阵描述，也可以用李代数来描述。

$sz_{k,j}=K\exp(\xi^\wedge)y_j$ ， $y_j$ 即所看到的路标点，也就是世界坐标系下的点，内参×外参×路标点 = 像素点距离×像素

问题：当已知观测方程和运动方程的具体形式，如何对估计值进行优化？也就是说已知 $u_k,z_{k,j}$ ,如何得到 $x_k,y_k$ ?

答：

状态变量，所有时刻的位姿x 和所有时刻的路标（地图）y

$x={x_1,...,x_N,y_1,...,y_M}$

所以状态估计等同于求解条件分布，即求解 $P(x|z,u)$ 的概率分布。最完整的描述为：在k个时间点上，基于初始状态信息、一系列观测数据、一系列输入，以及系统的运动模型和观测模型，来计算系统的真实状态估计值。

不知道输入控制u，只有图像时，忽略u并只考虑观测数据

后验概率：P(x|z)：已知图像z，推断x的分布

似然概率：p(z|x)：在某一个状态x下观测图像（确定图像与当前图像的相似程度）

先验概率：P(x)：x自己的状态

${x^*}_{M A P} = \arg \max P(x|z) = \arg \max P(z|x)P(x)$

贝叶斯告诉我们，求解最大后验概率等价于最大似然和先验概率的乘机，当我不知道机器人的位姿或者路标在哪里是，也就没有了先验，那么问题转化为求解最大似然估计。最大似然估计直观的可以理解为：在怎样的x下拿到的相机数据和现在数据（已有的）是最像的！！！或者说在怎样的状态下，最可能产生现在观测到的数据。

求解最大似然估计的方法最常使用的是非线性最小二乘（高斯牛顿和LM等），相关推导查看书籍SLAM十四讲即可。

同时要学会使用ceres和g2o来求解非线性优化问题。其实感觉就是学会使用这两个模板。

5.视觉里程计

视觉里程计：根据相邻图像的信息估计相机的运动，作为后端的初值。

ORB特征提取：理解并会编写ORB特征提取。

计算相机运动

2D-2D

特征匹配之后，通过Opencv里面的findFundamentalMat()函数计算基础矩阵，通过findEssentialMat()函数计算本质矩阵，如果两个图像在同一平面，还需要使用相关函数计算单应矩阵。最后使用内部函数recoverPose()从基础矩阵或者本质矩阵中恢复出R,t。

三角测量的目的：获得像素的深度信息。

3D-2D(PnP)

使用ceres和g2o解决最小重影误差问题。

6.后端

个人理解后端是：后端优化考虑的是一段时间的状态估计问题，保持对当前状态的估计，加入新的信息，进而更新对当前状态的估计。在后端使用位姿图或者BA优化整个轨迹，也就是一段时间的最优状态量估计问题。数学推导书中已详细给出，其核心依旧是使用ceres和g2o解决最小重影误差问题。

7.回环检测

首先，在视觉SLAM问题中，位姿的估计是一个递推的过程，也就是由上一帧位姿解算当前帧位姿，所以我们的位姿约束都是与上一帧建立的，但是每一次估计位姿都有误差，随着位姿递推的进行，误差也在不断的累计位姿，也就形成了我们所说的累计误差，这样将会导致长期估计的结果不可靠，或者说，我们无法构建全局一致的轨迹和地图。

回环检测的关键就是有效的检测出相机曾经经过同一地方这件事。类似于机器学习中的分类。直观的理解就是回环边将带有累计误差的边拉到了正确的位置--如果回环本身正确的话。

回环检测的方法（如何检测回环是否发生）

（1）最简单的方法：对任意两个关键帧进行特征匹配

（2）基于里程计的方法

（3）基于外观的方法

相较其他两种方法，基于外观的方法优点更加突出，更被人普遍接受，是目前回环检测中的主流方法。

基于外观的方法，它和前端、后端的估计都无关，仅根据俩副图像的相似性确定回环检测关系，这种做法摆脱了积累误差，使回环检测模块成为SLAM一个相对独立的模块。

首先，在这里，需要明确的一点是我们不能采用让两个图像直接相减，然后取某种范数。因为像素灰度是一种不稳定的测量值，它严重受环境光照和相机曝光的影响。假设相机未动，我们打开了一支电灯，那么图像会整体变亮一些。这样，即使对于同样的数据，我们都会得到一个很大的差异值。另一方面，当相机视角发生少量变化时，即使每个物体的光度不变，它们的像素也会在图像中发生位移，造成一个很大的差异值。

在基于外观回环检测算法中，核心问题是如何计算图像间的相似性。比如对于图像A和图像B，我们要计算它们之间的相似性分：s(A,B)。这个评分会在某个区间内取值，当它大于一定量后我们认为出现了一个回环。

准确率和召回率

回环检测的结果分类

这里写图片描述

其中假阳性又称为感知偏差，假阴性称为感知变异。真阳性(True Positive)简称TP，假阳性称为(False Positive)简称FP。显然，我们都希望所有的结果中FP和FN尽可能要低。对于某特定算法，我们可以统计它在某个数据集上的TP、FP、FP、FN的出现次数，并统计俩个统计量：准确率Presicion=TP/(TP+FP)和召回率Recall=TP/(TP+FN)。
从上述公式可以得到，准确率描述的是，算法提取的所有回环中确实是真实回环的概率。而召回率是说在所有真实回环中被正确检测出来的概率。

我们发现一个现象就是Precision和Recall通常是矛盾的：

P高则R低

R高则P低

由于在这里我们针对的是SLAM，而在SLAM中我们对准确率要求更高，而对召回率则可以进行适度的牺牲。

2.词袋模型

词袋（ Bag-of-Words（BoW）），目的是用“图像上有哪几种特征”来描述一个图像。例如，如果某个照片，我们说里面有一个人、一辆车；而另一张则有两个人、一只狗。根据这样的描述，可以度量这两个图像的相似性。

字典

字典中的每个元素可以看作相邻特征点的集合，我们就将这一问题转化成了一个聚类问题，采用K-means均值来做：

1. 随机选取 k 个中心点：c1,...,ck；

2. 对每一个样本，计算与每个中心点之间的距离，取最小的作为它的归类；

3. 重新计算每个类的中心点；

4. 如果每个中心点都变化很小，则算法收敛，退出；否则返回 1。

词袋对特征的聚类：

·特征聚类形成Word

·许多Word组成了Dictionary

·图像的相似性=Word的相似性

·只看Word的有无，无视Word的顺序

K-means存在小问题，就是：需要指定聚类数量，随机选取中心点等。
为了解决这个问题，我们使用一种K叉树来表达字典。假定我们构建一个深度为d、每次分叉为k的树，那么做法如下：

1. 在根节点，用 k-means 把所有样本聚成 k 类（实际中为保证聚类均匀性会使用 k-means++）。这样得到了第一层。

2. 对第一层的每个节点，把属于该节点的样本再聚成 k 类，得到下一层。

3. 依此类推，最后得到叶子层。叶子层即为所谓的 Words。

这样一个 k 分支，深度为 d 的树，可以容纳 kd 个单词。另一方面，在查找某个给定特征对应的单词时，只需将它与每个中间结点的聚类中心比较（一共 d 次），即可找到最后的单词，保证了对数级别的查找效率。

3. 相似度计算

原则上通过之前的步骤，我们得到了很多Word，我们似乎已经可以比较Word了，也就是可以计算图像相似度。但是我们会发现我们对于所有的Word都是“一视同仁”的。但是我们知道，一些Word是常见的，另一些则很罕见。我们希望对单词的区分性或重要性加以评估，给它们不同的权值以起到更好的效果。

针对上述问题，这里引入TF-IDF（Term Frequency-Inverse Document Frequency）：

思路：单词在字典中出现频率越高，则区分度越低/在图像中频率越高-则分类图像时区分度越高

IDF部分可在字典训练过程中计算

TF部分则需要对图像的特征进行计算

IDFi = log n ni

另一方面，TF 部分则是指某个特征在单个图像中出现的频率。假设图像 A 中，单词 wi 出现了 ni 次，而一共出现的单词次数为 n，那么 TF 为：

TFi = ni n

于是 wi 的权重等于 TF 乘 IDF 之积：

ηi = TFi ×IDFi.

考虑权重以后，对于某个图像 A，它的特征点可对应到许多个单词，组成它的 Bag-ofWords：

A = {(w1,η1),(w2,η2),...,(wN,ηN)} ∆ = vA.

由于相似的特征可能落到同一个类中，因此实际的 vA 中会存在大量的零。无论如何，通过词袋，我们用单个向量 vA 描述了一个图像 A。这个向量 vA 是一个稀疏的向量，它的非零部分指示出图像 A 中含有哪些单词，而这些部分的值为 TF-IDF 的值。

接下来的问题是：给定 vA 和 vB，如何计算它们的差异呢？这个问题和范数定义的方式一样，存在若干种解决方式，比如 [102] 中提到的 L1 范数形式：

s(vA −vB) = 2
N ∑ i=1
|vAi|+|vBi|−|vAi −vBi|

若想要进行实践，可以参考《视觉SLAM十四讲》回环检测部分，这里就不加以叙述了。

8建图

我们所谓的地图，即所有路标点的集合。一旦我们确定了路标点的位置，那就可以说我们完成了建图。

地图的作用：（1）定位；（2）导航；（3）避障；（4）重建；（5）交互

莫名其妙的明明

发布了16 篇原创文章 · 获赞 1 · 访问量 4008

私信关注

SLAM十四讲-个人总结