[冲啊!!!!!]小白SLAM相关基础知识

SLAM基础问题(黑色是我的答案,红色是未正确回答的部分)

1. Mat是如何访问元素的? 先访问行还是先访问列?

多通道的Mat类矩阵是一个类似于三维的数据，而计算机的存储空间是一个二维空间，因此Mat类矩阵在计算机存储时是将三维数据变成二维数据，先存储第一个元素每个通道的数据，之后再存储第二个元素每个通道的数据。每一行的元素都按照这种方式进行存储，因此如果我们找到了每个元素的起始位置，便可以找到这个元素中每个通道的数据。图2-5展示了一个三通道的矩阵的存储方式，其中连续的蓝色、绿色和红色的方块分别代表每个元素的三个通道。

矩阵常用的属性:

属性	作用	举例 Mat (3, 4, CV_32FC3)
cols	当前矩阵的列数	4
rows	当前矩阵的行数	3
step	以字节为单位的当前矩阵的有效宽度	elemSize()*12=48
elemSize()	每个元素的字节数	32/8*channels()=12
total()	矩阵中元素的个数	=cols*rows = 12
channels()	矩阵的通道数	3 (cuz C3)

矩阵的一些访问:

1. 用at进行访问 at<数据类型>(行数,列数) 需要说明的是，如果矩阵定义的是uchar类型的数据，在需要输入数据的时候，需要强制转换成int类型的数据进行输出，否则输出的结果并不是整数。

访问多通道矩阵, 与上面彩色图片中矩阵的图像结合起来理解, 因为蓝绿红代表的是一个元素,也就代表他们的行数和列数是一样的, 所以对于一个三通道矩阵,按照行数和列数进行访问时,会得到三个数, 在opencv中定义为:cv::Vec3b、cv::Vec3s、cv::Vec3w、cv::Vec3d、cv::Vec3f、cv::Vec3i六种类型用于表示同一个元素的三个通道数据；

2. 用ptr访问. Mat类矩阵在内存中的存放方式，矩阵中每一行中的每个元素都是挨着存放，如果找到每一行元素的起始地址位置，那么读取矩阵中每一行不同位置的元素就是将指针在起始位置向后移动若干位即可。

3. Mat类变量同时也是一个容器变量，所以Mat类变量拥有迭代器，用于访问Mat类变量中的数据，通过迭代器可以实现对矩阵中每一个元素的遍历.

Mat类的迭代器变量类型是cv::MatIterator_< >，在定义时同样需要在括号中声明数据的变量类型。Mat类迭代器的起始是Mat.begin< >()，结束是Mat.end< >()，与其他迭代器用法相同，通过“++”运算实现指针位置向下迭代.

4. 通过矩阵元素地址定位访问;

前面三种读取元素的方式都需要知道Mat类矩阵存储数据的类型，而且在从认知上，我们更希望能够通过声明“第x行第x列第x通道”的方式来读取某个通道内的数据.

这种方式与我们通过指针读取数据的形式类似，都是通过将首个数据的地址指针移动若干位后指向需要读取的数据，只不过这种方式可以通过直接给出行、列和通道数进行读取，不需要用户再进行计算某个数据在这行数据存储空间中的位置。

37. ORBSLAM初始化时为什么要同时初始化H矩阵和F矩阵?

主要是针对初始化时不同的图像中不同的状态, H矩阵适用与特征点都处于同一个平面或者发生纯旋转时的状况, 这种状态时F举矩阵退化, 因此需要使用H矩阵; 而F矩阵适用于特征点在不同深度的情况;

接下来总结一下关于F矩阵,H矩阵和E矩阵的一些相关的知识点:
首先来说E矩阵,也就是本质矩阵, 本质矩阵有几个很重要的特性:

本质矩阵在不同尺度下都是等价的; 对E乘以任意的非常数之后, 对极约束依然满足;
本质矩阵的奇异值必定是[a, a, 0]的形式, 这称为本质矩阵的内在性质;
因为旋转和平移各有三个自由度,所以t^R共有6个自由度, 但是由于第一条的尺度等价性, 实际上E只有五个自由度; 也就是我们用五个点就可以求出E矩阵;

先来说一下E的推导过程(这部分来自<视觉SLAM十四讲> 7.3对极几何):

假设存在空间点 P =[X, Y. Z] (世界坐标中的坐标), 则空间点在第一幅图像上的图像坐标p1 和在第二幅图像上的图像坐标p2 分别可以表示如下:

$s_{1}p_{1}=K*P , s_{2}p_{2}=K(R*P+t) %u516C%u5F0F(1))$ 公式1

使用齐次坐标系, 可以将上面的公式转换成up-to-scale的类型, 就是等式一侧乘以任意非零常数之后,等式仍然成立; 因此上面的式子也就变成了

$p_{1}=K*P , p_{2}=K(R*P+t)$ 公式2

将两个等式做如下一系列转换:

$\because x_{1}=K^{-1}*p_{2}, x_{2} = K^{-1}*p_{2}$ 公式3

这里注意x1和x2都是新出现的, 属于归一化相机平面上的坐标点, 与相机坐标系下P点的真实坐标相差一个尺度; 和上面出现的第一个式子不同, 从空间点P我们可以经过内参和外参得到像素坐标p1,p2, 但是由像素坐标p1,p2就只能推导出无真实尺度的x1,x2了

而,对极几何是由两张图像推导两个相机之间变换的过程, 也就是这个求解问题的已知部分是像素点, 未知部分是R,t 和空间点P;

那么公式2 就转换成了
$%u5E26%u5165%u5230p_{2}%u7684%u516C%u5F0F%u4E2D, %u5373%u6709p_{2} =K(R* K^{-1}* p_{1} +t)$ $p_{1}=K*x_{1} , p_{2}=K(R*x_{2}+t)$

因此就得到了: $x_{2} = R*x_{1}+t$

等式两边同时先左乘 $t^{^}$ ^ 再左乘 $x_{2}^{T}$ 即可得到 $x_{2}^{T}*t{}'*x_{2} = x_{2}^{T}*t{}' R*x_{1}$ 这里用t' 来替代t^ ,纯粹是公式书写的原因;

因为t^x2相当于将t和x2做叉乘,也就是寻找了这两个向量的垂直向量, 因此当这个向量再和x2T做乘的时候, 结果为零; 因此就得到了关于本质矩阵的公式,也就是: $x_{2}^{T}*t{}' R*x_{1}$ , 或者 $p_{2}^{T}K^{-T}*t{}' RK^{-T}*p_{1}$

这两个式子都成为对极约束, 它的几何意义是两个相机的光心,以及物点P共面; 这两个式子中也就包含了本质矩阵E和基础矩阵F, 其中 $E= t{}' R$ , $F = K^{-T}*E*K^{-T}$

有了上面关于本质矩阵和基础矩阵的理解,接下来总结一下在ORBSLAM中是如何应用这两个矩阵(其实是F矩阵, 因为内参已知)进行初始化的; 具体步骤如下:

已知所有的匹配点, CurrentFrame和InitFrame中的关键点已经找好, 分别存放在mvKeys1, mvKeys2中;
对所有的关键点进行归一化处理;;
选择8个点, 然后用SVD分解对F矩阵进行求解(8个点为一组,这里会计算很多组);
求解出F矩阵后,将该矩阵应用到图片的每个特征点中, 利用 $p_{2}^{T}*F*p_{1} = 0$ 这个特性,来检测该F矩阵是否合格; 对于图像一中的每一个点,计算其投影到图像2中之后距离对极线的距离:
利用公式

对F矩阵打分;

上面计算F矩阵的过程, 同样也适用于H矩阵的计算过程,最后根据两者的分数,决定使用F矩阵还是H矩阵;

float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)// 步骤5：从H矩阵或F矩阵中恢复R,t
if(RH>0.40)return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF>0.6)return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

H矩阵也是如此的计算过程, 求出这些矩阵之后,接下来就是通过H或者F恢复出R|t, 对R|t进行重投影误差的验证即可;

补1:说一下最大化后验概率是什么

在解释Dog-Leg算法之前, 需要了解与SLAM相关的几个重要的概念, 先验(prior), 后验(posterior),似然(Likehood);

这三个概念所对应的公式分别如下, 一直x是结果(如买东西), $\theta$ 是原因(双十一来了,或者钱有点多), 即导致x发生的原因, 则

先验: $P(\theta )$ 就是原因发生的概率

似然: $P(x|\theta )$ 也就是在某个原因发生时,导致特定结果的概率, 如对于我来讲,双十一发生的情况下, 我买东西的概率就是百分之百

后验: $P(\theta |x)$ 也就是已知结果的情况下,求某个原因发生的概率, 如已知我今天剁手了,那是因为双十一的概率有多大呢?

后验概率可以粗略地说是一种“马后炮”或者“事后诸葛亮”，就是结果都已经看到了，再根据结果推测某个原因发生的概率；

这三个概念在一起组成了最常见到的贝叶斯法则：

贝叶斯法则： $P(\theta |x) \propto P(x|\theta) *P(x)$

接下来还是要和SLAM相结合在一起, 在SLAM中原因一般对应为前一时刻相机的位姿, 结果一般对应为当前时刻相机的观测数据；

假设状态变量为x，其中不仅包含了位姿，还有路标点，统称为状态，对机器人状态的估计，就是已知输入数据u 和观测数据z的条件下，求计算状态x的条件概率分布：

$\mathbf{\mathit{\mathit{}}P(x|z,u)}$ ,

为了估计状态变量的概率分布，我们需要借助于上面的贝叶斯法则，也就是： $P(x|z) = \frac{P(z|x) *P(x)}{P(z))} \propto P(z|x) *P(x)$

一般，已知一个相机的观测结果去推测当前相机的位姿是有些难度的，也就是后验分布的估计是比较难的，但是我们可以去估计后验分布的最大值，即求一个状态x，使得后验概率的值最大，这也就是最常见到的MAP，最大化后验概率(Maximaz a Posterior)

最大化后验概率的公式: $x^{*} _{MAP} = argmax(P(\theta |x))$ 即求一个状态量x，使得在这个状态下得到结果 $\theta$ 的概率最大

根据贝叶斯法则，求最大后验也就相当于求最大似然与先验的乘积，如果不知道相机的位姿，也就是可以说是直接求最大似然估计，因此最大化后验概率的公式也就转换成了最大似然估计：

因为x表示状态变量， z表示观测结果，因此最大似然P(z|x) 可以理解为在当前状态下x下，得到观测结果z的概率，或者也可解释为：在什么样的状态下，最可能产生现在观测到的数据；

这种情况成为最大似然估计(Maximize Likehood Estimation, MLE):

最大似然估计: $x^{*} _{MLE} = argmax(P(x|\theta ))$

到此，也就理解了最大似然在SLAM中意义；

补2：SLAM中最小二乘问题的引入及求解：

这一部分是我自己加的，对于理解非线性优化以及那些常用的优化算法有很大的帮助，所以有时间也可以看看，互相交流；

我们都已经知道SLAM中的问题求解其实就是要求出一个位姿，使得噪声项的平方，即误差最小化。因此，对于所有的运动和任意的观测，都可以定义数据与估计值之间的误差：

运动方程的误差： $e_{\emph{v,k}} = \textit{x}_{k} - f(x_{k-1},u_{k})$

观测方程的误差： $e_{\emph{y,j,k}} = \textit{z}_{k,j} - h(x_{k},y_{j})$

对这两项误差做平方并求和，也就的到了一个经典的最小二乘问题。

直观地讲，由于噪声的存在，当我们把估计的轨迹与地图带入SLAM的运动，观测方程中时，他们并不会完美地成立。这时怎么办呢？我们对状态的估计值进行微调，使得整体的误差下降了一些，当然这个下降也是有限度的，他一般会达到一个最小值。

上面这段话，其实也就阐明了整个最小二乘求解的关键，也就是求解一个增量值 $\Delta x$ ，x基于该增量的变化，使得误差方程达到一个极小值。以下面的非线性最小而成为例，

$min_{x}\frac{1}{2}\left \| f(x) \right \|_{2}^{2}$

如果f(x)是一个简单的函数，那么我们就可以用解析式求解，令目标函数导数为0，求x的最优解即可；即

$\frac{d(\frac{1}{2}\left \| f(x) \right \|_{2}^{2})}{dx} = 0$

解了这个方程，就得到了令导数为0的极值，它可能是极大，极小或者鞍值，将他们带入到函数中，算一下结果，比较一下就可以；但是如果这个函数不方便求解呢？我们就可以用迭代的方法，从一个初始值出发，不断地更新当前的优化变量，使得目标函数的值下降，具体步骤可以列写如下：

给定某个初始值 $x_{0}$

对于第k次迭代，寻找一个增量 $\Delta x_{k}$ ，使得 $\left \| f(x_{k}+\Delta x_{k}) \right \|_{2}^{2}$ 达到极小值

若 $\Delta x_{k}$ 足够小，则停止；

否则，令 $x_{k+1} = x_{k} + \Delta x_{k}$ , 则返回2步；

上面这四个步骤就是得一个求解导函数为零的问题变成了一个不断寻找梯度并下降的过程。

38. 最速梯度下降法，牛顿法，高斯牛顿法，LM算法的区别

将几种优化方法一起说一下：
首先说一下需要优化的问题：　 $min_{x}\frac{1}{2}\left \| f(x) \right \|_{2}^{2}$ , 其中ｘ是自变量，而ｆ(x)是任意的非线性函数；对非线性最小二乘问题的求导，上面说道，转换成求解一个增量，使得 $\left \| f(x_{k}+\Delta x_{k}) \right \|_{2}^{2}$ 达到最小；

求解增量的方法可以分为以下几种：最速下降法，牛顿法，高斯牛顿法，列文伯格-马夸尔特法等，这些方法的区别可以解释如下：

如果将目标函数以如下方式进行泰勒展开：

可以推导得到最速下降法（将函数展开到一阶）和牛顿法（将函数展开到如上二阶）；

因此利用最速下降法得到求解公式为：

而牛顿法将目标函数转换为求解

$\Delta x^{*} = arg min\left \| f(x)) \right \|_{2}^{2} +\mathbf{J(x)}\Delta x + \frac{1}{2}\Delta x^{T}\mathbf{H}\Delta x$

求得增量的解：　

上面的方法都是基于对目标函数f(x)^2进行展开后求解，　另一种方法不是对目标函数操作，而是对f(x)进行一阶泰勒展开之后再带入到目标函数中，即

将这个函数展开并且对 $\Delta x$ 求导，　即可得到　

$\Delta x^{*} =\frac{1}{2} arg minf(x)^{2} +2*f(x)^{T}J(x)*\Delta x+\Delta x^{T}J(x)^{T}J(x)\Delta x$

对该式求导，可以得到：

这也就是高斯牛顿法，主要是求解增量方程 $H\Delta x=g$ , 其中Ｈ是用一阶导Ｊ近似二阶导海森矩阵的，　因此也叫作Ｈ；

列文伯格－马夸尔特方法就是基于高斯牛顿法，　又对该展开添加了信赖区域后得到的；即优化问题转换为带有不等式约束的优化问题；

对该式同样进行展开求解，可以得到

基于上面的增量方程，列文伯格提出将Ｄ看做是单位阵，　而马夸尔特则提出将Ｄ看做是非负数对角阵，　因此有了列文伯格－马夸尔特方法，　这中方法在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题。

REF:

从零学Opencv: https://cloud.tencent.com/developer/article/1534424

均值,方差,归一化和标准化:https://www.jianshu.com/p/583b289841e7

最大似然和高斯：https://blog.csdn.net/li8zi8fa/article/details/76999747

SLAM十四讲6,2