文章目录
1.IMU加速计和陀螺仪的误差类型
(1)确定性误差
bias: 即零偏
scale: 实际数值与传感器输出值之间的比值
(2)随机误差
高斯白噪声: IMU 数据连续时间上受到一个均值为 0,方差为 σ,各时刻之间相互独立的高斯噪声
bias随机游走: 表示离散时间下bias随时间的变化
2.IMU误差标定
确定性误差标定:六面法标定加速计和陀螺仪零偏bias和scale
加速计通过将加速度计的3个轴分别朝上或者朝下水平放置一段时间,采集6个面的数据完成标定。
陀螺仪则是将各个轴顺时针和逆时针旋转。
随机误差标定:Allan方差法,标定工具有kalibr和港科大的imu_utils
3.什么是预积分
IMU的加速计和陀螺仪获得的是body运动的加速度和角速度信息,在进行位姿计算和优化时是对相邻两个时刻的位姿进行优化,为了避免重复积分和节省计算资源,预先将两个相邻时刻的加速度和角速度积分成位移、速度和旋转角度等信息,在进行位姿估计时直接使用。
4.非线性优化方法求解最小二乘
非线性优化方法即通过将损失函数进行一阶或二阶泰勒展开,寻找一个下降方向和下降的步长,使损失函数随着自变量x的迭代不断减小,最终收敛为最小值。其中称损失函数的一阶导数矩阵为雅格比矩阵,二阶导数矩阵为Hessian矩阵。
(1)类别: 最速下降法,牛顿法,高斯牛顿法,L-M法
(2)一句话简介每种求解方法
最速下降法: 沿梯度的负方向为下降方向,下降快但收敛慢,会在最优值附近震荡
牛顿法: 下降步长使用了二阶导矩阵,适合在最优值附近使用,收敛稳定但二阶导矩阵计算复杂。
高斯牛顿法: 在牛顿法的基础上进行改进,不再直接计算损失函数f(x)的二阶倒数,而是将f(x)求一阶导展开之后求2范数,然后对新式子再继续求一阶导,使用一阶导的雅格比矩阵近似计算出Hessian矩阵
L-M法: 在高斯牛顿法的基础上进行改进,引入了阻尼系数,为了限制步长的大小,在最优值附近可以准确收敛到最优解。
5.鲁棒核函数
在优化损失函数时,如果某些数据为错误数据即外点(outlier),会导致误差变得很大,在进行迭代求解时为了更快的让误差下降会设置一个比较大的梯度,但是这样就会使其他正确数据得到的结果受到影响。
因此,为了避免外点带来的这种影响,在损失函数外层又套用了一层核函数,当误差下降的梯度过大时会限制梯度的大小(表现在损失函数曲线的斜率)
6.IMU运动模型的离散积分
- 欧拉法:两个相邻时刻k到k+1的位姿是用第k时刻的测量值a,ω来计算。
如: v k + 1 = v k + a k ⋅ Δ t v_{k+1} = v_k + a_k \cdot \Delta t vk+1=vk+ak⋅Δt, R k + 1 = R k ⋅ ( w k ⋅ Δ t ) R_{k+1} = R_k \cdot (w_k \cdot \Delta t) Rk+1=Rk⋅(wk⋅Δt) - 中值法:相邻两个时刻k到k+1的位姿是用两个时刻测量值a,ω的平均值来计算
如: v k + 1 = v k + a k + a k + 1 2 ⋅ Δ t v_{k+1} = v_k + \frac{a_k + a_{k+1}}{2} \cdot \Delta t vk+1=vk+2ak+ak+1⋅Δt, R k + 1 = R k ⋅ ( w k + w k + 1 2 ⋅ Δ t ) R_{k+1} = R_k \cdot (\frac{w_k + w_{k+1}}{2} \cdot \Delta t) Rk+1=Rk⋅(2wk+wk+1⋅Δt)
7.在滑动窗口理论中的一些问题
参考链接:
https://blog.csdn.net/weixin_41394379/article/details/89706298
https://blog.csdn.net/jdy_lyy/article/details/119259128
https://blog.csdn.net/heyijia0327/article/details/52822104
7.1.为什么要使用滑动窗口算法
-
随着 VSLAM 系统不断往新环境探索,就会有新的相机姿态以及看到新的环境特征,最小二乘残差就会越来越多,信息矩阵越来越大,计算量将不断增加。
-
为了保持优化变量的个数在一定范围内,需要使用滑动窗口算法动态增加或移除优化变量。
7.2.基于滑动窗口算法和基于局部地图优化的区别
区别在与对老数据的处理方式不同。
-
局部地图根据共视关系确定优化变量(位姿,地图点);其次,通过在优化中固定一些变量(如orb-slam 固定了和局部地图存在共视但不和当前关键帧直接相连的关键帧的位姿),提供先前信息的约束。
-
滑动窗口通过相对固定的窗口大小对待优化参数的数量进行限制,平衡计算量;通过边缘化得到的先验在优化中保留一些旧的状态的信息。
基于局部的优化不会出现线性化点不一致的问题在于局部地图优化不会进行边缘化,因为共视图可以链接到比较久远的环境信息,当共视图中的某个关键帧与当前帧共视信息比较少时说明该帧和当前帧已经相距很远了,这时优化时直接丢弃该关键帧的信息也不会对当前帧的位姿优化造成影响.
7.3.什么是滑动窗口算法中的fill-in问题
原本关于待边缘化变量条件独立的其他变量由于边缘化变量的固定而具有相关性,在因子图中表现为两两之间建立约束即fill-in,这种现象会导致信息矩阵变得稠密。
7.4.信息矩阵在局部地图优化问题中是怎么传递和更新的
在滑动窗口算法中,老的状态被进行边缘化处理,其位姿和观测等信息被固定下来不再参与优化,同时被作为一种先验信息对新的状态进行约束,在边缘化过程中新窗口中关键帧的信息矩阵随之进行了同步和更新,那基于局部地图优化的算法信息矩阵是怎么更新和传递的?
在基于局部地图优化的SLAM算法中,信息矩阵通过观测误差函数的协方差矩阵求逆获得
7.5.什么是边缘化处理marginalization
边缘化处理是滑动窗口算法的实现方法。
对于滑窗外的状态(旧的状态变量信息),我们不去进行优化,但也不能直接丢掉,这样会破坏原有的约束关系,损失约束信息。采用边缘化的技巧,将约束信息转化为待优化变量的先验分布
7.6.舒尔补是什么
舒尔补是在对优化变量进行边缘化处理时使用的一种矩阵块处理方法,主要用于求解加速求解信息矩阵的逆和分解多元高斯分布的先验概率和条件概率
-
(1)进行矩阵求逆
-
(2)分解多元高斯分布
7.7.对信息矩阵进行边缘化会出现什么问题或者隐患
-
首先,边缘化会将被边缘化掉的变量所带有的信息转化为先验增加到H矩阵中(Fill-in),这样会破坏BA的稀疏结构,使得信息矩阵变得稠密;
-
滑动窗口算法优化的时候,信息矩阵变成了先验信息和新残差两部分,且这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化,从而在求解时引入错误信息。
7.8.对于这些隐患的改进方法
-
FEJ算法:不同残差对同一个状态求雅克比时,线性化点必须一致。这样就能避免零空间退化而使得不可观变量变得可观。
-
在对观测点进行边缘化处理时,尽量选择不同时被其他帧观测到的路标点
7.9.自由度可观和不可观的概念
在SLAM系统中对状态变量估计时,如果说针对某个状态变量具有多个解或则说某条曲线上的点均满足状态方程,就说当前状态变量是不可观的,反之如果具有唯一解就是可观的。
对于单目SLAM系统来说,其尺度无法被直接估计出来,因此单目SLAM的状态变量7自由度不可观
单目 + IMU 系统是4自由度不可观:yaw角 + 3自由度位置不可观。roll 和 pitch 由于重力的存在而可观,尺度因子由于加速度计的存在而可观
7.10.怎么理解边缘化过程中一致性consistency
滑动窗口算法优化的时候,信息矩阵变成了先验信息和新残差两部分,且这两部分计算雅克比时的线性化点不同。
7.11.FEJ(First Estimated Jacobian)算法的原理
执行边缘化过程中,我们需要不断迭代计算H矩阵和残差b,而迭代过程中,状态变量会被不断更新,计算边缘化相关的雅克比时需要注意固定线性化点。也就是计算雅克比时求导变量的值要固定,而不是用每次迭代更新以后的x去求雅克比,这就是FEJ(First Estimate Jacobians)。也被称为系统的一致性问题。
比如: 计算r_27对位姿ξ_27的雅格比时,ξ_2的线性化点必须和r_12对ξ_2求导时的线性化点一致。
8.在VIO算法对位姿进行联合优化时为什么要优化重力的方向
加速计获得的是绝对加速度(由受到的合力获得)减去重力加速度(由重力获得),即当加速计静止不动时读数是一个反向重力加速度值(0.0, 0.0, 9.8)。
如果不对重力方向进行优化,当姿态不准确,重力投影误差将对速度和位置积分产生严重影响。
9.为什么要进行预积分
- 减少误差的传播,在积分过程中偏置等信息需要跟着计算,随着重复积分会造成误差的累计,预积分只计算相邻两个图像帧之间的相对运动,假定图像之间的偏置保持不变,作为变量和位姿进行同一优化。
- 在SLAM系统中激光或相机的频率要远小于IMU频率的,在后端优化时会迭代很多次,每次调整位姿都需要重新对两帧之间的IMU数据进行重复积分,会占用比较大的计算资源。
10.IMU给纯视觉SLAM系统带来的提升
- 重力的存在将使整个系统对绝对姿态(指相对水平地理坐标系的俯仰角和横滚角,不包括真航向)可观。
- 通过纯视觉得到的位姿使得IMU的bias可观,从而可以在优化中被有效估计
- 给单目SLAM系统带来真实的尺度信息
- 应对纯旋转:在纯旋转的情况下,VO位姿解算会出现奇异,VIO可以利用IMU的陀螺仪(角速度计)来估计纯旋转运动。
- 应对短时间图像特征缺失:在出现图像过曝、图像过暗、环境纹理不足时,VO会无法工作,而VIO能在VO失效的情况下利用IMU积分来进行运动估计,能够应对短时间的视觉特征缺失(IMU积分越久累积误差会越大),比VO具有更高的鲁棒性。
11.比力
比力可以简单理解为加速度,在数值上等同于物体的绝对加速度(由合力计算得到)减去重力加速度(由重力得到)。由此,加速计得到的测量包含了反向加速度的信息,即当加速计静止时,加速计的读数为(0.0,0.0,-9.8)。
12.单目SLAM中的纯旋转问题
当特征点共面,或者相机发生纯旋转的时候,基础矩阵的自由度下降,这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声,这时候如果我们继续使用八点法求解基础矩阵,基础矩阵多余出来的自由度将会主要由噪声决定。
13.IMU预积分为什么可以避免重复传播
IMU预积分与通常积分相比,解决了初始状态更新时以及加速度计、陀螺仪偏差变化时所导致的积分重新计算问题。
其主要思路是计算两个关键帧之间的状态增量,当初始状态变化,就在原来增量基础上乘以初始状态的变化量;
当偏差变化时,则通过求取预积分量关于偏差的雅各比,实现预积分的一阶线性近似更新。
14.F,E和H矩阵各有几个自由度,为什么发生了自由度退化现象
-
基础矩阵(Fundmental Matrix)具有7个自由度
首先F矩阵是三阶的,最高自由度为9,然后其满足尺度等价性,因此自由度减一;再其次其满足不可逆矩阵的性质,行列式为零的约束等式,因此自由度再减一,因此自由度为7 -
本质矩阵(Essential Matrix)具有5个自由度
旋转和平移一共6个自由度,由于对极约束的原因,本质矩阵是具有尺度等价性的,所以自由度减1,因此本质矩阵共5个自由度。 -
单应矩阵(Homography Matrix)具有8个自由度
单应性矩阵也是一个3x3的矩阵,其具有尺度等价性,因此自由度为8
15.LOAM和LeGO-LOAM的区别
- 地面点只匹配面点,分割点(上面的点)只匹配角点
- 将分组聚类后达不到30个点的全部剔除
- 同一聚类标签的点才能在里程计中匹配
- 两步LM法可减少35%的计算时间
- 用地面特征点得到的Z轴,roll角,pitch角的约束来求分割点的位姿变换
16.简述MSCKF算法
16.1.MSCKF算法简介
MSCKF的目标是解决EKF-SLAM的维数爆炸问题。
传统EKF-SLAM将特征点加入到状态向量中与IMU状态一起估计,当环境很大时,特征点会非常多,状态向量维数会变得非常大。MSCKF不是将特征点加入到状态向量,而是将不同时刻的相机位姿(位置 P和姿态四元数Q)加入到状态向量,特征点会被多个相机看到,从而在多个相机状态(Multi-State)之间形成几何约束(Constraint),进而利用几何约束构建观测模型对EKF进行update。
由于相机位姿的个数会远小于特征点的个数,MSCKF状态向量的维度相较EKF-SLAM大大降低,历史的相机状态会不断移除,只维持固定个数的的相机位姿(Sliding Window),从而对MSCKF后端的计算量进行限定。
MSCKF的本质上是一个EKF算法,在预测阶段使用IMU的加速度和角速度信息建立运动模型,在更新阶段使用多个相机对同一特征点的约束关系优化相机位姿和IMU的相关信息(因此是紧耦合)。
16.2 MSCKF算法中的滑动窗口
在滑动窗口中只维持一定数量的待优化帧对应的状态变量,每次有新帧到来则删除最老的帧。
17.S-MSCKF算法简介
S-MSCKF是一个双目版的MSCKF算法,可以简单的分为前端和后端两个部分。
前端主要完成视觉部分的跟踪,包括视觉特征提取,跟踪匹配等操作。视觉特征提取选择fast特征,然后基于LK光流进行跟踪匹配。在前后帧进行跟踪匹配时,会首先使用IMU角速度积分得到的旋转作为计算帧间匹配的初值,然后假设所有特征点都在深度为1的normalize平面,所以可以基于RANSAC迭代直接计算Homography。
后端进行的任务主要有重力和陀螺仪bias的初始化以及EKF状态更新,在初始化时重力和陀螺仪bias时需要使系统处于静止状态,这样加速度的均值即重力值,陀螺仪均值即为陀螺仪bias。
18.为什么单目纯旋转无法完成初始化
初始化的目的是建立三维的空间点和地图并为之后的计算提供初始值
从E分解到R; t的过程中,如果相机发生的是纯旋转,导致t为零,那么,得到的E也将为零,这将导致我们无从求解R。
不过,此时我们可以依靠H求取旋转,但仅有旋转时,我们无法用三角测量估计特征点的空间位置(这将在下文提到),于是,另一个结论是,单目初始化不能只有纯旋转,必须要有一定程度的平移。如果没有平移,单目将无法初始化。