标签：phi text cos Manhattan SLAM World StructVIO sin Structure

一、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

摘要

标签：phi,text,cos,Manhattan,SLAM,World,StructVIO,sin,Structure
来源： https://blog.csdn.net/ydy1107/article/details/121231148

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。