SVO、vinsmono、 OKVIS系统比较

几个经典视觉slam系统的比较

SVO

• 高翔链接：https://www.zhihu.com/question/39904950/answer/138644975
• 处理的各个线程:
  tracking部分-frame to frame 、frame to map

0. 金字塔的处理。这一步估计是从金字塔的顶层开始，把上一层的结果作为下一层估计的初始值，最后迭代到底层的。顶层的分辨率最小，所以这是一个由粗到精的过程（Coarse-to-Fine），使得在运动较大时也能有较好的结果
1. SVO自己实现了高斯——牛顿法的迭代下降，并且比较取巧地使用了一种反向的求导方式：即雅可比在k-1帧图像上进行估计，而不在k帧上估计。这样做法的好处是在迭代过程中只需计算一次雅可比，其余部分只需更新残差即可（即G-N等式右侧的）。这能够节省一定程度的计算量。另一个好处是，我们能够保证k-1帧的像素具有梯度，但没法保证在k帧上的投影也具有梯度，所以这样做至少能保证像素点的梯度是明显的。
2. 根据更精确的投影位置，进行位姿与地图点的优化。这一步类似于Bundle Adjustment，但SVO实现中，是把Pose和Point两个问题拆开优化的，速度更快。
   地图点初次被观测到的图像与当前帧的图像进行比对时，不能直接对两个图像块求差，而需要计算一个仿射变换（Affine Warp）。这是因为初次观测和当前帧的位移较远，并且可能存在旋转，所以不能单纯地假设图像块不变。

• mapping部分：
  Mapping部分主要是计算特征点的深度。
  单目VO中，刚刚从图像中提取的热乎的关键点是没有深度的，需要等相机位移之后再通过三角化，再估计这些点的深度。这些尚未具备有效深度信息的点，不妨称之为种子点（或候选点）。然而，三角化的成功与否（以及精度），取决于相机之间的平移量和视线的夹角，所以我们通常要维护种子点的深度分布，而不是单纯的一个深度值。
  实际可以操作的只有高斯分布一种——高斯只要在计算机里存均值和协方差即可。在逆深度[8]流行起来之后，用逆深度的高斯分布成了SLAM中的标配。然而SVO却使用了一种高斯——均匀混合分布的逆深度（由四个参数描述），推导并实现了它的更新方式，称为Depth Filter。它的工作逻辑大概是这样的：如果进来一个关键帧，就提取关键帧上的新特征点，作为种子点放进一个种子队列中。如果进来一个普通帧，就用普通帧的信息，更新所有种子点的概率分布。如果某个种子点的深度分布已经收敛，就把它放到地图中，供追踪线程使用。

单目初始化既要有旋转，也要有平移；E=t^R,当纯旋转，t=0时，E为零，无法恢复出R；而当t不为零，R为零时，三角化求深度的时候s1x1=s2Rx2+t,也就没有办法求取深度

vinsmono（四自由度，平移和yaw(z方向的角速度未知)）

视觉的sfm和惯性的预积分对齐（主要就是将相机的sfm和IMU的预积分进行对齐，相机只有位置、姿态的6维状态，而IMU15状态，对齐的量是什么呢？
回答是利用相机估计出来的位姿相对量和imu的相对量进行对齐。深度优化量在视觉重投影中进行优化，一些零偏量在IMU误差中进行优化，
但是因为共同的位姿量的存在，所以虽然不在某一项优化函数里，但是其实还是受到影响的）

单目的初始化很鲁棒，视惯的紧耦合，有效初始化相机与imu之间的外参、陀螺仪的零偏、尺度、速度、重力加速度；（由于估计了速度，就可以在动态场景下进行初始化，更适合系统应用场景）
由于利用相机求解的位姿和IMU预积分结合外参有一个约束（这里应用旋转约束），利用估计和量测信息可以构建优化函数，求解外参；求出来外参可以继续估计陀螺仪的零偏
利用平移约束，可以估计尺度、速度、重力加速度（同时对重力加速度进行细化）
对齐第零帧相机系下的重力加速度和世界系下的重力加速度，就可以对齐相机系和世界系。

建立关于IMU、视觉的残差函数
一个特色：基于优化的方法，通过滑动窗口的schur边缘化进行位姿维护。根据前一帧相机是否是关键帧，确定是否保留该帧留在窗口中，不是关键帧的话就丢弃该视觉信息；是关键帧就将最老的一帧边缘化掉，同时保留视觉信息在窗口中。（这里是实现的时候，优化函数分为四部分：1.所有的IMU残差2.窗口里的局部视觉重投影进行优化3.产生回环帧的部分）

误差来自滑动窗口和边缘化，主要体现在四个自由度(利用重力向量可以将第0帧相机系和世界坐标系的x、y平面也就是水平面进行对齐，从而感受到滚转角和偏航角,这两个角度是没有偏差的，只有yaw、x、y、z有误差)
重定位模块： 闭合回环检测、建立当前帧与回环的特征关联、将这些特征关联集成到前面的VIO模块以消除漂移如果当前窗口有多个回环帧的话，那么就一起加到回环的优化函数中，增加多个角度的约束

全局位姿图优化 global pose graph optimization（VIO模块使得位姿中的滚转和俯仰变得可观，只剩下了3自由度平移和偏航角（绕重力向量的旋转）不可观，因此这里的全局图优化只用来估计这四个自由度，也就是优化函数是关于这四个变量的函数，所有又称为4自由度全局位姿图优化）：位姿图包括所有关键帧的IMU位姿为顶点，约束为回环边和正常的递推下来的边。

论文实验中出现的实验说明：
VINS-Mono在滚转roll和俯仰pitch的估计中效果不如OKVIS，原因可能是这两个量是根据IMU的预积分得到的，预积分其实是为了节省计算量而采用的一种一阶近似手段，因此可能造成了误差存在。关闭了回环检测和重定位功能的VINS-Mono表现大打折扣，和OKVIS一样在四自由度（三自由度平移和yaw角）上有漂移，但是打开回环检测后效果提升就很显著了。移植到IOS移动端的程序与商用的Google tango比起来丝毫不落下风，尤其是在全局的误差消除上，甚至比tango还要好，这得益于4自由度的全局位姿图优化。

OKVIS:Keyframe-Based Visual-Inertial SLAM UsingNonlinear Optimization

initial :not dynamic /yaw points to the z 论文里说是强制固定位置和yaw角的
(freeze yawing around the gravity direction (world z-axis), as well as the position)
In the case where the oldest frame in the temporal
window (x c−ST ) is not a keyframe, we will drop all its
landmark measurements and then marginalize it out together
with the oldest speed and bias states.
根据确定的滑动窗口大小（大小即是滑动窗口内图像关键帧的多少），滑动窗口内的关键帧是滞后于图像当前帧的，okvis论文以三帧为例，vinsmono直接是1，因此如果图像帧已经到kc，但实际上还在判断kc-3/kc-1帧是不是关键帧，要不要边缘化。
okvis：是关键帧：就把窗口里最旧的关键帧边缘化，且因为看到相同地图点的两帧已经具有相对位姿信息，就可以把之前帧里看到但在当前帧没看到的地图点和最旧的关键帧都踢掉
不是关键帧，把该帧、地图点通通都边缘化掉
而且在边缘化中okvis是把估计的变量拆分为两部分，机器人的位置、姿态信息由是否是关键帧在滑动窗口中慢慢被边缘化，而速度、加速度计、陀螺仪的偏移量与该帧是不是关键帧没有关系，马上被边缘化掉
//different from VINSmono