一、StructVIO : Visual-inertial Odometry with Structural Regularity of Man-made Environments

摘要

• 基于Atlanta World模型对人工环境规则性进行描述。
• • Atlanta World 定义为包含有几个不同方向的局部Manhattan World世界。
• • 每个局部Manhattan World可以被实时检测，且它们的方向会在新观测到来时由状态估计器不断进行refine。

介绍

• 主要贡献
• • Atlanta World假设和结构化特征整合后的最小参数无缝线表示方法。每个局部Manhattan世界用一个航向方向来表示，并且会被不断refine。
• • 结构线（和主方向一致）和局部Manhattan World可以实时在线检测。即使没有检测到Manhattan World，垂直线（vertical line）依旧可以提供位姿信息观测。
• • 对状态估计器和线跟踪方法进行了改进。

结构线和Atlanta World

Atlanta World是多个具备不同水平方向 ϕ i ∈ [ 0 , π / 2 ] \phi_i \in [0,\pi/2] ϕi​∈[0,π/2]的Manhattan Worlds的组合，如图所示。
我们在里程计起点建立 Z Z Z 轴朝上（重力方向相反）的全局世界坐标系 W {W} W 。
每条线被锚定在第一次被图像中被观测到的局部坐标系，称这个坐标系统为“起始帧” S {S} S，其方向与这条直线所属的局部Manhattan World的 ϕ i \phi_i ϕi​ 相同，原点设置为该帧相机的位置。该起始帧原点位置 W p s ^Wps Wps， 会在滤波器中继续进行优化估计。

对于锚定在起始帧 S S S 上的一条给定直线，我们可以找到一个旋转 L S R ^S_LR LS​R 来将这条线从参数空间 L L L 转换到 S S S 。该条直线被对齐到 L L L 的 Z Z Z 轴上，如下图所示。
在参数空间 L L L 中，该条线可以用其与 X Y XY XY平面的交点来表示，即 L l p = ( a , b , 0 ) T ^Ll_p=(a,b,0)^T Llp​=(a,b,0)T。这里我们使用逆深度重新表示该条直线，即 ( θ , ρ , 0 ) T (\theta,\rho,0)^T (θ,ρ,0)T, 其中 ρ = 1 / a 2 + b 2 \rho=1/\sqrt{a^2+b^2} ρ=1/a2+b2 ​ 和 θ = atan2 ( b , a ) \theta=\text{atan2}(b,a) θ=atan2(b,a)。
该条线段在起始帧中可以通过以下公式计算：
L S R l p = a L S R ( : , 1 ) + b L S R ( : , 2 ) = cos θ ρ L S R ( : , 1 ) + sin θ ρ L S R ( : , 2 ) \begin{aligned} ^S_LRl_p &=a{^S_LR(:,1)}+b^S_LR(:,2) \\ &= {\text{cos}\theta \over \rho}{^S_LR(:,1)}+{\text{sin}\theta \over \rho}{^S_LR(:,2)} \end{aligned} LS​Rlp​​=aLS​R(:,1)+bLS​R(:,2)=ρcosθ​LS​R(:,1)+ρsinθ​LS​R(:,2)​
对于每个结构线，无论对齐到局部Manhattan World的哪一个轴上，旋转 L S R ^S_LR LS​R 一定是下面三个矩阵之一：
[ 0 0 1 0 1 0 − 1 0 0 ] \begin{bmatrix}0 & 0 & 1\\ 0 &1 & 0\\ -1 & 0 & 0 \end{bmatrix} ⎣⎡​00−1​010​100​⎦⎤​， [ 1 0 0 0 0 1 0 − 1 0 ] \begin{bmatrix}1 & 0 & 0\\ 0 &0 & 1\\ 0 & -1 & 0 \end{bmatrix} ⎣⎡​100​00−1​010​⎦⎤​ 和 [ 1 0 0 0 1 0 0 0 1 ] \begin{bmatrix}1 & 0 & 0\\ 0 &1 & 0\\ 0 & 0 & 1 \end{bmatrix} ⎣⎡​100​010​001​⎦⎤​，分别是对齐到 X X X, Y Y Y, Z Z Z轴。

从起始帧 S S S 到世界坐标系下的变换由旋转 S W R ( ϕ i ) ^W_SR(\phi_i) SW​R(ϕi​) 和 相机位置 W P s ^WPs WPs决定，其中 S W R ( ϕ i ) ^W_SR(\phi_i) SW​R(ϕi​)是绕重力方向轴的旋转，即
S W R ( ϕ i ) = [ cos ϕ i sin ϕ i 0 -sin ϕ i cos ϕ i 0 0 0 1 ] ^W_SR(\phi_i)=\begin{bmatrix} \text{cos}\phi_i & \text{sin}\phi_i & 0\\\text{-sin}\phi_i & \text{cos}\phi_i & 0\\ 0 & 0& 1\end{bmatrix} SW​R(ϕi​)=⎣⎡​cosϕi​-sinϕi​0​sinϕi​cosϕi​0​001​⎦⎤​
对于竖直线段，它们的起始帧坐标轴和世界坐标系保持一致，即 S W R = I 3 × 3 ^W_SR=I_{3\times3} SW​R=I3×3​。我们使用 S W R ( ϕ 0 ) ^W_SR(\phi_0) SW​R(ϕ0​), ( ϕ 0 = 0 ) (\phi_0=0) (ϕ0​=0) 来表示这类起始帧。
为了获取图像中的结构线投影，可以如下计算：
W l p = S W R ( ϕ i ) R S R L l p + W p S ^Wl_p= {^W_SR}(\phi_i){^S_R}R^Ll_p+{^Wp_S} Wlp​=SW​R(ϕi​)RS​RLlp​+WpS​
以及
C l p = W C R W l p + C p W ^Cl_p={^C_WR}^Wl_p+^Cp_W Clp​=WC​RWlp​+CpW​
用逆深度表示法 ( θ , ρ , 0 ) T (\theta,\rho,0)^T (θ,ρ,0)T 来取代 L l p ^Ll_p Llp​，则其在图像平面上的2D投影齐次坐标为：
C l p ∼ W C R S W R ( ϕ i ) L S R ⋅ r + ( W C R W p S + C p W ) ⋅ ρ ^Cl_p \sim {^C_WR}{^W_SR(\phi_i)}{^S_LR}\cdot r+({^C_WR^Wp_S+{^Cp_W}})\cdot \rho Clp​∼WC​RSW​R(ϕi​)LS​R⋅r+(WC​RWpS​+CpW​)⋅ρ，其中 r = [ cos θ , sin θ , 0 ] T r=[\text{cos}\theta, \text{sin}\theta,0]^T r=[cosθ,sinθ,0]T。经参数空间 Z Z Z 轴投影后的消影点齐次坐标计算如下：
C v ∼ W C R S W R ( ϕ i ) L S R ( : , 3 ) ^Cv \sim {^C_WR} {^W_SR(\phi_i)} {^S_L}R(:,3) Cv∼WC​RSW​R(ϕi​)LS​R(:,3)
考虑相机内参，这图像中的直线表示为：
i m l = ( K − T ) ( C l p × C v ) ^{im}l=(K^{-T})(^Cl_p \times {}^Cv) iml=(K−T)(Clp​×Cv)

通过上述定义，给定逆深度表示 l = ( θ , ρ ) T l=(\theta,\rho)^T l=(θ,ρ)T，局部Manhattan World 方向 ϕ i \phi_i ϕi​ ，以及该直线对齐的轴(用 L S R ^S_LR LS​R 描述)等参数，我们在3D线和2D观测之间建立了联系。
直线投影可以写作这些参数的函数，
i m l = Π ( l , ϕ i , L S R , C I τ , I W τ ) ^{im}l=\Pi(l,\phi_i,^S_LR,{^I_C\tau},{^W_I\tau}) iml=Π(l,ϕi​,LS​R,CI​τ,IW​τ)
其中 C I τ ^I_C\tau CI​τ 为IMU和相机之间的相对位姿变换，可以继续被滤波器优化。

系统概述

主要是EKF框架，略过。

关键实现

1. 直线检测和跟踪

• • LSD直线检测
• • 3D-2D匹配来跟踪直线量测。优点是利用了相机运动去预测结构线的可能位置，减少搜索范围。当获取新图片时，结构线会根据预积分预测的相机位姿进行投影，下一步就是找寻与该投影接近的线段。
• • 我们在结构线的两端点之间上均匀采样为： r s , r ( 1 ) , r ( 2 ) , . . . , r ( K − 2 ) , r e r_s,r^{(1)}, r^{(2)},...,r^{(K-2)},r_e rs​,r(1),r(2),...,r(K−2),re​, 对每个采样点，我们保存它在最新一帧的投影附近的图片块，然后通过ZNCC图像匹配方法来在候选线段中搜索它的相关点，如下图所示。最终我们选取相关点最多的线段。

2. 结构线段识别

• • 对于每个结构线段，我们试图找到它们位于哪个局部Manhattan World下。
• • 首先，计算某个Manhattan World三个方向的所有消影点。则 Z Z Z 轴上的消影点为：
    v z = K W C R [ 0    0    1 ] T v_z=K {^C_WR}[0 \ \ 0\ \ 1]^T vz​=KWC​R[0  0  1]T同理，对于 X X X 和 Y Y Y 方向，计算如下：
    v x ϕ i = K W C R [ cos ϕ i , -sin ϕ i , 0 ] T v y ϕ i = K W C R [ sin ϕ i , -cos ϕ i , 0 ] T \begin{aligned} v^{\phi_i}_x&=K{^C_W}R[\text{cos}\phi_i, \text{-sin}\phi_i, 0]^T\\ v^{\phi_i}_y&=K{^C_W}R[\text{sin}\phi_i, \text{-cos}\phi_i, 0]^T \end{aligned} vxϕi​​vyϕi​​​=KWC​R[cosϕi​,-sinϕi​,0]T=KWC​R[sinϕi​,-cosϕi​,0]T​ 注意到只有水平方向的消影点才会依赖局部Manhattan World的方向 ϕ i \phi_i ϕi​ 。因此，即使没有检测到Manhattan World，我们也可以识别出竖直线。
• • 为了从所有的检测线段中识别出结构线，从每个消影点引一条向线段 S S S 中点的射线，然后检测该射线和线段 S S S 的一致性。对每条线段都进行所有消影点的测试。一条线段若是与消影点其中一条一致，则判断为结构线。与该线段相关的Manhattan World就可以以对应的消影点确定下来。

3. 结构线初始化

• • 选择最具信息量的线段用来初始化：1）长度最长的。2）与已初始化线段距离较远的。
• • 初始化一个新结构线 l = ( θ , ρ ) l=(\theta,\rho) l=(θ,ρ) 的关键是找到角度参数 θ \theta θ , 因为逆深度值可以设定为一个预设值。初始化第一步是从结构线建立一个起始帧。
• • 对所有竖直方向的结构线，我们选择与世界坐标系轴对齐的那个起始帧，或者一个虚拟的Manhattan World坐标系， ϕ 0 = 0 \phi_0=0 ϕ0​=0。
• • 对水平方向的结构线，起始帧选择为与局部Manhattan World坐标一致， ϕ i \phi_i ϕi​。
• • 角度参数 θ \theta θ 由相机光心到局部参数空间的XY平面上的线的方向决定。该方向可以近似为相机光心到线段 s s s 中点到方向。 m m m 为 s s s 中点的齐次坐标，其相机平面的逆投影射线为 K − 1 m K^{-1}m K−1m，通过下式可以进一步转化到局部参数空间
    L m = S L R W S R ( ϕ i ) C W R K − 1 m ^Lm={^L_SR}{^S_WR}(\phi_i){^W_CRK^{-1}m} Lm=SL​RWS​R(ϕi​)CW​RK−1m其中对于竖直方向的结构线，前两个旋转皆为单位阵，简化为 L m = C W R K − 1 m ^Lm={^W_C}RK^{-1}m Lm=CW​RK−1m
    角度参数 θ \theta θ 就可以通过局部参数空间的水平方向来确定。让 L m = ( m x , m y , m z ) T ^Lm=(m_x,m_y,m_z)^T Lm=(mx​,my​,mz​)T, 则角度参数为 θ 0 = atan2 ( m y , m x ) \theta_0=\text{atan2}(m_y,m_x) θ0​=atan2(my​,mx​)，逆深度都初始化为 ρ 0 \rho_0 ρ0​。

二、Leveraging Structural Information to Improve Point Line Visual-Inertial Odometry

摘要