Direct Monocular Odometry Using Points and Lines
Abstract
大多数VO都用点: 特征匹配 / 基于像素intensity的直接法关联.
我们做了一种直接法算法结合了点和edge.
它在纹理少的场景下工作的更好,而且对于光线的变化和快速的运动的情况下通过扩大收敛域(convergence basin - 盆)的方式提升鲁棒性.
我们对于关键帧保持了depth map.
Tracking: 相机位姿是通过最小化广度误差和几何误差(匹配的edge)来恢复相机位姿.
Mapping: edge被用来提速和提高stereo matching的准确度.
Introduction
在单目VO中, edge受到的关注很少.
edge的检测对于自然光照的影响更不敏感.
如图1所示,使用点的直接法由于小图像地图可能根本不能鲁邦的工作. 但是我们依旧能够检测到很多线.
在我们的框架中,我们保持了关键帧的semi-depth map.
我们对每帧也检测了和匹配edge.
- 在tracking部分,我们同时优化光度误差和几何误差.
- 在mapping部分,edge也可以用来指导和加速stereo search, 同时也提升深度图的质量(by edge regularizing).
我们的contribution:
- 一个实时的单目VO用了点和edge.
- 提供了在tracking和mapping部分的点和edge 的不确定性分析.
- 研发了analytical edge based regularization.
- 在数据集中比现存的直接法VO厉害.
Related Work
A. Feature-based VO
...
B. Direct VO
...
C. Edge-based VO
除了点以外, edge也是人造场景中另一个很重要的特征.
在[14], [15] 中基于线的BA已经在SLAM / SfM中使用了, 但是很费计算力, 对于有效的优化也需要至少3帧.
在VO中用线但是没有BA也有人用了.
Kuse(一个人) 最小化结合误差到最近的edge pixel, 但是有可能会导致错误的匹配.
也有一些工作只优化两个edge端点的几何误差但是很有可能会产生更大的误差因为单目相机的不准确的深度估计.
Problem Description
A. System Overview
我们的算法是一个frame-to-keyframe的单目VO.
我们为关键帧的高梯度的像素保持一个深度图.
对于每一个新帧,我们会做三步:
- 检测line segments, 然后和关键帧的edge匹配
- camera pose tracking, 优化光度误差和几何误差.
- 通过variable baseline stereo, 更新深度图.
B. Notations
...
Tracking
A. Overview
Ref帧的深度图是假设不变的.
当前图像用光度误差\(r(\xi)\)和线的重投影误差\(g(\xi)\)来关联.
\[
E(\xi)=\sum_{i \in \Omega} r_{i}(\xi)^{T} \Sigma_{r_{i}}^{-1} r_{i}(\xi)+\sum_{j \in M} g_{j}(\xi)^{T} \Sigma_{g_{j}}^{-1} g_{j}(\xi)
\]
光度误差\(r_i\)的定义如下:
\[
r_{i}=I_{r e f}\left(x_{i}\right)-I\left(\tau\left(x_{i}, D_{r e f}\left(x_{i}\right), \xi\right)\right)
\]
\(g_j\)是像素\(x_i\)对于它对应的线\(l_j\)的重投影误差:
\[
g_{j}=l_{j}^{T} \widehat{\tau}\left(x_{i}, D_{r e f}\left(x_{i}\right), \xi\right)
\]
这里\[\widehat{\tau}(. )\]标识取homogeneous coordinate.
我们用Gauss-Newton优化能量函数.
B. Tracking Uncertainty Analysis
不同误差项的权重和误差Variance\(\Sigma_r和 \Sigma_g\)成比例.
我们分析下\(\Sigma_g\), 因为光度误差的不确定性在[6]里面分析过了.
一般情况下, 一个函数\(f(x)\)的输出的不确定性是从输入的不确定性传播的:
\[
\Sigma_{f} \approx J_{f} \Sigma_{x} J_{f}^{T}
\]
line equation是用两个线端点叉乘计算的: \(l_j = p_1 \times p_2.\)
我们可以假设\(p_1\)和\(p_2\)的不确定性$ \Sigma_p\(是一个bi-dimensional Gaussian (\)\sigma = 1$).
最后的重投影误差是两个不确定性源的结合:
\[
\Sigma_{g_{j}}=l_{j}^{T} \Sigma_{x_{i}^{\prime}} l_{j}+x_{i}^{\prime T} \Sigma_{l_{j}} x_{i}^{\prime}
\]
Mapping
A. Overview
深度图\(D_{ref}\)会用逆深度滤波框架的stereo triangulation来更新, 再用line regularization来提升精度.
深度优化的能量函数如下:
\[
E(D)=\sum_{i} r_{i}(d)^{T} \Sigma_{r_{i}}^{-1} r_{i}(d)+\sum_{j} G_{j}(d)^{T} \Sigma_{G_{j}}^{-1} G_{j}(d)
\]
这里\(r_i\)表示stereo匹配的光度误差. 计算整个图像patch的SSD用来提高鲁棒性.
对于一根线\(l_j\), 我们希望它的像素也可以在反投影之后在3D空间形成一条线.所以\(G_j\)是一个edge regularization cost, 代表edge像素的3D点和3D线的距离. 这种regularization技术也在其他稠密建图的算法中使用[12], [21].
如果只使用了第一个项\(r_i\),那么所有的像素都是相互独立的, 只是在极线上搜索来寻找匹配的像素.
Regularization项\(G_j\)使得一条edge上的深度是相互关联的, 然后可以经典的被Duality principles的iterative alternating optimziation来解决.
B. Stereo match with Lines
对于没有在edge上的像素,或者是在edge上但是没有匹配edge的像素, 我们用穷尽法来寻找立体匹配像素(by 最小化SSD误差). 深度的搜索被限制在\(d + 2\sigma_d\).
对于有匹配edge的像素, 点的重投影应该在匹配的edge上, 也应该在它的极线上. 所以我们可以直接计算他们的交点.
我们也可以如上图一样直接三角化线,然后计算所有像素的深度. 3D线可以被表示为两个反投影平面的交线.
\[
L=\left[\begin{array}{c}{\pi_{1}^{T}} \\ {\pi_{2}^{T}}\end{array}\right]=\left[\begin{array}{cc}{l_{1}^{T} K} & {0} \\ {l_{2}^{T} K R} & {l_{2}^{T} K \mathbf{t}}\end{array}\right]
\]
这里\(l_1\)和\(l_2\)是在I帧和I'帧的line equation. \(K\)是相机内参矩阵.
对于退化的场景, 也就是说极限和匹配的线几乎平行, 我们不能通过平面的intersection来获取深度了. 这样就只能通过穷举法来在极线上搜索来获得了.
C. Line matching uncertainty analysis
基于立体视觉在极线上的搜索来计算intensity的不确定性在[6]中已经分析过了.
这里我们分析一下基于stereo matching的edge的误差.
对于\(I_{ref}\)的每一个edge像素, 把它在图像\(I\)上的极线叫做\(g\), 它对应的匹配线叫\(l\), 这样这个像素的匹配点是\(g\)和\(l\)的相交.
这两根线都有positioning error \(\epsilon_l\)和\(\epsilon_g\), 最后会导致图下所示的disparity error \(\epsilon_\lambda\).
当\(g\)和\(l\)接近平行的时候, \(\epsilon_\lambda\)会很大:
\[
\epsilon_{\lambda}=\epsilon_{l} / \sin (\theta)+\epsilon_{g} \cot (\theta)
\]
这里\(\theta\)标识\(g\)和\(l\)的夹角.
根据误差传播的原理, 我们可以计算disparity error的variance:
\[
\sigma_{\lambda}^{2}=\sigma_{l}^{2} / \sin ^{2}(\theta)+\sigma_{g}^{2} \cot ^{2}(\theta)
\]
利用逆深度\(d\)和disparity\(\lambda\)成比例的近似, 我们可以计算观测\(d\)的观测方差. 它在标准的EKF滤波中可以用来更新像素的深度方差.
D. 3D line regularization
在深度图EKF更新之后, 2D edge上的像素可能不能对应于3D空间中的一条线, 所以我们需要在3D空间中拟合一条线来更新像素深度.
- 3D weighed line fitting最近在RGB-D line based odometry中被解决了.
这里我们提出一种快速而analytical solution来解决weighted 3D line ftting problem.
由于3D点是从同一条2Dedge反投影的,他们应该在同一个投影平面上\(G\).
我们可以创建另一个坐标系\(F\), 这个坐标系的\(x, y\)轴在这个平面\(G\)上,
我们首先用RANSAC在选择一组内点2D点, the metric for RANSAC is Mahalanobis distance, 是一个考虑不确定性的带权重的像素到线的欧式距离.
\[
d_{\mathrm{mah}}=\min _{q^{\prime} \in l^{\prime}}\left(p^{\prime}-q^{\prime}\right)^{T} \Sigma_{p^{\prime}}^{-1}\left(p^{\prime}-q^{\prime}\right)
\]
这里\(q'\in l'\)表明在帧\(F\)中在线\(l'\)上的.
在RANSAC以后, 我们可以找到largetest consensus set的点集\(p_i' , i = 1, ...n\). 这样的话,这就变成了一个2D weighted Line fitting problem.
\[
L^{*}=\min _{L} \sum_{i} \delta\left(p_{i}^{\prime}\right)^{T} \Sigma_{p_{i}^{\prime}}^{-1} \delta\left(p_{i}^{\prime}\right)
\]
这里\(\delta(p_i')\)是点\(p_i'\)到线\(L\)沿着\(y\)轴的距离. 这是一个点到线距离的近似, 但是应该会导致一个差不多的结果.
把所有的点\(p'_i\)堆在一起,以\([X, Y]\)(在去平均以后)的形式,然后权重矩阵\(W\)就可以近似原图像像素的协方差了.
然后, 这个线模型是\(Y= X\beta + \epsilon\), 这里 \(\beta\)是线系数，\(\epsilon\)是正态分布的噪声.
那么MLE的公式就是:
\[
\widehat{\beta}=\arg \min _{\beta} \sum_{i} \epsilon_{i}^{2}=\left(\mathbf{X}^{T} W \mathbf{X}\right)^{-1} \mathbf{X}^{T} W \mathbf{Y}
\]
Experiments and Results
A. Implementation
边缘检测和匹配: 我们用了[23]的LSD. 为了提高tracking的准确性, 我们使用了coarse-to-fine的方法 - 用尺度系数为2的金字塔. 由于线检测算法的不确定性,
一个完整的线有时候会断成几截. 所以我们显式的融合距离和角度都很接近的线.
我们会移除很短的线, 因为可能会导致特别大的拟合误差*
我们会计算LBD描述子[24]然后尝试匹配线.
最后, 线的追踪会寻找edge上所有的像素. 我们发现下面这个方法让系统更加鲁邦和精准, 如果我们把线变宽了一个像素, 更多的像素就会被involved作为线的约束
关键帧VO: 我们的方案里面灭有用点和线的BA.
B. Experiments
Conclusions
没啥新的.
Appendix
[6] Jakob Engel, Jurgen Sturm, and Daniel Cremers. Semi-dense visual odometry for a monocular camera. In Proceedings of the IEEE international conference on computer vision, pages 1449–1456, 2013.
[12] Richard A Newcombe, Steven J Lovegrove, and Andrew J Davison.Dtam: Dense tracking and mapping in real-time. In 2011 international conference on computer vision, pages 2320–2327. IEEE, 2011.
[21] Pedro Pinies, Lina Maria Paz, and Paul Newman. Dense mono reconstruction: Living with the pain of the plain plane. In 2015 IEEE International Conference on Robotics and Automation (ICRA), pages 5226–5231. IEEE, 2015.
[23] von Gioi R Grompone, Jeremie Jakubowicz, Jean-Michel Morel, and Gregory Randall. Lsd: a fast line segment detector with a false detection control. IEEE transactions on pattern analysis and machine intelligence, 32(4):722–732, 2010.