LR: Direct Sparse Mapping
Abstract
Photometric bundle adjustment (PBA).
目前的PBA没法处理reobservation.
我们提出DSM(direct sparse mapping).
1. Introduction
边缘化策略通过移除旧的相机和点来减少计算量, 但是还保持系统对于不可观的*度(绝对的位姿和pose)的一致性.
通常光度收敛半径在1-2个像素. 所以一个标准的PBA不能补偿drift. 所以, 我们提出了multi-scale PBA来成功处理收敛性的难题.
我们需要保证PBA对于spurious(假的) observations的鲁棒性. 他们主要由稀疏的关键帧造成, 会导致阻挡和场景的reflections这样会破坏光度一致性的假设. 我们用基于t-distribution的influence function融合进PBA来处理.
- 可以重复使用已存地图信息的地图(用光度的)
- Local Map Covisibility Window(LMCW)来选择同样场景下的关键帧, 即使不是时间上连续的.
- coarse-to-fine的优化scheme来提升PBA的收敛半径. 这个策略可以利用丰富的几何信息.
- 用t-distribution来做外点处理.
- 在EuRoC数据上的验证
- make our implementation publicly available
2. Related Work
VINS-mono的前段和OKVIS类似, 但是用了额外的BRIEF来做回环检测.
LSD-SLAM用了FAB-MAP.
LDSO在DSO上接了一个ORB-DBoW.
他们都有以下缺点:
- A different objective function and points to those of the odometry
- loop closure detection relies on feature repeatability, missing many corrections
- 误差是均匀分配的, 很有可能不是最优解.
- 尽管轨迹在空间里被修正了, 但是地图点没有被重复使用.
一直相对的, Full SLAM像ORB-SLAM和DSM复用了地图.
DVO提出了probabilistic formulation给直接法的关联. [16]提出了用t-distribution鲁棒性. [17]提出在光度误差李t-distribution好使, 但是几何误差不好使.
3. Direct mapping
有front-end和back-end.
前端跟踪帧和点, 也提供了粗略的初始化.
后端决定用哪些帧构成局部滑窗, 共同优化所有的关键帧和地图点.
每个地图点有一个主帧, in which it was spawn, 由逆深度表示.
投影: \(\mathbf{u}=\pi(\mathbf{p})=\mathbf{K}\left(p_{x} / p_{z}, p_{y} / p_{z}, 1\right)^{T}\)
反投影: \(\mathbf{p}=\pi^{-1}(\mathbf{u}, \rho)=\rho^{-1} \mathbf{K}^{-1}\left(u_{x}, u_{y}, 1\right)^{T}\)
A. Photometric Model
\(E_{p}=\sum_{\mathbf{u}_{k} \in \mathcal{N}_{p}} w_{k}\left(\left(I_{i}\left[\mathbf{u}_{k}\right]-b_{i}\right)-\frac{e^{a_{i}}}{e^{a_{j}}}\left(I_{j}\left[\mathbf{u}_{k}^{\prime}\right]-b_{j}\right)\right)\)
\(w_{g_{k}}=\frac{c^{2}}{c^{2}+\|\nabla I\|_{2}^{2}}\)
B. Photometric Bundle Adjustment
\(E=\sum_{I_{i} \in \mathcal{K}} \sum_{\mathbf{p} \in \mathcal{P}_{i}} \sum_{j \in \operatorname{obs}(\mathbf{p})} \sum_{\mathbf{u}_{k} \in \mathcal{N}_{p}} w_{k} r_{k}^{2}(\boldsymbol{\xi})\)
LMCW复用地图点, 其中的初始解不在收敛半径内, PBA也没法优化.
所以提出了coarse-to-fine的优化框架over all active keyframes. 在每一层, 我们迭代直道收敛, 结果作为下一层的初值.
我们用LM. 每一次迭代的增量为: \(\delta \boldsymbol{\xi}^{(t)}=-\mathbf{H}^{-1} \mathbf{b}\)
这里\(\mathbf{H}=\mathbf{J}^{T} \mathbf{W} \mathbf{J}+\lambda \operatorname{diag}\left(\mathbf{J}^{T} \mathbf{W} \mathbf{J}\right), \mathbf{b}=\mathbf{J}^{T} \mathbf{W} \mathbf{r}\), \(\mathbf{W} \in \mathbb{R}^{m \times m}\)是对角阵, 由权重\(\omega_k\)组成.
4. LMCW: Local Map Covisibility Window
是用来选择active关键帧和地图点的策略.
LMCW包括两部分: 1. temporal. 2. covisible
\(N_t\)很重要, 因为相当于探索, 初始新的点. 当一个新的关键帧被插入, 我们把它插入到temporal part, 然后移除另一个. 所以我们维护了固定大小的temporal KFs.
- 永远保持最新的两个关键帧, 保证里程计的精度.
- 剩下的关键帧在空间中均匀的分布. 我们丢掉关键帧最大化: \(s\left(I_{i}\right)=\sqrt{d\left(I_{1}, I_{i}\right)} \sum_{j=3}^{N_{t}}\left(d\left(I_{i}, I_{j}\right)\right)^{-1}\)
- 这里\(d(I_i, I_j)\)是关键帧i和j的\(L_2\)距离. 这个策略偏爱高度共视的.
第二个部分是\(N_c\)共视的关键帧. 我们希望选择和temporal part共视的帧.
- 计算distance map来定义depleted(废弃的)区域. temporal part的所有地图点投影到最近的关键帧, 这样distance map注册了, 对于每一个像素, 有最近地图点投影的距离.
- 在旧关键的list中选择一个关键帧. 选择最大化在depleted area投影点数的帧. 我们丢弃掉视角大于一个阈值的点来检测和移除可能被遮挡(occluded)的点.
- 更新distance map来定义新的depleted areas.
- 从(2)迭代直道\(N_c\)共视关键帧被选择了, 或者没有合适的帧.
5. Robust Non-Linear PBA
LMCW选择了widely separated关键帧但是没有考虑任何光度一致性. 所以有可能有些点不能产生广度一致的图像. 他们可能是因为遮挡或者是场景反光导致的.
为了使得PBA对于缺少光度一致的情况的鲁邦, 我们提出了一种外点管理策略 based on photometric error distribution.
最大后验概率MLP等价于最小化负对数概率(negative log-likelihood). \(\boldsymbol{\xi}^{*}=\underset{\boldsymbol{\xi}}{\operatorname{argmin}}-\sum_{k}^{n} \log p\left(r_{k} | \boldsymbol{\xi}\right)\)
上面的式子等价于让导数为0. 也就是re-weighted最小二乘. \(w\left(r_{k}\right)=-\frac{\partial \log p\left(r_{k}\right)}{\partial r_{k}} \frac{1}{r_{k}}\)
所以这个解直接被光度误差的分布\(p(r_k)\)(看[15])影响. 然后我们考虑不同的分布.
Gaussian Distribution: 如果误差s被认为是在0附近的正态分布\(\mathcal{N}(0, \sigma^2_n)\), 那么误差分布的模型就是: \(p\left(r_{k}\right) \propto \exp \left(r_{k}^{2} / \sigma_{n}^{2}\right)\). 这个模型导致constant distribution of weights, 也就是标准的最小二乘. 但是它等价的对待所有点, 这样外点也不能被neutralized. \(w_{n}\left(r_{k}\right)=\frac{1}{\sigma_{n}^{2}}\)
Student's t-distribution: [15] 分析了RGB-D里程计的稠密光度误差. 它展示说t-distribution解释了稠密光度误差比正态分布更好, 权重函数是: \(w_{t}\left(r_{k}\right)=\frac{\nu+1}{\nu+\left(\frac{r_{k}}{\sigma_{t}}\right)^{2}}, \quad\) when \(\mu=0\)
我们学习了稀疏光度误差, 然后结论是t-distribution也合理的解释了稀疏模型.
与正态分布相比, t-distribution快速的降低权重因为errors move to the tail, 外点被分配了一个更小的权重.
除此以外, [15]固定了\(\nu=5\), 我们学习了当\(\nu\)和尺度\(\sigma_t\)切合时的表现.
为了切合t-distribution, we minimize the negative log-likelihood of the probability density function with respect to ν and σt using the gradient free iterative Nelder-Mead method [20].
Besides, we filter out the gross outliers before fitting the t-distribution. We approximate the scale value \(\hat{\sigma}\) using the Median Absolute Deviation (MAD) as \(\hat{\sigma}=1.4826\) MAD and reject errors that \(r_{k}>3 \hat{\sigma}\)
M-estimators: 当误差的分布难以获知的时候, 或者它被认为是正态分布的. 只用M-estimator是一个流行的方案. 就是huber estimator. 因为它没有完全移除高误差的观测, 只是降低了他们的影响力, 这个对于再观测处理很关键. \(w_{h}\left(r_{k}\right)=\left\{\begin{array}{ll}{\frac{1}{\sigma_{n}^{2}}} & {\text { if }\left|r_{k}\right|<\lambda} \\ {\frac{\lambda}{\sigma_{n}^{2}\left|r_{k}\right|}} & {\text { otherwise }}\end{array}\right.\)
这里\(\lambda\)一般是固定的, 或者动态的改动让\(\lambda = 1.345\sigma_n\) 对应 \(\mathcal{N}(0, \sigma_n^2)\). 这样的话, huber给外点线性的影响.
A. Implementation of the probabilistic model into the PBA
我们研究了在每个关键帧的误差分布, 结论说他们之间是有区别的. 这些变化可能来自于运动模糊, 遮挡或者是噪声. 所以, 我们对于每一个关键帧来拟合误差分布. 这样使得PBA对于不同的场景可以有调节, 比如一个特定的误差可能在常规的情况是外点, 但是在motion blur的情况下是内点.
B. Outlier Management
每个观测有8个像素. 为了考虑一个像素观测是不是内点, 它需要在目标关键帧的误差分布的95%内. 对于比较挑战的关键帧, 这个阈值可以高一些, 更permissive(许可的). 当局部的PBA完成的时候, 我们会在mask里计算内点的个数. 当一个观测的外电像素大于30%的时候, 这个观测会被标记为外点, 并从地图点的观测list中移除. 另外, 在优化的时候, 如果外点像素超过60%, 这个观测会直接被舍弃, i.e. \(\omega(r) = 0\).
6. Front-End
Frame-tracking: 每一帧用local map来跟踪. 初始估计由速度模型给出. 我们用coarse-to-fine的优化策略, 跟在PBA里一样, 来估计初始估计比较粗糙的情况. 对于大误差的物理和session 5一样. 另外, 我们用[21]里的inverse compositional approach来处理每次迭代重复计算雅克比的过程.
New Keyframe Decision: 当移动至没有探索过的区域的时候, 会插入新的关键帧.
- 地图可见比率和最新关键帧, i.e. \(s_{u}=N^{-1} \sum \min \left(p_{z} / p_{z}^{\prime}, 1\right)\). \(N\)是在上一个关键帧总可见点.
- 跟踪帧和上一帧的视差, 由translation \(t\) 和local map的平均逆深度\(\bar{\rho}: s_{t}=\|\mathbf{t} \bar{\rho}\|_{2}\)定义
- 光度变化: 由relative brightness transfer function(跟踪帧和上一关键帧), i.e. \(s_a = |a_k - a_i|\).
一个综合的分数会被用于确定是否插入关键帧: \(w_{u} s_{u}+w_{t} s_{t}+w_{a} s_{a}>1\)
New Map Point Tracking: 每一个关键帧有一列候选点. 我们用新跟踪的帧初始化候选点的逆深度. 在极线上搜索最小光度误差的匹配. 只有distinctive points with low uncertainty会被激活.
记住, 这个延迟的策略会要求几个匹配来获得一个好的初始化, 当极线比较小的时候, 同时也产生比较小的视差. 为了保证有足够的初始化好的点来激活, 我们保持keyframe的候选点直道这个关键帧掉出滑窗.
7. Results
A. Parameter analysis and tuning
1. Coarse-to-fine PBA: 我们衡量了金字塔层数\(N_p\).
没有金字塔的时候, DSM-SM(滑窗版本) 比DSM效果好. 这时候DSM没法从reobservation获利. 但是DSM在有更多金字塔层级的时候, 可以复用地图点从而获得更高的准确度.
尽管coarse-to-fine的策略对DSM提升明显, 对于DSM-SW却没有啥提升. 这个也是可预见的, 因为DSM需要更大的收敛半径, 但是DSM-SW缺不需要.
注意DSM在80%的runs下获得低于0.1m的RMS ATE的精度, 但是DSM-SW只有40%.
2. Robust Influence Function: 我们研究了model和权重的选择.
3. Number of covisible keyframes in the LMCW:
B. Quantitative results
8. Discussion & Future Work
即使用persistent map, 我们也知道不可能处理所有的再观测. 在大场景下, 累计误差使得重复检测地图点再观测成为不可能.