msckf回环检测相关论文1

先快速通读一遍，一脸懵
开始精读

abstract

本文主要工作：设计适用于提前建图的手机3D定位算法；
实现方法：使用C-SKF（Cholesky-Schmidt-Kalman filter ）和sC-SKF来减少内存需求

1.introduction

在室内机器人和人需要精确的定位，定位精读的提升可以依靠建图，像msckf或PTAM就是这样，但是作者说它们not consistent（还没搞明白这个consistent到底什么意思）（in this work, we refer to a consistent estimator as having zero-mean error with covariance equal to or larger than the estimation error’s true covariance）
一种可能的将地图的不确定性和估计状态结合起来的方法是SKF(Schmidt Kalman filter)，它所需要的处理资源随着地图尺寸增加而线性增长，但是存储地图的协方差是特征数的平方倍。
为解决上面的问题，作者提出了C-SKF，它的内存需求随着地图尺寸线性增长，但是性能和SKF相当；为了减少计算资源需求，作者提出a consistent relaxation of the C-SKF, the sub-map (s)C-SKF，它将权衡定位精度和处理速度。
本文的主要贡献：

• 提出Cholesky-Schmidt-Kalman filter(C-SKF)
• 提出sub-map(s) C-SKF
• 验证有效性

2.MAP-BASED LOCALIZATION ALGORITHM OVERVIEW

我们的目标是通过IMU和视觉测量以及预先构建的地图得到实时的移动设备的3D位置和姿态。为此我们在处理离线地图后需要得到如下信息

• (子)地图的Hessian的Cholesky因子
• 地图特征的3D位置以及它们索引图像的向量

前者用来代表地图的不确定性，后者用来识别地图特征以及用特征估计位姿来更新移动设备的位姿。

算法主要包括图像处理和估计器。估计器的核心是msckf，它处理IMU数据并且传播设备的状态和协方差估计。当图像特征检测成功时，msckf的(s)C-SKF接受器会更新设备的状态方差和地图的协方差。同样的，当2D-3D特征匹配成功时，(s)C-SKF更新除了地图和他的协方差之外的估计量。

3. SYSTEM STATE AND MEASUREMENT MODELS

A. 设备状态

对于第k时间步，估计状态为

可以看成三部分，x_E表示当前IMU的状态，x_Ci表示之前N个IMU状态，x_τ表示已经建图的参考帧和global之间的转换。
其中X_E是设备的进化状态

其中
表示global坐标系到IMU当前帧坐标系之间的转换四元数，b_ak和b_gk分别表示加速计和里程计的偏差，^Gv_Ik和^Gp_Ik分别表示在global坐标系下IMU的速度和位置；

表示之前的IMU位姿，N是滑窗大小

表示设备global帧{G}到一个或者多个地图参考帧{M_i}之间4个自由度的变换，其中^Gp_Mi表示{M_i}在{G}中的位置，
表示两帧之间重力的旋转角，由于roll角和pitch角对于VINS是可以测量的，我们在global帧和地图参考帧将z轴对齐重力方向。

B. IMU测量模型

其中u_k表示线性加速测量值，g是非线性函数，w_k是零均值，方差已知的高斯噪声。

C. 局部特征测量模型

当设备在环境中穿梭时，它通过KLT光流法观测和跟踪到一些尚未建图的点特征，这些局部特征可以为状态向量中维持的N+1个IMU-相机姿态提供测量约束。
非线性和线性化的局部特征测量模型如下所示

其中z是测量值，h是投影相机测量模型，r是线性化的测量残差，x_R($\tilde{x}$_R)表示设备状态（误差状态），p_f($\tilde{p}$_f)表示局部特征状态（误差状态），H_R和H_f表示和设备状态和特征状态相关联的雅克比矩阵，n表示零均值，已知方差R的高斯白噪声。
我们通过投影r到H_f的左零空间，也就是等式(5)左乘U^T来边缘化局部特征

其中

新的测量残差r^o,雅克比矩阵H^o_R将被用于更新设备状态估计。

D. 已建图特征的测量模型

当设备检测到已经提前建图的点特征f时，可以得到如下几何关系

转换关系如下图所示：

{G}表示global坐标系，{M_i}表示已经建图的第i个参考帧坐标系，{I^M~i~_λ}表示已经建图的IMU坐标系，f表示检测的特征，{I_k}表示第k时刻IMU坐标系。
所有的C表示旋转矩阵，它可以由相关联的三个自由度的四元数转化而来，除了^G_MiC，它是表示重力旋转的那个角^MiΦ _G。应用相机投影转换，可以得到测量模型

其中$\pi$表示相机投影转换，n表示零均值，方差为R的高斯白噪声。
为了简化表示，我们忽略时间索引，用x_R表示x_k,用x_M表示组成所有已建图特征和IMU-相机姿态的的向量，得到如下关系：

这里H_R和H_M表示设备和地图的雅克比矩阵，它们都是稀疏的，因为测量等式只和当前帧和已建图的IMU-相机配对的位置和四元数，四个自由度的地图到global的转换，以及特征的位置有关。

4. ALGORITHM DESCRIPTION

接下来首先介绍的是SKF，然后介绍C-SKF以及sC-SKF，符号约定：
表示更新量（表示通过测量值辅助估计的量，较为准确），
表示传播量（表示通过运动学公式等估计的量，较为粗略），
例如

将被记为

A. 基础：Schmidt-Kalman filter

当前的传播系统协方差P和雅克比矩阵H如下：

EKF的状态和协方差更新等式如下：

其中

而SKF只会更新一部分的状态，而将状态向量元素中估计维持不变的卡尔曼增益设置为0（在我们的例子中，就是地图x_M）。

因此我们的状态更新如下，也就是将(14)带入(11)：

将(14)带入(12)，我们可以得到SKF的协方差更新公式

如果我们直接用EKF的更新协方差作为SKF协方差的函数，我们可以得到如下的形式

因为S^-1是正定矩阵，因此

是半正定矩阵。

因为SKF没有低估系统协方差，因此它是连续的；而且从(16)可以看成SKF协方差更新计算量是地图尺寸的一次方，因为只有设备的协方差和互相关项需要被更新，而H是稀疏的。
另外SKF要求存储地图的协方差矩阵P_MM，这是个稠密矩阵。我们发现1.5min的视觉和惯性数据生成的协方差地图要求1.2GB的存储空间，而Hessian 矩阵的稀疏Cholesky 因子只需要76MB的存储空间，因此我们接下来将介绍C-SKF（Cholesky-SKF）。

B. C-SKF device-map initialization

接下来我们将介绍估计map帧和global参考帧之间的4个自由度的转化，以及它们的协方差、相关系数等信息。

当移动设备第一次观测到2个或者更多提前建图的特征时，将通过2+1 pt RANSAC得到4个自由度的转换初始估计$\hat{x}$_τ，进一步我们可以得到当前的状态向量

其中x_R’表示(1)中除去x_τ的剩余部分，x的协方差矩阵为：

其中P_ττ是4个自由度转换矩阵的协方差矩阵，GG^T是地图的Hessian矩阵，G是Cholesky因子，通过将(8)中的测量模型进行线性化，我们可以得到相关的雅克比矩阵

当我们用(13)-(15)去更新x_R’和x_τ时，可以得到如下关系

通过应用(16)我们可以得到更新SKF的协方差矩阵

其中


J的定义如下

我们方法的一个关键地方是我们不更新协方差矩阵项P_RM，而是用像(26)那样的因子形式表示它，然后更新时更新它的因子Γ，这在接下来的C、D部分会有介绍，根据这种思路，我们有

这里我们不需要计算G的逆，所有涉及G的等式的更新都使用(27)这种形式去更新，这里面为了快速计算J^T要求G是一个稀疏矩阵。

C. C-SKF propagation and local-feature update

1. IMU-based Propagation

通过应用(4)中IMU的测量模型，设备状态将进行传播，地图状态保持不变

为了传播（或者叫预测）(28)中C-SKF的协方差矩阵，我们使用EKF中协方差传播公式，得到

其中Φ和Q分别是IMU相对于状态和噪声协方差的雅克比矩阵。

从(30)可以看到，设备传播协方差消耗计算量是很小的，互相关项只需要更改互相关因子Γ，
Γ^θ = ΦΓ，这个计算量是地图尺寸的一次方。

2. C-SKF Local-Feature Measurement Update

当一个局部特征跟踪测量可用时，我们可以得到如下的雅克比矩阵

正如(14)中所说的那样，我们将地图状态相关的卡尔曼增益设置为0，这样只更新设备状态，地图状态保持不变，如下所示：

首先有

展开就是

我们通过将(34)和(28)带入(16)，但是不估计$\overline{K}$_M，可以得到

从(33)-(36)我们可以看到，局部特征更新的复杂度是地图尺度的一次方，我们只需要对设备协方差应用一次标准的EKF更新，并且更新互相关因子Γ就可以了。

D. C-SKF map-based updates

当设备观测到提前建图的特征时，我们用(8)-(9)的测量模型应用(13)-(16)的公式，然后用因子化的互相关操作(28)中系统协方差，我们记

也就是

我们不精确计算$\overline{K}$_M，而是先计算J

因为G是三角矩阵，所以我们可以用back-solve的方法来计算J，然后将J带入(37)

接下来我们计算残差的协方差矩阵

状态更新（参考(15)）如下

最后我们用(16)和(39)更新协方差，为了避免计算$\overline{K}$_M，我们采用(36)一样的处理方式，我们可以得到更新互相关的因子图形式

在这里我们发现，不想传播和矩阵特征更新，地图特征更新的计算量并是不是严格的地图尺寸的一次方。主要的瓶颈在于(38)，计算J花费的计算资源是地图尺寸的一次方到平方，所以当地图尺寸增长时，在移动设备上可能不能够进行实时操作。所以我们在4-E和4-F部分提出了sub-map relaxation。它是通过将地图划分成独立的子地图来减小G的尺寸，以此来保证consistency。

E. Sub-map relaxation

我们将轨迹和相关联的特征划分成两个独立的集合。首先，对于IMU-相机位姿 ξ_i划分成两个集合[ ξ₁, ξ_N]和[ ξ_N+1, ξ_M]。讨论如何对子地图进行划分也是未来研究的工作。然后，我们将特征划分到这两个集合当中。

对于共同的特征，CM算法复制了这些特征到两个子地图当中，但是对于这些特征有一个非线性的约束。

这里面x_a是所有IMU-相机位姿，x_τ是两个子地图的4个自由度的转化向量，对于两个子地图共有的特征，这个约束保证了它们的物理位置相同。

最后所有的相机和IMU测量z_i,j和u_i,i+1将被分配到它们对应的子地图当中。

如果忽略共同特征的约束，我们可以得到两个独立的代价函数

这里面g和h是(4)和(5)定义的函数，下面将两个代价函数合起来并添加约束

我们通过CM算法来最小化代价函数。

F. Cholesky-Kalman-Schmidt with sub-maps(sC-SKF)

我们通过(45)保持估计的精确性，但是通过存储Cholesky因子放宽(44)中信息的限制，这样可以使得子地图保持独立性，同时维持连续性。

定理1：当忽略共同特征的约束时，子地图的协方差矩阵会更大或保持不变，也就是半正定的含义。

为了处理子地图的测量，我们可以这样表示系统协方差

其中Γ_i和G_i是第i个子地图的互相关因子和Cholesky因子。

当一个提前建图的特征被第一个子地图观测到时，测量的雅克比矩阵如下：

这里面H₁是第一个子地图的雅克比矩阵。我们通过类似(38)和(39)的方式计算J₁和$\overline{K}$：

残差协方差矩阵和状态更新如下：

最后我们使用和(42)类似的方法，使用相同的因子化作为单地图情况

通过应用子地图松弛，我们极大地减小了C-SKF的计算量。我们现在的计算瓶颈是计算J₁，但是通过增加子地图的数量来减小系统的尺寸从而减小计算J₁所需要的时间。但是需要注意到，增加子地图的数量会减小地图中的信息，从而导致设备位姿估计精度稍微下降。

5. 2D-3D Feature correspondence Pipeline

在应用基于地图的更新之前，我们先确认当前图像2d特征和提前建图的3D特征之间的关联。Fig 1中的格子B。我们考虑两种情况：

• 估计器没有设备到地图的4个自由度的预先估计
• 估计器有设备到地图的4个自由度的预先估计

A. Pose-less correspondence generation

• 1. VT 索引：我们对当前图像索引保存的VT，将返回已经见图的具有相似外观的5张图片
• 2. 特征匹配：我们在索引特征和返回图像当中使用2进制描述子
• 3. 外点投影：我们对VT返回的每张图片使用3+1 pt RANSAC。如果少于7个关联被保留，我们将VT返回作为外点；否则我们对剩下的内点使用 2+1 pt RANSAC ，如果少于13个关联被保留下来，我们将所有特征观测作为外点

B. Pose-assisted correspondence generation

对于少部分的操作当中，估计器有设备到地图的可靠的转换，这时候我们绕开VT将预先建图特征的子集重投影到当前的图像当中。

• 1. 基于位姿的匹配：我们可以发现和当前相机位姿附近的已建图的图像。在我们的方法中要求在当前位置的3米之内，并且在当前光轴45度范围内。这些满足条件的预先建图的图像将作为匹配对的初始集合。
• 2. 共视图：我们可以将和基于位姿的图像匹配共同特征超过50个的预先建图的图像加入集合。这可以保证pipeline中包含一些和当前相机位姿距离较远但是场景很相似的图像。
• 3. 特征重投影和匹配：所有预先建图的匹配图像的所有特征将重投影到当前的图像，匹配使用2进制描述子，匹配特征只考虑当前特征30个像素半径范围内的圆圈内。
• 4. 外点投影：我们使用2+1 pt RANSAC 在2D-3D匹配集合中，如果小于13个关联，我们将所有特征测量作为外点。

C. Additional outlier rejection

通过RANSAC我们已经尽可能地移除了2D-3D的外点，我们进一步对每一个特征使用马氏距离来提升系统的可靠性。

6. Experiment Results

硬件平台：Google Project Tango，鱼眼相机，全局快门，灰度相机，IMU，2.3GHz ARM Cortex-A15 CPU, 4GB of RAM

A. Data collection and ground truth

我们共收集了三个数据集

• DS1是用于测试估计器的
• DS2是用于生成地图中感兴趣区域的
• DS3是更大的地图

我们使用VOCON系统用贝叶斯最小估计来生成DS1的真值

B. Results

第一个场景是用户访问一个VICON房间很多次，这对非连续的估计器实际上是有优势的，因为设备估计器和地图变得有很强的关联，这里面比较了误差和3σ(σ = 7.5像素)边界，sC-SKF使用了2个子地图。


下表比较了C-SKF和sC-KF以及不连续方法的位姿RMSE，可以发现后者的性能明显弱于前面两者

前面我们谈到将地图划分成子地图的动机是为了减小计算J的计算量，我们在Tango上测试在不同地图尺寸下计算J所需要的时间

可以看到随着地图尺寸的增加，计算J将不可能实时地进行。

最后我们列出sC-SKF各个部分所需要的时间

7. Conclusion

在本文，我们聚焦于解决基于地图的连续定位估计问题。启发于SKF的计算代价随着地图增加而线性增长，但是需要的内存呈平方增长，我们提出了C-SKF，它使用地图的Cholesky因子来表示之前建好的地图的信息，再通过Cholesky因子的稀疏性，C-SKF的计算需求和内存需求随着地图尺寸基本线性增长。但是仍然会稍微高于线性增长，所以我们提出使用子地图的方法来进一步降低计算量，虽然这样会带来一定程度的精度损失。我们在Tango上证明了C-SKF和sC-SKF的性能要优于其他近似的，不连续的基于地图的方法。

Appendix: Proof of sub-map consistency