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)1.

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

总结相关特点有:

  • 金字塔
  • Inverse-compositional
  • 残差定义LSSD1考虑光照强度尺度的不变性
  • 计算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形式如下图:

Basalt的VIO Factor Graph
有几处稍不同的,包括landmark的参数化,以及相关的reprojection error形式。

2.2.1 Landmark参数化

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

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

( x y z ) = ( η u η v η − 1 ) , η = 2 1 + u 2 + v 2 \begin{pmatrix} x \\ y \\ z \end{pmatrix}= \begin{pmatrix} \eta u \\ \eta v \\ \eta - 1 \end{pmatrix}, \eta = \frac{2}{1+u^2+v^2} xyz=ηuηvη1,η=1+u2+v22

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

利用上述参数化,Reprojection error定义为
r = z − π ( T t − 1 T h q ( u , v , d ) ) , q ( u , v , d ) = ( x ( u , v ) y ( u , v ) z ( u , v ) d ) T \textbf{r} = \textbf{z} - \pi ( \textbf{T}_t^{-1} \textbf{T}_h \textbf{q}(u,v,d)), \\ \textbf{q}(u,v,d) = \begin{pmatrix} x(u,v) & y(u,v) & z(u,v) & d \end{pmatrix} ^T r=zπ(Tt1Thq(u,v,d)),q(u,v,d)=(x(u,v)y(u,v)z(u,v)d)T

其中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 Sparsification2.

E G ( s ) = ∑ r T Σ r + E nfr ( s ) E^\text{G} (\text{s}) = \sum{ \textbf{r}^T \bf{\Sigma} \textbf{r}} + E_\text{nfr}(\text{s}) EG(s)=rTΣr+Enfr(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,如下图左边。
NFR示意图
然后,用论文2中的方法,对这个dense prior进行sparse近似,使其形式变为右图。
具体而言,这个近似的结果,使得左图中的dense information matrix变成了4部分:

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

这4部分的residual定义分别如下,但其中Roll-Pitch和Yaw部分的定义还未完全理解
r rel ( s , z rel ) = log ⁡ ( z rel T j − 1 T i ) , r rp ( s , z rp ) = [ z rp R i − 1 ( 0 , 0 , − 1 ) T ] x y , r pos ( s , z pos ) = z pos − p i , r yaw ( s , z yaw ) = [ R i z yaw ] y , \begin{aligned} \textbf{r}_\text{rel}(\textbf{s}, \textbf{z}_\text{rel}) &= \log(\textbf{z}_\text{rel} \textbf{T}_j^{-1} \textbf{T}_i), \\ \textbf{r}_\text{rp}(\textbf{s}, \textbf{z}_\text{rp}) &= [ \textbf{z}_\text{rp} \textbf{R}_i^{-1} (0,0,-1)^T]_{xy}, \\ \textbf{r}_\text{pos}(\textbf{s}, \textbf{z}_\text{pos}) &= \textbf{z}_\text{pos} - \textbf{p}_i, \\ \textbf{r}_\text{yaw}(\textbf{s}, \textbf{z}_\text{yaw}) &= [ \textbf{R}_i \textbf{z}_\text{yaw}]_{y}, \end{aligned} rrel(s,zrel)rrp(s,zrp)rpos(s,zpos)ryaw(s,zyaw)=log(zrelTj1Ti),=[zrpRi1(0,0,1)T]xy,=zpospi,=[Rizyaw]y,

整个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 nfr ( s ) = ∑ i , j ∈ Relative-Pose r i , j T Σ i , j r i , j + ∑ i ∈ Roll-Pitch r i T Σ i r i E_\text{nfr}(\text{s}) = \sum_{i,j \in \text{Relative-Pose} } { \textbf{r}_{i,j}^T {\bf{\Sigma}}_{i,j} {\textbf{r}}_{i,j}} + \sum_{i \in \text{Roll-Pitch}} { \textbf{r}_{i}^T {\bf{\Sigma}}_{i} {\textbf{r}}_{i}} Enfr(s)=i,jRelative-Poseri,jTΣi,jri,j+iRoll-PitchriTΣiri

关于NFR近似的具体意义,等看论文2再补充


  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. ↩︎ ↩︎ ↩︎

  • 0
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值