VSLAM算法中的数学基础知识详细了解

2023-05-16

学习SLAM经验告诉我，入门SLAM一般只需要两种两个方面的条件，一是要有扎实的数学基础，二是要有强大的动手编程能力，但是这两个条件对于刚入门的同学来说，极具挑战性。

学习SLAM的心里历程：本来先研究了基于滤波系列的源码，嗯，感觉还能看得懂，然后去研究VINS-MONO、ORBSLAM系列，咬咬牙，坚持着。直到碰见了ICE-BA源码，我的心情——我在哪？我能回去吗？我裂开了。

本文主要大致介绍一下SLAM所需的数学知识，先从SLAM算法框架来介绍，SLAM一般分为前端和后端，前端主要是视觉，后端一般分为滤波和非线性优化两种，具体的可以参考近十年的VI-SLAM算法综述与发展

因此，VSLAM主要涉及两大方面的知识，一方面是前端的计算机视觉，另一方面后端的优化（滤波也可以看成是优化的一种，在这便于介绍，也将回环检测加到后端了）

SLAM所需的基础知识

1.计算机视觉

计算机视觉涉及的知识有很多，针对图像处理相关的知识，其中包含了特征提取（ORB,SUFT,SIFT fast,harris,shi-thomas等），特征匹配（光流法，GMS，极线搜索块匹配等），多视图几何（对极约束，单应矩阵，基础矩阵，本质矩阵，三角化等）等等

还有视觉传感器的相关知识，包括相机模型，图像畸变，相机标定，如果是VIO的算法框架还需要VIO标定等，这里也将ICP,PNP,Sim3算法包括在这里了

2.优化

2.1 滤波

滤波主要涉及卡尔曼滤波的原理以及相关改进，包括EKF、UKF、IKF等等，还有概率论

2.1非线性优化

非线性优化主要涉及梯度下降、牛顿法、高斯-牛顿法、LM算法、bundle adjustment等知识。很多具体计算都会涉及到线性代数、矩阵论（QR分解，LDLT分解，SVD分解等）还有泰勒展开，求（偏）导，积分等。是不是在本科和硕士期间都学过。

3.李群李代数

这一部分知识应该很多人都没有接触过，在SLAM中很重要，因此，单独列出来了。

接下来详细介绍这些知识的相关理论。

刚体运动

刚体，顾名思义，是指本身不会在运动过程中产生形变的物体，如相机的运动就是刚体运动，运动过程中同一个向量的长度和夹角都不会发生变化。刚体变换也称为欧式变换。视觉SLAM中使用的相机就是典型的刚体，而运动一般包括形式包括旋转、平移、缩放、切变等，SLAM中旋转是表达形式相对比较复杂，总共有4种方法，分别为旋转矩阵、四元数、欧拉角、旋转向量。

1.旋转矩阵

旋转矩阵在SLAM系统中用的比较多，它是一个正交矩阵，其自身带有约束，就是R和R的转置的乘积是单位阵，且行列式值为1。旋转矩阵R的逆矩阵表示了一个和R相反的旋转，这一性质在实际实践过程中两个相对坐标系的转换是非常方便的。通常情况下，我们会将旋转和平移通过齐次坐标的方法整合成齐次的变换矩阵T，变成欧式变换，目的是可以方便我们表达连续的欧式变换。齐次这个概念以及引入齐次坐标的原因下面会详细介绍，这里先不详细介绍，由于旋转矩阵3维方阵，信息比较冗余。

2. 四元数

四元数可能很多人没有接触过，但是学习SLAM一定要学会基本的性质，首先四元数，顾名思义，就是四个数组成，一个实部，一个虚部。如下图所示：

根据实部在四元数中的位置不同，又分为哈密尔顿Hamilton和JPL，在写代码的时候这一点需要注意。

3. 旋转向量

用一个旋转轴n和旋转角θ来描述一个旋转，所以也称轴角。存在旋转向量到钻展矩阵的变换公式，叫做罗德里格斯公式。这种表达方式存在奇异性。

4.欧拉角

欧拉角就是将一次旋转分解成3次绕不同坐标轴的旋转，例如偏航-俯仰-滚转”（yaw，pitch，roll），可以很直观的表示旋转，但是一般在SLAM中不怎么用，因为有一个万向锁问题。

上面我们介绍了刚体运动相关知识，接下来我们介绍在SLAM中哪里用到了这些知识。首先我们讨论一个很基础的问题：如何描述机器人的位姿。注意这里“位姿”的包含了位置和姿态，描述位置是很简单的。根据应用场景不同，需要不同的表达方式，如果机器人在平面内运动，那么用两个坐标来描述它的位置：

$p=\left ( x,y \right )^{_{T}^{}}$

相应的，如果它在三维空间中，就用三个空间坐标来表示：

$p=(x,y,z)^{_{T}^{}}$

在这里我们可以这么理解，位置就是相对世界坐标系的平移，姿态的表达比点稍为复杂。2D的姿态可以只用一个旋转角 $\theta$ 表达。3D姿态的表达方式则有多种。就是我们上面介绍的4中旋转表达方式。有了位置和姿态，我们就可以描述一个坐标系。进一步，还能描述坐标系间的变换关系，其实整个SLAM就是为了精确求解这两个物理量的。

齐次坐标

在学习SLAM的过程中，经常会遇到术语——齐次坐标，那么什么是齐次坐标呢？在位姿转换中，通常采用射影空间的齐次坐标表示,记n维射影空间为Ｐn，其中一个空间点的坐标为普通的3D坐标加一个齐次分量，这就是齐次坐标

例如：在2维和3维射影空间中的点，分别表示为：

$P^{_{2D}}=(x,y,w)^{_{T}^{}}$ $P^{_{3D}}=(x,y,z,w)^{_{T}^{}}$

这里应该会有疑惑，为什么3维点要用四个数表示?

简单举个例子：

在一个2维的平面上，存在两个点p1(x1,y1),p2(x2,y2)

两点之间的坐标关系为

我们可以用矩阵形式表达

缩放用矩阵形式表达：

旋转用矩阵的形式表达：

根据矩阵的乘法法则，介意将缩放和旋转化简为

这样看起来是不是简单了很多，但是第一项加法怎么办呢，怎么样采用让它也用乘法的形式表达呢。

这个时候就引入了齐次坐标，我先说一下齐次坐标在这里的作用吧

齐次坐标的作用，把各种变换都统一了起来，即把缩放，旋转，平移等变换都统一起来，都表示成一连串的矩阵相乘的形式。保证了形式上的线性一致性。

有没有感觉到数学的艺术美！

同样的也可以将旋转和缩放用矩阵表达为：

这样可以直接将旋转和平移以及缩放均变成矩阵乘法的形式

而在SLAM理论推导过程中，大量旋转和平移计算，因此，齐次坐标保证了形式上的线性一致性。其目的主要是合并矩阵运算中的乘法和加法，即它提供了用矩阵运算把二维、三维甚至高维空间中的一个点集从一个坐标系变换到另一个坐标系的有效方法。

齐次坐标还存在如下的优点：

1.非常方便的表达点在直线或平面上

根据点在平面上的充分必要条件：

存在一个点p(x1,y1),其在直线l：ax+by+c=0上的充要条件为ax1+by1+c=0

如果点用齐次坐标p’(x1,y1,1),则点在直线上表达为：ax1+by1+c*1=0；采用向量的方法表达：p’* $l^{_{}^{T}}$ =ax1+by1+c*1=0;

平面也是一样的，这里就不一一证明了。

2.方便表达直线与直线，平面与平面的交点

在齐次坐标下，可以用两个点 p, q 的齐次坐标叉乘结果来表达一条直线 l，也就是l = p x q；也可以使用两条直线 l, m 的叉乘表示他们的交点 x = l x m

3.能够区分一个向量和一个点

3.1从普通坐标转换成齐次坐标时

如果(x,y,z)是个点，则变为(x,y,z,1);

如果(x,y,z)是个向量，则变为(x,y,z,0)

3.2从齐次坐标转换成普通坐标时

如果是(x,y,z,1)，则知道它是个点，变成(x,y,z);

如果是(x,y,z,0)，则知道它是个向量，仍然变成(x,y,z)

4.能够表达无穷远

比如两条平行的直线 ax+by+c=0, ax+by+d=0，可以分别用向量 l = (a, b, c), m = (a, b,d)表示根据前面直线交点的计算方法，其交点为 l x m根据叉乘计算法则

的叉乘结果可以用如下方法计算得到

最终：l x m = (d-c)(b,-a,0)，忽略标量(d-c)，我们得到交点为(b,-a,0)，并且是齐次坐标，如果要转化为非齐次坐标，那么会得到 (b/0, a/0)，坐标是无穷大，可以认为该点为无穷远点，这与我们通常理解的：平行线相交于无穷远的概念相吻合。

因此，如果一个点的齐次坐标中，最后一个元素为0，则表示为无穷远点。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)