SLAM十四讲-个人总结

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

1.三维空间的刚体运动

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

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

//定义一个2*3的float类型的矩阵,前三个参数:数据类型,行,列
Eigen::Matrixmatrix_23;
//Eigen::Vector3d实质上是Eigen::Matrix; Matrix3d、MatrixXd等等;
//定义一个三维的向量
Eigen::Vector3d v_3d
//注意矩阵的维度和类型
Eigen::Matrixresult=matrix_23.cast()*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十四讲-个人总结_第1张图片

 

相机在三维空间中是连续的旋转或者变换,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,通过这个符号,我们把向量和矩阵建立了对应关系。

SLAM十四讲-个人总结_第2张图片

第二个结论:

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

SLAM十四讲-个人总结_第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_jy_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特征提取。

SLAM十四讲-个人总结_第4张图片

计算相机运动

2D-2D

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

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

3D-2D(PnP)

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

6.后端

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

7.回环检测

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

SLAM十四讲-个人总结_第5张图片

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

回环检测的方法(如何检测回环是否发生)

(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。
 

SLAM十四讲-个人总结_第6张图片

这样一个 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)交互

 

你可能感兴趣的:(SLAM十四讲-个人总结)