滑动窗口光束法平差的平方根边缘化

13260√滑动窗口光束法平差的平方根边缘化Nikolaus Demmel David Schubert Christiane Sommer Daniel Cremers Vladyslav Usenko慕尼黑{nikolaus.demmel，d.schubert，c.sommer，cremers，vlad.usenko} @ tum.de摘要在本文中，我们提出了一种新的平方根滑动窗口光束法平差适用于实时测距应用。平方根公式贯穿于我们基于优化的滑动窗口估计器的三个主要方面：对于光束法平差，我们用零空间投影消除界标变量;为了存储边际化先验，我们采用Hes的矩阵平方根;并且当边缘化旧姿势时，我们避免形成正规方程并且直接用专门的QR分解更新平方根先验。我们表明，提出的平方根边缘化是代数等价的常规使用舒尔补10410401041040 1000 2000 3000（SC）在Hessian上。此外，它优雅地处理秩亏雅可比矩阵产生一个事先相当于SC与Moore-Penrose逆。我们对真实世界数据集的视觉和视觉惯性里程计的评估表明，所提出的估计比基线快36%。它还表明，在单精度，传统的基于Hessian的边缘化导致数值图1. 上图：eurocMH 01序列上估计的视觉-惯性里程计轨迹。常规基线VIO-64在双精度浮点数下工作良好，但在单精度（VIO-32）下失败。相比之下，所提出的平方根估计器V10- 32即使在单精度下也保持相同的精度。它在台式机CPU上在26秒内处理整个序列（比实时快7.1倍）。底部：随时间演化的边缘化先验HessianH_m的最小特征值σmin（线性y-1）。故障和准确性降低我们分析数字的正确-轴线|标准差min| <10−8, logarithmic elsewhere). 我们期待价值观之前的边缘化的关系解释了为什么我们的平方根形式没有遭受同样的效果，因此需要更好的性能。1. 介绍长期以来，视觉里程计一直是环境测绘、机器人导航和自主系统中的关键组成部分随着智能手机或机器人吸尘器等低成本设备在我们的日常生活中变得越来越普遍，我们看到越来越需要以快速和稳健的方式解决此外，专用硬件上的可扩展解决方案要求算法以有限的浮点精度运行。使系统大小保持在一个固定的数值范围内这项工作得到了ERC高级资助SIMU-LACRON和德国科学基金会 资 助 CR 250/20-1“Splitting Methods for 3D Reconstruction andSLAM”的支持接近于零（半正定海森与规范自由）。而传统的（平方）制定在单精度导致负特征值与大幅度，积累的错误，（最终）数值失败，所提出的平方根方法有界幅度的σmin，并保持稳定。对于状态变量随时间的变化，边缘化是一种常用的技术，其中剩余的子问题可以用与以前相同的解的边缘分布然而，对于边缘化先验的实现以及相关联的优化问题的解决方案，存在多个选项。舒尔补技术是许多最先进的里程计和SLAM系统采用的易于实现的选择，但它依赖于线性化系统的海森矩阵。虽然这在许多应用中不是问题，但是在SC中涉及雅可比矩阵的平方以及因此平方条件数的事实可能导致min地面实况VIO-32（我们的）VIO-32VIO-64（我们的）VIO-32（我们的）VIO-64VIO-3213261二、二√√数值不稳定性。在卡尔曼滤波器文献中，这通常通过使用平方根滤波器方法来解决[29]。此外，最近已经表明，即使在基于优化的光束法平差设置中，也可以利用矩阵平方根来增加数值稳定性[6]。跟进这些发现，我们提出了一个基于优化的制定的视觉（惯性）圆顶尝试，使用矩阵平方根在优化阶段，以及存储和更新的边缘alization先验。我们把我们的方法的两种味道 VO和 VIO 分别用于纯视觉和视觉惯性测距本文的主要贡献如下：• 我们提出了一种新的平方根制定基于优化的滑动窗口估计。• 这个平方根公式的组成部分之一是一个专门的QR分解，我们证明了分析等价的边缘化的基础上SC和Cholesky分解。• 我们的QR边缘化自然包括秩亏Jacobian的情况。• 所提出的平方根配方，使优化为基础的VO和VIO与单精度浮点计算，而不损失的准确性。• 在几个真实世界的数据集上，我们系统地分析了使用零空间投影结合我们专门的QR分解对运行时间和准确性的影响。建议的平方根估计是23%的速度比基线和36%的速度更快时，额外切换到单精度。• 我们将我们的实现作为开源发布：https://go.vision.in.tum.de/rootvo网站。2. 相关工作在下文中，我们回顾了视觉和视觉惯性里程计的相关文献，讨论了基于滤波器和基于优化的技术，并强调了处理秩亏雅可比行列式的工作60多年前，Kalman提出了一种滤波器[12]，为许多不同应用中的状态估计奠定了基础，视觉惯性里程计就是其中之一。已经开发了卡尔曼滤波器的许多变体，对原始滤波器进行了不同的改进，其中VIO的一个突出示例是MSCKF [19]。平方根滤波器能够提高数值稳定性，并让系统在单精度硬件上运行[16，1，5，29]，特别是信息滤波器与基于优化的方法密切相关[26]。因此，所提出的方法与平方根中的许多优点第一个从卡尔曼滤波器的角度证明Schur补和零空间边缘化（使用矩阵平方根的技术）的等价性[30]。基于优化的视觉（惯性）里程计系统基于优化的方法已经成为视觉和视觉惯性里程计的最新技术。通常，它们被实现为固定滞后平滑器，其中旧的或冗余的状态变量被从优化窗口中删除，以保持系统的实时能力[25，13，7，22，28]。所有提到的方法都依赖于使用舒尔补的边缘化，这意味着边缘化能量被存储为剩余变量的二次函数。然而，最近的工作表明，以平方根形式执行边缘化[6]在数值上和运行时间方面都是有益的。虽然[6]仅针对具有临时界标边缘化的批优化示出了这一点，但我们引入了一种有效的方法来永久边缘化帧变量。秩亏雅可比矩阵大多数关于光束平差和测距系统的工作默认地假设满秩雅可比矩阵和信息矩阵，但情况可能并非总是如此。广义逆、广义Schur补和秩亏信息矩阵已经在BA或SLAM背景之外分析多年[23，15，2，4，20，21，14]。只有少数作品提出了处理SLAM中的秩不足的方法：Mazuran等人[18，17]引入了去除雅可比简并的所有相关矩阵的正交投影，并且Leutenegger等人。[13]在Hessian是奇异的情况下，使用伪逆用于3. 滑窗光束法平差3.1. 问题陈述3.1.1批量优化问题里程计的目标是从一组视觉、惯性或其他测量来估计系统状态。状态X可以由姿势、速度、在不同时刻的偏置（在下文中全部被称为帧变量）和界标位置组成在本文中，我们考虑视觉里程计（VO）和视觉惯性里程计（VIO），但这里描述的理论是一般的，可以应用到其他传感器（GPS，激光雷达）。如果涉及视觉测量，则界标位置是优化问题的一部分，并且最终解决束调整型问题。为了估计最优状态x，我们可以解决非线性最小二乘优化问题。E（x）=1W1/2r（x）2=1r（x）2（1）[29]但我们的可以重新线性化残差，直到它们离开滑动窗口并适应秩不足边缘化先验。Yang等人是其中r（x）是堆叠k个残差的向量函数，并且W 是与测量1326222.220Σ不确定性为了简化符号，我们将W吸收到加权残差向量r（x）中。我们的实现基于Basalt[28]，并且我们遵循他们的残差公式用于视觉和IMU术语。(1)是一个增量演化的批量优化问题，其大小不断增长，并且包括直到当前时间的测量值和变量。3.1.2滑窗能量为了保持系统的实时能力，优化被约束到最近状态和残差的小窗口。当添加新的状态时，通过边缘化去除旧的状态，并且我们保持来自残差的信息，所述残差依赖于在附加能量项Em（x）中去除的变量。与有效残差的平方向量ra一起，这形成滑动窗口能量Esw（x）=1ra（x）2 + Em（x）。（二）从这里开始，x只包含当前窗口中活动的变量。通过在添加新的测量和状态时重复边缘化变量我们把这种边缘化称为永久边缘化，因为边缘化变量不会重新引入问题。3.1.4优化为了获得状态估计，我们最小化（2）中的能量x=argminEsw（x），（7）X使用Levenberg-Marquardt优化。为了有效地做到这一点，我们利用地标雅可比矩阵的稀疏性：如通常在光束法平差中一样，我们使用所有地标的临时边缘化来显著减小问题大小，然后求解简化的相机系统，最后执行回代以恢复地标位置。3.2. 边缘化一旦（2）中的能量已经针对当前窗口被优化，并且在添加新帧之前，选择变量用于边缘化。现在，我们得出如何Em计算为即将到来的窗口的下一个时间步长与传统的海森为基础的方法，其次是建议的等效平方根制定在SEC。4.第一章3.2.1线性化使用线性化残差，我们可以根据来自当前（最佳）状态估计x的扰动Δx来近似（2）中的能量为Elin（∆x）=1r+J∆x2，（8）莱姆使用永久边缘化，我们描述了（2）如何从一个时间点转换到下一个时间点。3.2.r=ra（x）rm+ Jm（ x− x） 、（9）3.1.3Hessian与平方根形式最常见地，边缘化能量被存储为活动优化变量的二次形式：其中x0是旧边缘化先验的线性化点残差向量r包含有效残差ra和移位到当前状态估计X的旧边缘化残差。雅可比矩阵J包含ra的雅可比矩阵，并且Em（x）=1（x−x0）THm（x−x0）+bTm（x−x0），（3）边缘化JacobianJm.这就构成了一个线性方程组，并有相应的正规方程其中x0是线性化点。注意，严格地说，这里的x仅表示变量的子集，并且Hm和bm具有相应的大小（参见第2节）。3.2以取得详细数据）。还可以以平方根形式存储该能量H∆x=−b，（10）其中H=JTJ并且b=JTr。我们现在定义一组我们想要边缘化的变量xµ，即，我们希望下一个窗口中的能量（2）不依赖于xµany-0二个Em（x）=1rm+Jm（x− x）。（四）更多.设xκ是与xμ中的状态共享残差（或具有先验）的状态，而xu是与xμ中的状态不直接共享残差的直到加法常数，这与（3）相同，并且我们连接到xµ。有了这些，我们可以把H和b改写为13263κκ将膨胀点从x移动δxκκ可以通过以下方式将这两种表示联系起来HµµHµκ0联系我们T TH=HκµHκκ+HκκHκu，（11）Hm=Jm Jm，bm=Jm rm。（五）0HuκHuu注意，对于给定的H和b，J和r并不是独一无二的。意味µbµµ¯嗯嗯嗯0b=bκ+bκ。（十二）（3）中的等效移位可以使用Hessian块Hκκ被分成一个包含Hμ的依赖于μ变量的残差的雅可比行列式，以及b′m=bm+Hmδx ，Hm′ =Hm。（六）它包含了所有其他的。 这同样适用于bκ。HBu用rm′更新rm=Jm。的=rm+Jmδx，而Jm′13264µµκκκκκµµκκκ- -.Σ.Σµ κ r µ κ r µ κ r κ r残留块Oldmarg.之前的New Marg. 之前Householder向量住户住户下降图2.框架变量的QR边缘化最初，雅可比矩阵和残差向量（界标边缘化后）由依赖于μ变量的残差组成，并且到目前为止是活跃的，以及边缘化先验（左）。通过连续地应用Householder变换，矩阵被变换成上三角矩阵（在秩亏的情况下是平坦因此，在每次迭代中，除了Householder向量的最顶部元素之外的所有元素都消失。为了边缘化μ变量，我们删除相应的列和这些列非零的行，以及零行。这导致紧凑的新边缘化先验（右）。注意，左边的旧边缘化先验描述了总是最旧的变量被边缘化的情况在实践中，为了使µ列在左侧，变量可能必须重新排序。3.2.2Schur补可以通过将（11）的第一行乘以Hκµ H−1来证明解（10）w.r.t.xκ和xu等价于求解约化系统4. 平方根边缘化边缘化，如第二节所示。3.2是一种非常有效的方法，可以保持系统规模小，同时不断增加新的剩余项。但实施.H~+Hµ¯HκuΣ。∆xκΣ~b+bµ¯=−、（十三）当涉及到数值稳定性时，使用舒尔补具有一些缺点，例如，的条件数与HuκHuH~=Hµ∆xu-Hκµ H−1HµκBu，（14）与雅可比矩阵相比，海森矩阵的平方。在[6]中，表明用于变量的临时边缘化的平方根公式可以有利于速度、准确性和数值稳定性。我们现在应用类似的想法~b=bµ−HκµH−1bµ .（十五）滑动窗口光束法平差中的边缘化。故《易经》云：“以德为本，以德为本。H~和~b仅包含依赖于包含μ变量的残差的项因此，如果将取决于κ或u变量的新残差添加到能量，则它们不会改变。事实上，即使这样，简化系统的解仍然与包括新残差的完整系统的解相同。H~和~b已经使用线性化在当前状态估计x。为了保持系统的一致性，κ变量的线性化点x0可以在针对g iv enx0计算H ~和~ b之后不改变。4.1. 地标边缘化一种选择是一步到位地永久边缘化地标和框架变量。然而，我们从[6]中知道，我们可以使用零空间投影来利用Jacobian的特殊稀疏结构进行界标边缘化。另一方面，零空间投影对于帧变量的永久边缘化不是最佳的（见下文）。因此，我们选择两步过程：首先，我们通过投影将x µ中的界标边缘化这些所谓的第一估计Jacobian [11]防止零空间的破坏，并且通常使用[13，7，28]。因此，为了将新的边缘化能量写成（3），我们使用（6）得到Hm=H~和bm=~bH~（xκx0）。在实践中，在旧窗口的优化已经收敛之后计算一次Hm和bm，仅使用那些依赖于μ变量的能量项（始终包括旧的先验）。结果构成了新窗口中使用的边缘化先验。为了保持地标-地标Hessian块中的稀疏性，我们在计算H~和~b之前丢弃将在将要被局部化的帧中保持活跃的地标的观测。这意味着界标从来不是κ变量的一部分。将Jacobians映射到界标Jacobi的零空间Q2上bian使用QR分解，如[6]中。在第二步骤中，我们将xµ中的帧变量边缘化，将投影雅可比矩阵QT2J和残差QT2r作为输入。4.2. 框架变量边缘化为了获得平方根形式的边缘化先验，我们可以类似于[6]进行并对雅可比矩阵Jμ进行QR分解。然而，我们希望保持边缘化先验的大小尽可能小，并且我们的目标是更一般的解决方案，包括Jµ（或J）没有µµ13265满秩的情况。为了实现这一点，我们使用专门13266× −×01κκκ不R=nr n，标准r n，平坦在R中没有被丢弃，并且x和x0已经删除了边际化变量。4.3. 等同于基于SC的边缘化我们将只展示我们的平坦QR分解对于永久边缘化与Schur补的等价性，因为零空间零非零户主元素秩亏揭示暂时边缘化的边缘化可在[6]中找到。 我们集中讨论平方根形式的R~，r~和Hessian形式的H~，~b的等式。r~和~b上的移位图3.我们专门的平面QR分解。左：对于满秩矩阵，QR分解导致对角线上具有非零元素的上三角矩阵R中间：如果秩r小于最大可能秩η，则标准House_holder QR在对角线上产生零个元素。右：对于平面QR，当出现零对角元素时，下一列的Householder元素将在同一行中。使用Householder反射的QR算法，并且是秩-为了获得rm和bm，则由（6）等价。4.3.1满秩雅可比矩阵由于J=QR 中的Q是正交矩阵，因此H=JT J=RTR，并且R是全Hessian的平方根我们可以将R写为分块矩阵，并定义正交变换的残差向量r’=QT r，使得b=RT r’： . R1µ R1κΣ，.r1′Σ。（十六）没有任何旋转地显露。图2示出了满秩情况下的整个过程。标准Householder QR [9，第248页]将条目归零0R2κ当J是满秩矩阵时，R1µ大小的角矩阵r2′R2κ上三角形在Householder元素下面的对角线上，逐列。这可能导致高度大于当该列不线性依赖于之前的列和Householder元素时，结果Rnµnµ和（N nµ）nκ，残差数为N（在界标零空间边缘后-化）。上三角性质意味着R2κ的底部N−nκ行都为零，因此保持为零（如图所示）。3，中间）。在我们的特殊化QR中，如果这发生在矩阵元素jik处，则我们继续进行到下一列k+ 1，而不是将所有内容R2κ =. R~Σ。（十七）下面的元素j i+1，k+1像在标准QR，我们保持行索引在i和零以下的一切j i，k+1（见图）。3，右）。因此，我们永远不会得到结果是有效的QR分解，具有两个吸引人的属性：当J具有满秩时，其等同于标准Householder QR。此外，我们的QR算法是秩揭示的，没有任何枢转，秩r的J是-ing在R中的非零行的数量。 Jµ的秩rµ是QT Jµ中非零行的数量，即，第一个nµ注意，通过设置R〜=LT或R〜=D2LT，还可以通过与{ μ，κ }相关 的Hessian 子块 的LLT 或LDLT 分解 来获 得具有 相同属性（但一般不相等）的矩阵R。因此，我们甚至可以在计算后将边缘化先验以平方根的形式存储Hessian，而不执行任何QR分解。我们现在证明了Schur补的边缘化等价于保留R2Tκ和r2′，并丢弃R2Tκ和r2 ′。R和r′的主分量。 使用（16），我们得到列在QT J中。我们的R是在一般情况下更平坦的比其他枢轴自由QR算法。Hµµ =R1TµR1µ，Hκκ =R1TκR1κ +R2TκR2κ，对于边缘化，我们取R并去掉第一个rµ行、前nµ列和底部的所有零行。剩下的矩阵，我们在下面称之为R~，是Hµκ=R1TµR1κ，Hκµ=R1TκR1µ，bµ=R1Tµr1′，bκ=R1Tκr1′+R2Tκr2′，（十八）最大化HessianH~的平方根，如将在以下子部分中示出的。它具有与H~相同的列数ηκ，而其列数可以是其中，Hκκ和bκ表示Hμ和bμ。使用这些表达式并假设R1µ可逆，(14)和（15）可以变换为更小，并且等于秩（H~）。我们也要实践。需要较小的先验大小，并可直接读取掉秩H~=R1TκR1κ+R2TκR2κ的先验，这对于基于SC的边缘化不那么容易新边缘化能量的定义是（4）由Jm=R~且rm=r~-R~（x-x0）表示，其中r~包含其相应的r为-R1TκR1µ（R1TµR1µ）−1R1TµR1κ=R1TκR1κ+R2TκR 2κ−R1TκR 1κ=R2TκR2κ=R~TR~，×××××××× ××××××××× ×联系我们联系我们×个13267（十九）13268.Σ∈∈.Σ--∈µµµ µ1κ 1µµ2κ 21κ1µ1µ 1κκ11κκκ111111111Σ--和~b=RTr′+RTr′−RTR（RTR）−1RTr′也就是Jµ列所跨越的空间（见附录）。因此，使用我们的平坦QR分解，我们得到与使用伪SC的解决方案相同，但没有=R1Tκr1′+R2Tκr2′−R1Tκr1′=R2Tκr2′= R~Tr~。（二十）需要计算伪逆。4.3.3广义Schur补的两个等式中的最后一个等式是由于R2κ在下部是空的，参见（17）。4.3.2秩亏雅可比矩阵现在让我们假设我们的问题的雅可比矩阵J以及因此的海森矩阵H是秩亏的，即，它们的秩小于我们优化的参数的总数。例如，当系统没有被初始先验正确地约束时，或者当先验在操作期间变得小于数字噪声时，这可能发生我们执行秩揭示、无旋转的QR分解，以计算正交Q=Q1Q2，其中Q1RN×rµ，Q2RN×（N−rµ）和rµ= rank（Jµ）使得基质我们采用常用的Schur补技术的推广，在雅可比矩阵不是满秩的情况下，该技术简单地用伪逆来替换矩阵逆[13，17]。下面，我们将讨论为什么这是一个好主意：首先，我们将∆xtot定义为通过计算得到的解H+b，并且令Δxred是我们通过使用如（22），（23）中的广义H ~和~ b并使用伪逆求解简化系统（13）而获得的解，（可能）随后是Δx µ的回代。然后我们可以使用[14]中的引理2.3来发现，如果rank（J µ）+ rankJ κJ u= rank（J），（26）即，如果对应于μ和的雅可比子块R =. R1µR1κ=0R2κQT1QT2.Jµ中文（简体）κ，u不通过线性相关列耦合，两个解相同，证明见附录。在满足H∆x= −b的所有可能的∆x中，解通过H +得到的∆xtot是范数∆x最小的一个。左下角有一个零块，R1µRrµ×nµ。边缘化以与满秩情况相同的方式进行，即，我们去掉R1µ和R1κ，只保留R2κ，mi-nus在底部都是零，以矩阵R~结束。由此获得的最大化Hessian，R~TR~，与伪舒尔补相同，其中在（14）、（15）中，逆被伪逆替换：H~=Hκκ−HκμH+Hμκ，（22）~b=bκκ−Hκ µH+b。（23）为了证明这种等价性，我们首先重写R~TR~=R2TκR2κ=JκTQ2QT2Jκ在实践中，这通常是首选的解决方案，因为我们如果（26）不成立，例如在存在绝对位姿模糊的情况下，附加残差可以去除耦合秩不足，并且伪舒尔补方法产生用于简化系统的相同解，就好像计算了包括新能量项的整个系统的最小范数解一样。注意，虽然最小范数解的性质非常吸引人，但伪逆的计算通常使用SVD分解，这可能很慢。更快的矩阵分解技术，可以处理奇异矩阵，例如。LU或LDLT，可输出解决方案∆x，∆x 我们的平面QR结合了=JT J -JT Q QT J（二十四）它产生与最小范数解相同的解并且可以有效地实现。=Hκκ−JκTQ1QT1Jκ。另一方面，对于伪舒尔补，我们可以使用紧凑SVD分 解 Jμ=U1D1V1T （ 即 ， D1 的 大 小 为 rµ×rµ ， 见 附录），以获得Hκκ−HκµH+Hµκ=Hκκ−JκTJµ（JµTJµ）+JµTJκ=H−JT（U DVT）（V D−2VT）（V DUT）J5. 评价我们的实现基于开源odometryBasalt，这是一种高效的最先进的边缘化滑动窗口测距法，其中KLT特征跟踪作为前端，并且在后端进行基于SC的优化和边缘化[28]。在一个小的改编为了略微提高性能，我们将一个功能1µ1µκκκ.Σ13269=Hκκ−JκTU1U1TJκ=Hκκ−JκTQ1QT1Jκ，（二十五）其磁道已经立即丢失，并且不仅连同其主机帧一起丢失。NS-消除土地的投影-同样的，对于B。最后一个等式是由于Q1和U1跨越RN的相同rμ维子空间，marks的灵感来自[6]，但由于在优化过程中，我们解决了一个只有7个关键帧和几个关键帧的小系统13270√√EmVIO-64（我们的）十八103十两十七十十二VIO-64VIO-32010002000 3000010002000 3000010002000 3000010002000 3000图4.在视觉-惯性里程计中，我们期望4个计量自由度为了证实这一点的边缘化残差，我们调查的边缘化之前的成本变化时，扰动的线性化点的全局平移在x，y，或z，由一个全球性的（线性化）旋转滚转，俯仰，或偏航，或由一个随机的单位范数向量。虽然我们的平方根边缘化导致与单精度和双精度的预期零空间一致的先验在帧2000附近，对于VIO-32，似乎偏航错误地与滚转和俯仰一样可观察。√VIO-64√VIO-32VIO-64VIO-32√VO-64√VO-32VO-64VO-32eurocMH010.0930.0930.0930.991kitti003.923.923.92XeurocMH020.0480.0480.0480.048kitti029.729.729.72XeurocMH030.0510.0510.051Xkitti031.341.341.341.34eurocMH040.1090.1090.109Xkitti041.221.221.221.22eurocMH050.1370.1370.137Xkitti052.752.752.75XeurocV1010.0430.0430.0430.043kitti062.612.612.612.61eurocV1020.0480.0480.0480.048kitti071.521.531.521.44eurocV1030.0580.0580.058Xkitti083.853.853.85XeurocV2010.0370.0370.0370.037kitti094.134.134.13XeurocV2020.0530.0530.053Xkitti101.111.111.1126.12tumvi-corr10.3000.3000.300X表2. VO的绝对轨迹误差（以米为单位）显示出与表1中的VIO相同的趋势。1.一、表1. VIO的绝对轨迹误差（以米为单位）表明，与基线相比，所提出的方法也以浮点精度工作，提供基本上相同的精度。数百个地标，我们做了几个改进运行时间的调整：代替共轭梯度来求解简化的相机系统（RCS），我们显式地形成正规方程并用LDLT求解，并且我们跳过雅可比缩放和地标阻尼。另一种替代解决RCS，避免正常方程将是QR分解，我们发现有更高的运行时间。我们在一个代码库中实现所有内容，确保最大的可比性：所比较的变量之间的唯一区别是先验存储的选择（平方对平方根）以及用于优化和边缘化的算法。所有变体都很好地利用了多线程，并使用了最先进的密集线性代数库[10]。浮点精度由后缀表示，如VIO-64KITTI里程计基准（训练集）[8]我们评估VO，因为它没有同步的IMU数据（kitti01被排除在外，因为[28]的光流失败）。其他结果见附录。5.1. 精度和运行时间我们用绝对轨迹误差（ATE）评估姿态估计的准确性，绝对轨迹误差（ATE）是SE（3）与地面实况对准后相机位置的平移RMSE[27]。选项卡. 1和Tab。图2分别示出了VIO和VO的ATE，均为平方根和平方实现。可以看出，在双精度中，两个变量都产生相同的精度，如VIO-32，而VIO-32经常在数值上失败或产生非常高的ATE（对于VO也是如此）。定性地说，基于SC的单精度估计器最初表现良好，但通常很快就会发散（见图2）。1顶部）。总运行时间的最大部分花费在优化上-或VO-32。实验在Ubuntu 18.04上运行。 在那里，使用NS投影在单个VIO-32（我们的）x，y，z滚转，俯仰，偏航随机tumvi-corr20.4260.4260.426Xtumvi-mag11.4561.4571.456Xtumvi-mag20.9080.9200.908Xtumvi-room10.1020.1020.1020.104公司简介0.0710.0710.071Xtumvi-slides 10.3100.3100.310X台式机，64 GB RAM和英特尔至强W-2133，tumvi-slides0.7590.7590.759X12个3.60GHz虚拟内核VIO是在Eu-RoC MAV数据集[3]和TUMVI的子集[24]。为132712√√√√VIO-64√VIO-32VIO-64VIO-32eurocMH0123.4/2.518.6/2.3 35.9/1.833.4 /1.7欧洲MH 0220.0/2.115.6/1.9 31.7/1.529.0 /1.4欧洲MH 0317.6/1.813.9/1.6 26.3/1.3x欧洲MH 0413.1/1.310.3/1.2 19.5/0.9x欧洲MH 0515.0/1.511.6/1.3 22.6/1.1x沪公网安备31011502000112号欧洲V1028.3/1.06.8/0.911.5/0.710.6 /0.7欧洲V1038.3/1.06.7/0.911.1/0.7x拟定消融研究选购配件玛格NS+LDLTNS+QRSC+LDLTNS+LDLTNS+QR SC+SCSC+LDLTSC+SC精度六四三二六四三二六四三二六四三二ATE [m]0.068 0.0680.068 0.068 0.0680.0680.211实时t总计[s]6.9x 8.2x17.9 十四点九5.0x5.6倍7.1x7.9x5.2x 5.5x24.4 二十一点八 十七点四 十五点五 二十三点七 二十二点二t选项[s]14.4 十一点四22.2 20.3 十四点四 十一点五 二十二点一 20.4t margin [s] 1.6 1.5 1.61.3 1.4 1.41.31.2欧洲V20112.1/1.49.5/1.420.8/1.019.2 /1.0欧洲V20211.4/1.39.3/1.215.5/0.9xtumvi-corr124.4/3.218.7/2.6 36.7/2.2xtumvi-corr229.4/3.822.0/3.1 42.2/2.6xtumvi-mag178.1/ 10.557.4/8.4112.5/7.0xtumvi-mag259.6/7.742.2/6.3 88.2/5.1xtumvi-room113.2/1.710.0/1.4 21.6/1.319.6 /1.3tumvi-room212.2/1.89.4/1.520.2/1.3xtumvi-slides128.6/3.620.9/3.044.1/2.5xtumvi-slides224.8/3.118.5/2.538.8/2.1x表3.在VIO中花费在“optimization / ma r ginaliza t √ io n”上的总运行时间（秒）。优化：使用NS投影表4.优化和边缘化技术的不同组合，以及EuRoC上VIO所有变 体 以 平 方 根 形 式 存 储 边 缘 化 先 验 （ 4 ） 。 所 示 度 量（ATE，运行时间：总/优化/边际化）是所有序列的平均值，并且实时因子指示与序列持续时间相比处理快多少所提出的平方根边缘化NS+QR在单精度下具有良好的精度，而平方根优化NS+LDLT导致最佳运行时间。地标（VIO-32）的速度几乎是基线的两倍SC（VIO-64）。边缘化：传统的SC稍微快一点，但是这一步只占用整个运行时间的一小部分。精度相比，双，而对于SC的加速只有8%。这是因为对于NS-投影，我们可以在较大的矩阵上进行密集的线性代数运算，而有效的SC实现需要利用稀疏性并在小矩阵块上显式较大的矩阵运算从现代CPU的SIMD指令中受益更多。在总运行时间方面，建议的VIO-32比基准VIO-64快36%（参见表10）。（3）第三章。在消融研究中，我们将平方根先验形式（4）与不同的优化（NS+LDLT和SC+LDLT）和边缘化变体（NS+QR和SC+SC）组合。在这里，SC边缘化总是紧接着用LDLT分解将先验分解成平方根形式我们可以看到，这种因式分解本身是不足以防止在单精度的准确性的严重退化只有结合所有提出的改进，才能达到最佳的准确性和运行时间（见表1）。4）.5.2. 边缘化先验在分析上，Hm是半正定的，并且具有与非线性系统等价的零空间（这是我们使用第一估计雅可比矩阵的原因之一，参见第2节）。3.2.2）。该零空间（至少）包含与系统的全局规范自由度对应注意，虽然为了优化，我们在固定规范之前添加绝对姿态，但在这里我们考虑没有这种额外先验的Hm我们通过查看其最小特征值σmin和在预期规范自由度方向上线性化点周围的状态扰动ε的先验成本变化ΔEm，其中||ϵ||= 1：∆Em = Em（x0+ ε）− Em（x0）=1ε T Hmε + ε Tbm。（ 二十七）注意，特征值和成本变化总是在将先验转换为双倍之后计算，并且对于平方根估计量，我们计算Hm=JmTJm。 图图1（底部）示出了利用单精度的平方公式，我们得到具有大幅度的负特征值（不确定先验）或大的正最小特征值（零空间消失）。同样，我们在图中观察到。第四个在仪表上自由速度似乎消失了。所提出的VIO-32和VO-32都不存在这些问题，并且不存在这些问题。数值稳定，从而保持完全的准确性。6. 结论我们介绍了一种平方根滑动窗口束调整方法，非常适合于实时视觉和视觉惯性里程计应用。该方法将使用零空间投影消除地标变量与Hessian的矩阵平方根相结合，用于存储maginalization先验，其进而使用专门的QR分解直接更新。我们证明了专门的QR分解是（解析）等价于舒尔补。然而，对一系列真实世界数据集的实验评估表明，所提出的方法比基线快23%此外，与基线方法相比，所提出的方法在以单浮点精度运行时保持数值稳定，从而导致36%的组合加速，同时保持相同的准确性和鲁棒性。√13272引用[1] 杰拉德·J·比尔曼。离散序列估计的因子分解方法。Courier Corporation，2006.[2] Fennell Burns，David Carlson，Emilie Haynsworth，andThomas Markham. 使用schur补的广义逆公式。SIAMJournal on Applied Mathematics，26（2）：254[3] Michael Burri、Janosch Nikolic、Pascal Gohl、ThomasSchneider 、 Joern Rehder 、 Sammy Omari 、 Markus WAchte-lik和Roland Siegwart。欧洲微型飞行器数据集。国际机器人研究杂志（IJRR），2016年。[4] 大卫·卡尔森艾米莉·海恩斯沃思托马斯·马卡姆。利用moore-penrose逆推广schur补。SIAM Journal on AppliedMathematics，26（1）：169[5] 弗兰克·德拉尔和迈克尔·凯斯平方根SAM：通过平方根信息平滑同时定位和映射。The International Journal ofRobotics Research（IJRR），25（12）：1181[6] NikolausDemmel ， ChristianeSommer ， DanielCremers，and Vladyslav Usenko.大范围重建的平方根光束法平差在计算机视觉和模式识别会议，2021年。[7] Jakob Engel，Vladlen Koltun，and Daniel Cremers.直接稀 疏 测 距 法 。 Transactions on Pattern Analysis andMachine Intelligence（PAMI），40（3）：611[8] Andreas Geiger，Philip Lenz，and Raquel Urtasun.我们准备好自动驾驶了吗？KITTI视觉基准测试套件。在计算机视觉和模式识别会议中，2012年。[9] 吉恩·H Golub和Charles F.范·洛恩矩阵计算。JHU出版社，第四版，2013年。[10] G ae¨ lGuennebaud，Ben o¨ıtJacob，etal.本 征v3。http://eigen.tuxfamily.org，2010年。[11] 黄国权，阿纳斯塔西奥斯. Mourikis和Stergios I.鲁梅利奥蒂斯第一估计雅可比EKF用于改善SLAM一致性。实验机器人，第373- 377382.施普林格柏林海德堡，2009年。[12] R.E.卡尔曼线性滤波和预测问题的一种新方法。Journalof Basic Engineering，82（1）：35-45，1960.[13] Stefan Leutenegger 、 Simon Lynen 、 Michael Bosse 、Roland Siegwart和Paul Furgale。使用非线性最佳化的基于关键帧的视觉-惯性里程计。The International Journalof Robotics Research（IJRR），34（3）：314[14] 刘建州和黄蓉。矩阵的广义Schur补与复合矩阵。《线性代数电子期刊》，2010年，第21期。[15] George Marsaglia和George PH Styan。分块矩阵广义逆的秩条件。Sankhya¯ ：The Indian Journal of Statistics，Series A，pages 437[16] 彼得·S·梅贝克随机模型、估计与控制。学术出版社，1982年。[17] Mladen Mazuran ， Wolfram Burgard ， and Gian DiegoTipaldi. 长期SLA