Robust Stereo Visual Odometry through a Probabilistic Combination of Points and Line Segments

Abstract

本文介绍了一种考虑传感器输入中高斯噪声的视觉里程计，它结合特征点和特征线，提升了系统的鲁棒性和在较低语义场景下的稳定、可靠性。

Introduction

介绍了一些之前的案例，本文中主要的创新点建立在考虑到了传感器采集数据时候存在的高斯噪声，这样可以直接和存在高斯噪声的系统进行融合，通过最小化加权代价函数的方式求解相对位姿变换。

System Overview

通过提取匹配帧内的特征信息，估计帧之间的位姿变换信息和相应的不确定性。
1）Point Features
特征点的提取利用高效的$ORB$实现，通过帧之间最优的特征描述信息匹配实现特征点之间的匹配操作，这里考虑到特征信息匹配的准确性，保证最优和次优的特征描述信息之间有足够的差异性，通过为了保证特征点的均匀分布将原图信息分为16个方格，每个方格中至少保证有20个特征信息（这个操作和ORB_SLAM里的$cell$操作很类似）。
2）Line Segment Features
线性特征通过$LSD$进行提取，通过$LBD$进行匹配，通过比较最优和次优描述符距离踢除可靠性不够的匹配。线性特征没有构建方格保证特征的均匀分布，效果会变差。
3）Motion Estimation
位姿估计通过高斯牛顿最小化特征点以及特征线端点之间的重投影误差来估算位姿变换，通过利用核函数来避免无匹配带来的影响，踢除无匹配特征信息。
4）Uncertainty Propagation
为了改善位姿估计的精度，本文将零均值的高斯分布噪声通过协方差矩阵的形式以加权求和的形式和代价函数相结合，这样做的目的能使得该视觉里程计更好的其他存在高斯随机分布的系统相结合。

Combined Stereo Visual Odometry

通过将前帧中特征点对应的3D信息重投影到后帧中对应匹配的特征点位置附近，通过最小化重投影误差信息来估算最优的位姿变换信息。
A.Problem Statement
通过估算前一帧后后一帧之间的未知子变换信息实现位姿变换信息的估计（这里一般会用左侧视图信息进行位姿估计）。将匹配的特征点以及匹配的特征线以及传感器检测过程中存在的高斯噪声共同构建的代价函数最小化，获取的位姿变换信息就是我们的目标。针对特征点对应的重投影误差方程为$$\Delta p_i(\xi )={\hat{p}}_i(\xi )-p_i^{\prime }$$其中，$\xi$表示6维的位姿变换信息，${\hat{p}}_i(\xi )$表示投影到后一帧中的特征点投影坐标信息，$p_i^{\prime }$表示后一帧和前一帧中匹配的特征坐标信息。
对应的特征线的误差函数为$$\Delta l_j(\xi )=[l_j^{\prime T}\cdot [{\hat{p}}_j(\xi )\times {\hat{q}}_j(\xi )]]$$
其中${\hat{p}}_j(\xi ),{\hat{q}}_j(\xi )$分别表示从前一帧中特征线的端点在后一帧中的投影坐标，$l_j^{\prime }(\xi )$表示后一帧中匹配的特征线对应的无限线（注重方向而忽律长度）。
在进行线性特征投影误差函数构建的时候，大多还是通过端点信息进行构建，例如MEVO论文中是将投影端点与右侧视图中对应的特征线之间的距离作为误差函数。
B.On-Manifold Optimization
将俩种特征信息对应的代价函数结合并考虑到高斯噪声问题，于是将问题转换为最小化以下的代价函数 ξ ∗ = a r g m i n ξ { ∑ i N p Δ p i ( ξ ) T Σ Δ p i − 1 Δ p i ( ξ ) + ∑ j N l Δ l j ( ξ ) T Σ Δ l i − 1 Δ l j ( ξ } \xi^*=argmin_\xi\{\sum^{N_p}_i\Delta p_i(\xi)^T\Sigma^{-1}_{\Delta p_i} \Delta p_i(\xi)+\sum^{N_l}_j\Delta l_j(\xi)^T\Sigma^{-1}_{\Delta l_i} \Delta l_j(\xi\} ξ∗=argminξ​{i∑Np​​Δpi​(ξ)TΣΔpi​−1​Δpi​(ξ)+j∑Nl​​Δlj​(ξ)TΣΔli​−1​Δlj​(ξ}其中，${\Sigma }_{\Delta p_i}^{-1},{\Sigma }_{\Delta l_j}^{-1}$分别表示对应特征的协方差矩阵的导数，用于将不确性因素加权到代价函数中。
对应该代价函数的雅可比矩阵为$$J(\xi )=\frac{\partial E(\xi ⨁\epsilon )}{\partial \epsilon }$$其中，$⨁$用于表示位姿信息之间的李代数加法的表示形式。
C.Fast Outlier Rejection
通过使用阈值函数$$\rho (s)=log(1+s)$$来将不合适的匹配踢除，其中$s$对应的代价函数值。

Uncertainty of the error functions

针对上述在代价函数中使用到的权重矩阵，对应的是不同匹配之间的不确定性程度矩阵，理论上不确定越大就需要更大的权重用于增加迭代步长信息。于是对应的来个权重矩阵（协方差矩阵）通过求解各自代价函数的雅可比矩阵获取$${\Sigma }_{\Delta k_i}\approx \frac{\partial \Delta k_i}{\partial x}{\Sigma }_x{\frac{\partial \Delta k_i}{\partial x}}^T$$其中， k = { p , l } k=\{p,l\} k={p,l}分别表示不同的特征信息，${\Sigma }_x$表示用单位矩阵表示。
于是将整个位姿信息的估计函数的不确定性利用总体的代价函数进行表示，忽律$Hessian$函数的二次项，可以通过雅可比矩阵进行求解$${\Sigma }_{{\xi }^{\ast }}\approx (J({\xi }^{\ast }{)}^{T}W({\xi }^{\ast })J({\xi }^{\ast }){)}^{-1}$$其中，$J({\xi }^{\ast })$对应的是总体误差函数对应的雅可比矩阵信息，$W({\xi }^{\ast })$对应的是每种特征信息对应的投影不确定性矩阵。于是整个位姿估计系统对应的不确定性可以表示为$$\xi \backsim N({\xi }^{\ast },{\Sigma }_{{\xi }^{\ast }})$$
A.Detecting ill-Pose Configurations
对于一些在优化之前无法发现的无匹配问题，可以在获取整个系统对应的不确定性矩阵之后将其转换为对角阵形式，对应的最大的对角值对应的运动估计需要踢除。

Experimental Validation

Conclusions

提出一种考虑高斯噪声的多类型特征的视觉里程计，可以结合各种系统直接使用。