SLAM（重要知识点）

吕福

已于 2022-09-07 21:41:02 修改

阅读量2.6k

点赞数 6

分类专栏： SLAM学习文章标签： c++

于 2022-03-22 21:51:33 首次发布

本文链接：https://blog.csdn.net/lv15076050705/article/details/123644797

版权

SLAM学习专栏收录该内容

4 篇文章 2 订阅

订阅专栏

欧拉角的重大缺点

会碰到著名的万向锁问题，在俯仰角为正负 90°时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转。这被称为奇异性问题。

李群李代数的引入

在做SLAM研究的时候，会涉及到对旋转矩阵求导的问题。然而矩阵求导中多个变换矩阵相加问题很难解决，这时候需要使用矩阵李群的知识，将旋转或者变换等矩阵以李群形式，映射到李代数上求解。旋转矩阵对加法不封闭，就是说两个旋转矩阵相加并不是一个旋转矩阵。因此在利用最小二乘求最小化误差的过程中，不能直接对旋转矩阵求导。但是，李代数是由向量组成的，对加法封闭，因此我们可以通过对李代数求导来间接对旋转矩阵求导。
SLAM的定位问题中，需要对相机的平移和旋转进行估计，其中涉及到有关旋转和平移的函数优化问题。使用李代数的处理方式，你可以将此类问题转化为无约束的优化，便于求解。
**两种求导方式：**由于李代数由向量组成，因此对加法运算封闭。所以李代数可以根据我们熟悉的导数定义(加法)进行求导。然而这种求导的结果中有复杂的Jacobian，所以一般采用微扰法进行求导：对李群进行微扰然后对该扰动求导。结果分别为 -（Rp) ^J 和 -（Rp) ^。

批量状态估计

状态估计就是一直一段时间的运动方程和观测数据推断当前的机器人的状态（位姿和路标点的位置）。我们把状态看作服从高斯分布的随机变量，就是估计状态的均值和协方差矩阵。批量估计问题可以转换成最大似然估计问题，利用最小二乘进行求解。
卡尔曼滤波及其衍生方法都是增量的估计问题，只跟前一时刻的状态有关。（马尔可夫性），视觉一般以非线性优化为主。

批量估计问题转换成最小二乘的非线性优化问题：

对于机器人状态估计的问题可以建模为：一直机器人的运动和观测估计机器人的状态p(x,y|z,u)
利用贝叶斯法则：
p(x,y|z,u)（后验概率） = p(z,u|x,y)*p(x,y)/p(z,u) ≈ p(z,u|x,y)p(x,y)（似然先验）
第一项是似然估计，第二项是先验估计，最大化后验估计就变成了最大化先验和似然估计的乘积
最大化后验问题直观理解就是在已知观测数据和运动数据现在机器人最可能在什么位置。
那么似然估计的直观理解就是在什么状态下最可能产生当前的观测数据。

转换成最小二乘优化问题：

观测方程：z=h(y,x) +v,v 是观测误差（可以是激光雷达或者是相机的观测误差）
观测数据z符合高斯分布O(h(y,x),Q)，Q是观测误差的协方差矩阵。
所以最大化这个条件概率，转换成最小化负对数min((z - h(y,x))Q-1(z - h(y,x)T),这个式子就转换成了最小二乘问题了，Q是信息矩阵。
通常假设各个时刻的输入和观测是相互独立的，输入和观测也是独立的，所以联合概率在对数中是相乘的形式，取负对数就是相加的形式了，所以考虑多个时刻现在问题就变成了批量优化最小二乘问题。
这里的信息矩阵代表观测的误差的协方差矩阵的倒数，观测越准，协方差越小，信息矩阵越大，在优化的残差中占的比例就越大，优化越按照它来优化，非常符合实际，越准越按照这个来优化。

几种优化方法

最速下降法：目标函数在xk处泰勒展开，保留一阶项，x*= - J（xk），最速下降法过于贪心，容易走出锯齿路线，反而增加迭代次数。
牛顿法：二阶泰勒展开，利用二次函数近似原函数。H*X= - J,牛顿法需要计算目标函数的海森阵，计算量大。规模较大时比较困难。
高斯牛顿法：fx进行一阶泰勒展开，fx而不是Fx,高斯牛顿法用JJT来作为牛顿法中二阶海森阵的近似，HX=g,在使用高斯牛顿法时，可能出现JJT*为奇异矩阵或者病态的情况，此时增量稳定性较差，导致算法不收敛。
列文伯格–马夸尔特方法（LM）：基于信赖区域理论，是由于高斯-牛顿方法在计算时需要保证矩阵的正定性，于是引入了一个约束，从而保证计算方法更具普适性。(H+入I)x=g,当入较小时，H占主导，近似于高斯牛顿法，较大时，入* I 占主导，接近最速下降法。

ICP-----PL-ICP

误差形式不同，点到点距离与点到线距离误差
收敛速度不同，ICP为一阶，PL-ICP为二阶收敛
PL-ICP精度更高
PL-ICP对初始值更敏感

贝叶斯滤波

已知状态量t-1时刻的概率分布，在给定t时刻的观测数据（Zt,Ut）的情况下估计出状态量在t时刻的概率分布。

卡尔曼滤波（Kalman filter）

一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
线性系统的卡尔曼滤波方法：
运动方程和观测方程都是线性方程，卡尔曼滤波假设了马尔可夫性，我们知道了k-1时刻的后验分布估计当前时刻的后验分布。可以分为一下几步：
第一步：利用k-1的均值和协方差预测当前时刻的先验均值跟协方差
第二步：利用先验的均值和协方差的卡尔曼增益稀疏
第三步：更新后验的均值和协方差
一般SLAM系统不是线性系统，我们一般将系统做运动方程在k-1时刻的一阶泰勒展开，将观测方程在预测出的k时刻先验处一阶泰勒展开近似为线性系统，计算步骤跟线性系统一致。
EKF的局限性：

假设了马尔可夫性，当前数据可能跟之前的数据有关。
EKF在距离工作点的地方一阶泰勒展开可能不准，只相当于进行了一次迭代更新，没有优化的方法准，优化是每优化一次再在优化之后的点处展开。
需要存储状态量的均值和协方差，存储量太大，不适合大型场景。
EKF系统没有异常检测机制，遇到异常值可能会发散。

拓展：EKF（扩展卡尔曼滤波）把卡尔曼滤波扩展到非线性系统中，通常的做法是，在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。

EKF和BA的区别：
（1）EKF假设了马尔可夫性，k时刻的状态只与k-1时刻有关。
（2）非线性优化使用所有的历史数据，全局slam。
（3）EKF做了线性化处理，在工作点处用一阶泰勒展开式近似整个函数，工作点较远处不一定成立。（4）非线性优化每迭代一次，状态估计发生改变，会对新的估计点做泰勒展开。

总而言之，EKF可被看作是只有一次迭代的BA。

基于滤波和基于优化的区别以及做法？

slam的后端一般分为两种处理方法，一种是EKF（扩展卡尔曼滤波）为代表的滤波方法，一种是以图优化为代表的非线性优化（BA）方法。主流方法还是图优化。
滤波方法的优缺点：
（1）优点：在资源受限、待估计量比较简单的情况下，EKF比较有效，经常用在激光slam中。
（2）缺点：存储量和状态量是平方增长关系（存储的是协方差矩阵）。不适合大型场景。现阶段，基于视觉的slam中特征点数据大，滤波方法效率非常低。
图优化一般分为两个任务：构建图。机器人位姿作为顶点，位姿间的关系作为边；优化图。调整机器人的位姿来尽量满足边的约束，使得误差最小。

BA优化

Bundle Adjustment : 从视觉重建中提炼出最优的3D模型和相机参数（内参和外参），好似每一个特征点都会反射几束光线，当把相机位姿和特征点位置做出最优的调整后，这些光线都收束到相机相机光心。也就是根据相机的投影模型构造构造代价函数，利用非线性优化（比如高斯牛顿或列文伯格马夸而尔特）来求最优解，利用雅克比矩阵的稀疏性解增量方程，得到相机位姿和特征点3D位置的最优解。
(通俗解释)流程：先按照正常的步骤计算出3D点的坐标；然后重投影（按理说一个正向计算，一个反向计算，误差是0才对），但是由于各种原因（不清楚具体是啥）存在了重投影（像素坐标）误差。因此需要求MIN的误差。求的过程中需要不断调整3D点位置与相机外参，因此求出MIN误差的时候，也获取了最优的3D点位置与相机姿态。

如何对匹配好的点做进一步的处理，更好保证匹配效果

暴力匹配是，确定匹配最小距离，汉明距离要小于最小距离的两倍才是正确的匹配。（工程经验，当描述子的距离大于最小距离的两倍时，认为匹配有误）。
使用KNN-matching算法，令K=2。则每个match得到两个最接近的descriptor，然后计算最接近距离和次接近距离之间的比值，当比值小于阈值时，才作为最终match。
RANSAC（随机抽样一致)，利用RANSAC找到最好的对极约束D
1.从匹配好的点中找八对点，然后计算其他匹配点到极线的距离，当距离小于内点时看作内点。
2.当内点的数目占95或者到达迭代次数k时把当前的本质矩阵作为最好的模型。
3.以当前的本质矩阵求解出相机的运动。（使用RANSAC找到最佳单应性矩阵。由于这个函数使用的特征点同时包含正确和错误匹配点，因此计算的单应性矩阵依赖于二次投影的准确性）

描述PnP

Perspective-n-Points, PnP(P3P)提供了一种解决方案，它是一种由3D-2D的位姿求解方式，即需要已知匹配的3D点和图像2D点。目前遇到的场景主要有两个，其一是求解相机相对于某2维图像/3维物体的位姿；其二就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。给出初始位姿之后就可以利用三角测量得到点的3D位置，以后就可以利用PnP求导了，工程中一般将PnP和ICP放到一个优化框架下优化，因为他们只差一个内参矩阵。

如何选取关键帧

关键帧选取的指标主要有：

（1）跟踪质量（主要根据跟踪过程中搜索到的点数和搜索的点数比例）/共视特征点

（2）距离最近关键帧的距离是否足够远（空间）/运动

（3）距离上一关键帧的帧数是否足够多（时间）

单目视觉slam中尺寸漂移是怎么产生的

单目相机根据一张图片无法得出一张图片中物体的实际大小，同理也就无法得出运动的尺度大小，这是产生尺度漂移的根源。而在优化过程中，单目相机使用对极几何中的三角测量原理，而三角测量中，极小的角度误差在累积之后深度不确定都会变得很大，从而无法保证尺度一致性。
单目SLAM的尺度漂移，简单来说就是单目获得的单张图片无法知道拍摄物体到相机的距离，必须通过对极几何求的两张图片的位姿，然后三角化出来尺度信息。而这些都依赖的是两帧图像之间的逆深度求解。
任选两张图（设为第0帧和第1帧），先提取关键点并匹配，再根据对极几何求本质矩阵E，再从E中恢复R,t。这个在单目VO里属于初始化的部分，其中得到的t是个单位向量，因为任意缩放t，极线约束都成立。你可以对t进行任意缩放。缩放完后，设第0帧的位置为相机坐标系原点，则第1帧的位置就可以由R, t得到。虽然这个t不是真值，但是可以固定下来。然后根据两帧的位置，可以对图像帧0和1中匹配的三维点进行三角化。（多点获取本质矩阵；恢复R,t；并根据R,t三角化计算出伪深度；）。
对之后的图像帧，就不再是利用对极几何求R,t了。依旧是先提取关键点再匹配哈，但是这次匹配的上一帧的特征中，有些是已经被三角化过的，因此可以像之前答主说的用运动模型加BA求该图像帧与上一帧的位置关系，也可以用PnP求R,t。求出来R,t之后，再三角化该帧与上一帧的没被三角化过的匹配点。
我们可以发现在尺度漂移就是出现在第二步。因为对于新的图像帧，都是先计算其R,t，再利用该R,t三角化与之前图像帧匹配的点。问题就是，如果第一步算的R,t有误差，那么第二步三角化的三维点的深度就存在误差，也就是说其深度会存在一个缩放。对于新来的每一帧，都会有这样的问题。

SLAM中的绑架问题

绑架问题就是重定位，是指机器人在缺少之前位置信息的情况下，如何去确定当前位姿。例如当机器人被安置在一个已经构建好地图的环境中，但是并不知道它在地图中的相对位置，或者在移动过程中，由于传感器的暂时性功能故障或相机的快速移动，都导致机器人先前的位置信息的丢失，在这种情况下如何重新确定自己的位置。

描述特征点法和直接法的优缺点

特征点法
优点：
（1）精确，直接法属于强假设
（2）运动过大时，只要匹配点在像素内，则不太会引起误匹配，鲁棒性好
缺点：
（1）关键点提取、描述子、匹配耗时长
（2）特征点丢失场景无法使用
（3）只能构建稀疏地图
直接法
优点：
（1）省去计算特征点、描述子时间
（2）可以用在特征缺失的场合（比如白墙）
（3）可以构建半稠密乃至稠密地图
缺点：
（1）易受光照和模糊影响
（2）运动必须微小，要求相机运动较慢或采样频率较高（可以用图像金字塔改善）
（3）非凸性；单个像素没有区分度

特征点法和直接法的BA有何不同

误差函数不同。特征点法是重投影误差，直接法是光度误差
雅克比矩阵不同

光流和直接法有何不同：

光流法还是需要提取图像中有代表性的点，图像的特征点，只是不需要再计算描述子了，直接通过灰度假设计算特征点像素的运动，然后通过PNP、对极几何、ICP等方法求解相机的运动；而直接法直接通过灰度假设计算得到相机之间的运动，任何图像的点都可以，不需要提取有代表性的点。
光流仅估计了像素间的平移，但没有用相机结构,没有考虑相机的旋转和图像缩放,边界点追踪效果差

特征匹配（稀疏）和稠密匹配区别

特征匹配：
（1）速度快，效率高，可以到亚像素级别，精度高
（2）匹配元素为物体的几何特征，对照明变化不敏感
稠密匹配
（1）速度慢，效率低
（2）对无纹理区域匹配效果不理想，对光强条件敏感

EKF和BA的区别：

都是基于贝叶斯估计得到状态的最大后验估计。
EKF假设了马尔科夫性，认为k时刻的状态只与k-1时刻有关。非线性优化使用所有的历史数据，做全体的SLAM
EKF做了线性化处理，在固定点处用一阶泰勒展开式近似整个函数，但在工作点较远处不一定成立。非线性优化每迭代一次，状态估计发生改变，我们会重新对新的估计点做泰勒展开，可以把EKF看做只有一次迭代的BA。

单目，双目，深度相机对比：

单目：成本低，搭建简单，单目相机有尺度不确定性，需要专门初始化
双目：不需要专门初始化，能够计算深度，基线距离越大，测量距离越远，可以用于室内和室外，标定较为复杂，视差计算比较消耗资源
深度：测量范围窄，噪声大，易受日光干扰，无法测量透射材料，主要用于室内。

为什么SLAM中常用L-M？

G-N中的H矩阵可能为奇异矩阵或者病态矩阵（残差函数在局部不像一个二次函数，泰勒展开不是很准确），导致算法不收敛。而且当步长较大时，也无法保证收敛性，所以采用L-M求解增量方程，但是它的收敛速度可能较慢。L-M法可以避免线性方程组的病态和奇异问题，可以得到更稳定、更准确的增量。

RANSAC算法

RANSAC算法的基本假设是样本中包含正确数据(inliers，可以被模型描述的数据)，也包含异常数据(outliers，偏离正常范围很远、无法适应数学模型的数据)，即数据集中含有噪声。这些异常数据可能是由于错误的测量、错误的假设、错误的计算等产生的。同时RANSAC也假设，给定一组正确的数据，存在可以计算出符合这些数据的模型参数的方法。RANSAC算法可以在数据存在异常时得到正确的模型。
优缺点：
RANSAC算法的优点是能鲁棒的估计模型参数。例如，他能从包含大量局外点的数据集中估计出高精度的参数。缺点是
1.它计算参数的迭代次数没有上限，如果设置迭代次数的上限，得到的结果可能不是最优的结果，甚至可能得到错误的结果。
2.RANSAC只有一定的概率得到的可信的模型，概率与迭代次数成正比。
3.另一个缺点是它要求设置跟问题相关的阈值，RANSAC只能从特定的数据集中估计出一个模型，如果存在两个（或多个）模型，RANSAC不能找到别的模型。

如何优化重投影误差？采用什么方法求解？如果误匹配的点重投影之后误差很大，如何解决它对整个优化问题的影响？

图优化模型，将路标点和相机位姿作为两个节点，观测模型作为边，同时优化两个变量
SLAM中常用L-M求解，如果误匹配误差很大可以考虑用核函数（Huber），设置鲁棒和函数

激光雷达运动畸变

产生原因：在机器人运动过程中，每个激光点都在不同的基准位姿上产生（就是在不同的时刻发出激光时机器人的位置是不同的（由于机器人自身的运动），这也是运动畸变产生的根源所在。当激光雷达扫描的频率比较低的时候，机器人运动带来的激光帧的运动误差是不能被忽略的。
去除畸变的原理：去除激光雷达运动畸变的原理是把一帧激光雷达数据的每个激光点对应的激光雷达坐标转换到不同时刻的机器人里程计上（近似对应的里程计的位置，达到尽可能去除畸变的目的）
去除运动畸变的方法：
1.使用ICP算法：ICP算法的缺点：没有考虑激光的运动畸变，因此得到的激光数据是错误的（计算的时候假设是正确的），从而造成点云匹配发生错误，通过不断的迭代，虽然能够收敛，能够求解，但是存在误差。
2.VICP算法考虑了激光的运动畸变，也就是考虑了机器人的运动和速度的存在。ICP算法的变种，考虑了机器人的运动，匀速运动（假设是匀速运动），进行匹配的同时估计机器人的速度。VICP 方法的缺点：低频率情况下（5Hz）,匀速运动假设不成立，数据预处理和状态估计过程耦合
3.我们尽可能的反应运动情况，实现预处理和状态估计的解耦，我们提出了传感器辅助的方法。使用惯性测量单元IMU（直接测量获得角速度和线速度，但对于机器人位移和角度需要积分）或者轮式里程计（直接测量机器人的位移和角度）。

如何求解Ax=b的问题

A是满秩的方阵可以直接求逆
1、三角分解（LU分解）
2、LDLT分解与LLT分解（Cholesky分解）
3、QR分解
4、奇异值分解（SVD分解）
转变成||Ax-b||2利用最小二乘迭代求解。

SIFT和SUFT的区别

构建图像金字塔，SIFT特征利用不同尺寸的图像与高斯差分滤波器卷积；SURF特征利用原图片与不同尺寸的方框滤波器卷积。
特征描述子，SIFT特征有4×4×8=128维描述子，SURF特征有4×4×4=64维描述子
特征点检测方法，SIFT特征先进行非极大抑制，再去除低对比度的点，再通过Hessian矩阵去除边缘响应过大的点；SURF特征先利用Hessian矩阵确定候选点，然后进行非极大抑制
特征点主方向，SIFT特征在正方形区域内统计梯度幅值的直方图，直方图最大值对应主方向，可以有多个主方向；SURF特征在圆形区域内计算各个扇形范围内x、y方向的haar小波响应，模最大的扇形方向作为主方向

关于imu预积分

为什么进行imu积分
IMU的频率远高于Camera，而IMU获得的是每一时刻的加速度和角速度，我们通过积分获得两帧之间的由IMU测出的位移和旋转变换。传统惯性导航的递推算法，是在已知上一时刻的IMU状态量（姿态和速度、位移）情况下，利用IMU测量得到的线加速度和角速度，做积分运算得到当前时刻的状态量。
为什么要进行预积分
在基于非线性优化的VIO中，各个节点的状态量都是估计值（此处还包括Ba和Bg）。当这些状态量在优化算法中，会不断调整，每次调整都需要在它们之间重新积分，传递IMU测量值。因此提出了IMU预积分，希望对IMU的相对测量进行处理，使它与绝对位姿解耦，或者只要线性运算就可以进行矫正，从而避免绝对位姿被优化时进行重复积分。

传感器时间同步

因为时钟源都有钟漂，而且每个时钟源钟漂不同，所以即使把各个传感器时间戳在初始时刻对齐，运行一段时间之后，之前对齐的结果仍会偏离。解决这个问题的办法就是在硬件上把时钟源统一，常见的做法是做一个脉冲发生器，所有传感器都被这个脉冲触发，每次触发都校正一次自己的时钟，这样就可以消除时钟源的累计误差。

scan-context介绍
在这里插入图片描述

Scan Context 应用于基于3D点云的重定位和场景识别，主要思想是将场景3维信息压缩，将笛卡尔坐标系的信息转换到极坐标系下计算。优势是高效利用场景点云分布特征，引入"旋转不变性"描述子，快速搜索。
在这里插入图片描述

给定一帧点云，划分成20个环，每个环分成60等份，一共1200个格了
每个格子存里面点的最大高度值(z值)，这样一帧点云就用一个二维图像表示了，想象成一个带高度的俯视图，或者地形图，记为scan context
scan context进一步计算列的均值，得到一个1x60的向量，记为rina kev;计算行的均值，得到一个20x1的向量，记为sector kev
用ring key构造kd-tree，并且执行knn搜索
对于候选匹配scan context，首先要左右循环偏移一下，对齐，实际会用sector key去对齐，得到一个偏移量
对候选匹配scan context，施加偏移量，然后作比较

鲁棒核函数与RANSAC方法

我们构造3D-3D、3D-2D、2D-2D匹配，然后据此去估计相机的运动。完美估计需要完美的匹配，但实际的匹配中往往存在很多错误。如何消除或者降低错误匹配的影响呢？一种方法是选择那些正确的匹配进行估计（RANSAC），另一种是降低那些错误匹配的权重（鲁棒核函数）。
上述RANSAC方法进行模型估计，实际分了俩步，首先选出局内点，然后进行优化。鲁棒核函数只需要一步，直接优化求解模型参数，通过降低外点的权重，来降低数据错误的影响。鲁棒核函数能够保证每条边的误差不会大的没边而掩盖其他的边。具体方式是把原先误差的二范数度量替换成一个没有增长那么快的函数，同时保证自己光滑的性质。