自己理解,以做备忘
Struct VIO
结构线和亚特兰大世界
- 多个不同水平角度( ϕ i ∈ [ 0 , π / 2 ] , i = 1 , 2 , . . . . , N \phi_i \in[0,\pi/2],i=1,2,....,N ϕi∈[0,π/2],i=1,2,....,N)的曼哈顿世界组成一个亚特兰大世界。并不是每个局部世界都必须是一个包含三个主方向的实盒世界。
- 本文设置了一个虚拟的曼哈顿世界 ϕ 0 \phi_0 ϕ0,与真实全局世界坐标系 W {W} W 的方向一致。
关键内容
A .线段检测和跟踪=====================
- LSD检测,为每条结构线并使用3D-2D匹配跟踪线段测量。(3D-2D跟踪优势是利用相机运动来预测结构线在图像可能的位置来降低搜寻范围)
- 当新的帧到时,通过IMU积分来预测相机位姿来预测结构线。下一步在靠近这个线段投影(知道了预测位姿和上一帧3D线,可以投影到新的一帧)的位置附近在搜寻线段。
- 通过线投影和检测线段之间的位置和方向不同来搜寻备选线段集合。
- 在线段均匀采样多个点来提高跟踪鲁棒性
- ZNCC点块匹配跟踪
B.结构线段检测
在检测到线段之后就需要找到结构线,对齐到曼哈顿世界的3个方向,分以下几步:
- 1、计算所有的和曼哈顿世界相关的三个主方向的所有灭点。由式(7),得到Z方向上的灭点 v z v_z vz,同理,对于X,Y方向,得到 v x ϕ i v_x^{\phi_i} vxϕi,和 v y ϕ i v_y^{\phi_i} vyϕi。注:只有灭点的水平方向依赖于局部曼哈顿世界的航向 ϕ i \phi_i ϕi。因此,即使没有曼哈顿世界,也可以识别出垂直的线段。
- 2、连接灭点与线段中点,如果线段一致(满足阈值)的话则进行筛选出结构线段。然后从相应的灭点确定与此段相关的曼哈顿世界。
- note:如果没有检测到曼哈顿世界,垂直结构线仍然被认为是曼哈顿世界(假设的)的垂直方向??如公式(25)
C.结构线条初始化
在识别出当前图像中新提取的线段中的结构线段后,只选择其中一部分进行初始化,以免冗余的初始化(对单个环境3D线段的多次检测的线段进行初始化),并使初始化后的线段在图像中均匀分布
- 先利用IMU重力方向初始化垂直结构线,再初始化水平结构线
- 两个规则初始化结构线:
- 1、线段选择最长的
- 2、排除靠近已经初始化的线段
- 通过上述规则,首先排除那些已经初始化的线段,接下来,根据线段长度按降序排序,将它们放入队列
s
s
s,使用以下迭代来选择线段进行初始化:
- 1、取出队列 s s s前端的线段
- 2、从 s s s中初始化一个新的结构线段
- 3、移除那些接近队列 s s s中线段的线段并转到步骤1,直到队列为空或者结构线达到最大允许范围(阈值)
- 从所选的线段初始化新的结构线段 l = ( θ , ρ ) l=(\theta,\rho) l=(θ,ρ):关键是找到角度参数 θ \theta θ,而逆深度值 ρ \rho ρ可以设置为一个预设值。所有结构线的垂直方向,选择开始帧与世界坐标系 W {W} W对齐的,或者一个假设的曼哈顿世界( ϕ 0 = 0 \phi_0=0 ϕ0=0)。如果没有检测到曼哈顿世界,此选择将方便表示垂直线。对于水平方向的结构线,选择开始帧是因为其轴与局部曼哈顿世界 ϕ i \phi_i ϕi对齐。
- 角度参数 θ \theta θ由摄像机中心到局部参数空间 L {L} L的 X Y XY XY平面上的线的方向确定。方向可以用光线从相机中心到线段队列 s s s上的中点来近似表示。
D.用先验知识三角化结构线
- 在每个状态更新后调用三角测量来更新所有线段的参数。最小化可见线段的重投影误差来实现。若仅使用状态内可见视图中的观测值,则通常会导致较小的运动视差并产生不精确的三角化结果,如果我们使用所有可见视图进行三角化,则计算成本增加,并且状态外视图老的姿态估计也可能导致较大的误差。
- 为解决上述问题,每条结构线保留一个先验分布,其中均值 l 0 = ( θ 0 , ρ 0 ) l_0=(\theta_0,\rho_0) l0=(θ0,ρ0) 和协方差矩阵 Σ 0 \Sigma_0 Σ0存储初始先验分布,或者由历史测量的先验分布。
- 总的目标函数是:
- 以上通过GN求解迭代
- 三角测量后,为了更可靠跟踪线段。通过该结构线与最后一个可见视图中线段两端的反向投影光线的交点来更新结构线的范围。(不是很明白)
E.处理长线段跟踪丢失的测量
- 滑窗之外的测量可能仍然携带丰富的几何形状信息,故需要把这些信息作为一个先验
- (类似与滑窗吧,没看明白)
F.检测和合并曼哈顿世界
- 通过对平行线段分组聚类检测三个主方向
- 在开始检测曼哈顿世界时,垂直方向的线段已经确定(如B描述(由IMU重力方向确定)),世界坐标系下的XY平面的灭线可以被计算出来
- 之后,使用1-line RANSAC算法检测可能的曼哈顿世界:
- 1、 随机选取一条尚未被认定为结构线的线段,将其延伸至与水平灭线相交的灭点处,假设它可能是曼哈顿世界的X方向的投影。因为垂直方向已知,可以求得曼哈顿世界方向 ϕ \phi ϕ和 Y Y Y方向的灭点 v y v_y vy。
- 2、用和B部分描述的一致的方法,求出和两个灭点 v x v_x vx和 v y v_y vy一致的线段数目
- 3、重复上述两步直到最大迭代次数
- 最后,以线段数量最大的聚类作为可能的曼哈顿世界。进一步检查一致线段的数量是否大于阈值(本文为4),是否大于图像中现有结构线的数量(仅水平线)。
- 不是严格控制互相垂直才满足曼哈顿世界,允许有 Δ ϕ \Delta\phi Δϕ = 5度的偏离。(本文设置)。一旦曼哈顿世界建立,就将其加入到状态和协方差的更新中。
- 有时在一系列EKF更新后,两个不同的曼哈顿世界方向小于 Δ ϕ \Delta\phi Δϕ。在这种情况下,通过从状态中移除更新的曼哈顿世界来合并两个曼哈顿世界,并相应的调整协方差。固定在受影响的曼哈顿世界上的结构线也将被移动到另外一个世界。
G.移除外点
- 卡方检验外点
- χ 0 . 95 \chi_0.95 χ0.95对应95%的置信度。1、那些没有通过的EKF更新的结构线排除。在EKF更新后,重新三角化所有的结构线,并且检测所有可视帧的重投影误差。2、结构线条的重投影误差超过某一阈值就被抛弃(大于4个像素)。
疑问:
- 结构线存储,及结构线更新:在多帧连续下,同一位置不同线段对应同一结构线,怎么操作的?
- 多帧连续,结构线编号会固定。