Basalt Paper Reading

1 简要总结

项目网页：TUM CVG页面，以及github开源地址。

要点：

• Optimization based visual-inertial estimation.
• Two-layer odometry & mapping.
  – SOTA VIO system.
  – Non-linear factory recovery for visual-inertial mapping.

2 VI-Odometry

2.1 Sparse Optical Flow视觉前端

采用FAST角点检测，稀疏光流跟踪。除了用标准的金字塔和inverse-compositional方式以外，这里有所不同的是，patch相似性定义考虑了光照强度尺度的不变性，即LSSD(locally-scaled sum of squared differences)¹.

在光流跟踪时，计算frame-frame的SE(2)，既考虑了平移，也考虑了图像旋转。在outlier处理上，用正向+反向两次tracking的一致性来去除。

总结相关特点有：

• 金字塔
• Inverse-compositional
• 残差定义LSSD¹考虑光照强度尺度的不变性
• 计算SE(2)，同时考虑平移+旋转以最小化残差
• 正反两次tracking去除outlier

从demo视频看，tracking效果很好，值得看看optical flow具体代码实现。

2.2 VI Bundle Adjustment优化

Basalt维护了一个固定大小的sliding window，被估计状态有三部分：keyframe的pose，frame的pose+velocity+imu bias，landmark状态。这个State定义与OKVIS有一定的类似。Factor graph形式如下图：

有几处稍不同的，包括landmark的参数化，以及相关的reprojection error形式。

2.2.1 Landmark参数化

对于landmark，定义了host关键帧，即最早看到此landmark的关键帧。landmark的参数包括在host关键帧中的bearing angle单位向量 + inverse depth.

图像坐标[u,v]和bearing angle单位向量关系为

$$\left(\begin{array}{c}x\\ y\\ z\end{array}\right)=\left(\begin{array}{c}\eta u\\ \eta v\\ \eta -1\end{array}\right),\eta =\frac{2}{1+u^2+v^2}$$

这种方式可以将任意[u,v]表示为单位方向向量，只有相机系下(0,0,-1)的方向无法表示。

利用上述参数化，Reprojection error定义为
$$\begin{gathered} \text{r}=\text{z}-\pi ({\text{T}}_t^{-1}{\text{T}}_h\text{q}(u,v,d)), \\ \text{q}(u,v,d)={\left(\begin{array}{cccc}x(u,v)& y(u,v)& z(u,v)& d\end{array}\right)}^T \end{gathered}$$

其中q是三维齐次坐标，其中的x,y,z定义如上面单位方向向量公式，d是逆深度。
按论文的说法，这种残差定义形式在d接近零时也比较稳定。

2.2.2 优化与Marginalization

除了Reprojection error以外，其他残差项包括IMU和marginalization prior，与其他VIO方法类似。

Marginalization做法与VINS-Mono等经典方式类似，有两种情况：

1. 非keyframe，marginalize掉state中最新frame，并drop掉它host的landmark；
2. keyframe，marginalize掉最老的keyframe和它host的所有landmark（这里对landmark不是drop而是marginalize了），以及最老的frame的velocity和imu bias。

个人比较好奇gauge freedom的处理方式和OKVIS、VINS-MONO会有何不同，以及具体的marginalize实现方式（包括first-estimate jacobian在其中的实现），要看看代码实现。

3 VI-Mapping

全局优化的目标函数只有重投影误差和nfr(nonlinear factor residual)误差。其中NFR主要参考Michael Kaess组的Information Sparsification².

$$E^{\text{G}}(\text{s})=\sum {\text{r}}^T\mathbf{\Sigma }\text{r}+E_{\text{nfr}}(\text{s})$$

整个Mapping部分围绕的也是这两部分。
对于重投影误差，这篇文章是在keyframe中检测和匹配ORB特征来找视觉correspondence。

3.1 NFR factor

对于nfr误差，它来源于VIO进行keyframe的marginalize时保留的信息。

更具体一点，在VIO对keyframe进行marginalize时，先把window中keyframe pose（红色T）以外的所有信息全部marginalize，得到一个sliding window中keyframe pose之间dense的prior，如下图左边。

然后，用论文²中的方法，对这个dense prior进行sparse近似，使其形式变为右图。
具体而言，这个近似的结果，使得左图中的dense information matrix变成了4部分：

• Absolute Position
• Yaw
• Roll-Pitch
• Relative Pose

这4部分的residual定义分别如下，但其中Roll-Pitch和Yaw部分的定义还未完全理解。
$$\begin{array}{rl}{\text{r}}_{\text{rel}}(\text{s},{\text{z}}_{\text{rel}})& =\log ({\text{z}}_{\text{rel}}{\text{T}}_j^{-1}{\text{T}}_i),\\ {\text{r}}_{\text{rp}}(\text{s},{\text{z}}_{\text{rp}})& =[{\text{z}}_{\text{rp}}{\text{R}}_i^{-1}(0,0,-1{)}^T{]}_{xy},\\ {\text{r}}_{\text{pos}}(\text{s},{\text{z}}_{\text{pos}})& ={\text{z}}_{\text{pos}}-{\text{p}}_i,\\ {\text{r}}_{\text{yaw}}(\text{s},{\text{z}}_{\text{yaw}})& =[{\text{R}}_i{\text{z}}_{\text{yaw}}{]}_y,\end{array}$$

整个window中pose与global pose之间的约束都体现在了挂在红色T keyframe上的6DoF pose，以及它与其它keyframe的relative pose。

注意到在6DoF pose中，position和yaw是Visual-Inertial系统中unobservable的4个自由度，它们本质上对全局地图不提供有用信息。而roll-pitch是observable的，所以会分开处理。

最终使用时，只会用到Roll-Pitch和Relative Pose两部分：
$$E_{\text{nfr}}(\text{s})=\sum _{i,j\in \text{Relative-Pose}}{\text{r}}_{i,j}^T{\mathbf{\Sigma }}_{i,j}{\text{r}}_{i,j}+\sum _{i\in \text{Roll-Pitch}}{\text{r}}_i^T{\mathbf{\Sigma }}_i{\text{r}}_i$$

关于NFR近似的具体意义，等看论文²时再补充。

---

1. N. Roma, et al. A Comparative Analysis Of Cross-Correlation Matching Algorithms Using a Pyramidal Resolution Approach. ↩︎ ↩︎

2. Jerry Hsiung, et al. Information Sparsification in Visual-Inertial Odometry. ↩︎ ↩︎ ↩︎