PL-SVO: Semi-Direct Monocular Visual Odometry by Combining Points and Line Segments

IROS2016 西班牙马拉加大学
首先看系统流程图

主要分为跟踪线程和建图线程两大部分。

跟踪线程

1.基于稀疏模型的图像对齐

设定一个初始位姿$T_{k,k-1}$Tk,k−1​，且已经存在一些空间点线，然后把空间点线投影到当前帧，然后优化当前帧位姿$T_{k,k-1}$Tk,k−1​使得当前帧与参考帧的光度误差最小。关于线特征需要早端点pq之间等距采样多个图像块，表示如下
$\begin{aligned} \mathcal{L}:=\{& \mathbf{p}, \mathbf{q}, \mathbf{w}_{n} | \mathbf{p}, \mathbf{q} \in \mathcal{L}_{k-1} \\ & \wedge \boldsymbol{\pi}\left(\mathbf{T}(\boldsymbol{\xi}) \cdot \boldsymbol{\pi}^{-1}\left(\mathbf{p}, d_{\mathbf{p}}\right)\right) \in \boldsymbol{\Omega}_{k} \\ &\left.\wedge \boldsymbol{\pi}\left(\mathbf{T}(\boldsymbol{\xi}) \cdot \boldsymbol{\pi}^{-1}\left(\mathbf{q}, d_{\mathbf{q}}\right)\right) \in \boldsymbol{\Omega}_{k}\right\} \end{aligned}$L:={​p,q,wn​∣p,q∈Lk−1​∧π(T(ξ)⋅π−1(p,dp​))∈Ωk​∧π(T(ξ)⋅π−1(q,dq​))∈Ωk​}​
然后对于线特征的光度误差优化函数如下式所示
$\delta I_{l}(\boldsymbol{\xi}, \mathbf{l})=\frac{1}{N_{l}} \sum_{n=0}^{N_{l}}\left|I_{k}\left(\boldsymbol{\pi}\left(\mathbf{T}(\boldsymbol{\xi}) \cdot \mathbf{w}_{n}\right)\right)-I_{k-1}\left(\mathbf{w}_{n}\right)\right|$δIl​(ξ,l)=Nl​1​n=0∑Nl​​∣Ik​(π(T(ξ)⋅wn​))−Ik−1​(wn​)∣
最后得到总的光度误差优化函数如下
$\boldsymbol{\xi}_{k-1, k}^{*}=\underset{\boldsymbol{\xi}}{\operatorname{argmin}}\left\{\sum_{i \in \mathcal{P}}\left\|\delta I_{p}\left(\boldsymbol{\xi}, \mathbf{x}_{i}\right)\right\|^{2}+\sum_{j \in \mathcal{L}}\left\|\delta I_{l}\left(\boldsymbol{\xi}, \mathbf{l}_{j}\right)\right\|^{2}\right\}$ξk−1,k∗​=ξargmin​⎩⎨⎧​i∈P∑​∥δIp​(ξ,xi​)∥2+j∈L∑​∥δIl​(ξ,lj​)∥2⎭⎬⎫​
线特征误差对位姿的雅克比计算可以按成采样点光度误差对位姿的雅克比来求和得到，如下所示
$\left.\frac{\partial \delta I_{l}\left(\boldsymbol{\xi}, \mathbf{l}_{j}\right)}{\partial \boldsymbol{\xi}}\right|_{\boldsymbol{\xi}=\mathbf{0}}=\left.\frac{1}{N_{l}} \sum_{m=0}^{N_{l}} \frac{\partial \delta I_{p}\left(\boldsymbol{\xi}, \mathbf{w}_{n}\right)}{\partial \boldsymbol{\xi}}\right|_{\boldsymbol{\xi}=\mathbf{0}}$∂ξ∂δIl​(ξ,lj​)​∣∣∣∣​ξ=0​=Nl​1​m=0∑Nl​​∂ξ∂δIp​(ξ,wn​)​∣∣∣∣∣​ξ=0​
有了误差的定义，有了雅克比矩帧那么就开始优化求解了，文中使用的高斯牛顿法。
2.特征对齐

使用Lucas-Kanade光流跟踪算法利用前面多个参考帧的约束关系来对当前帧的图像块的像素位置进行修正，这里的误差函数定义如下
$\mathbf{w}_{j}^{\prime}=\underset{\mathbf{w}_{j}^{\prime}}{\operatorname{argmin}}\left\|I_{k}\left(\mathbf{w}_{j}^{\prime}\right)-I_{r}\left(\mathbf{A}_{j} \cdot \mathbf{w}_{j}\right)\right\|^{2}, \forall j$wj′​=wj′​argmin​∥∥​Ik​(wj′​)−Ir​(Aj​⋅wj​)∥∥​2,∀j
这里误差的定义虽然也是光度误差，但是这里和上面的图像对齐算法不同的是LK算法仅在2D的像素块上进行操作，因为不涉及到优化相机位姿，因此不会进行投影操作，而只是根据上一步的图像块位置作为初始位姿然后进行优化求精。
3.位姿和空间点线优化

上面两步，第一步获取了当前帧的初始位姿，第二步对当前帧的图像块位置进行了优化求精，然后第三步就是利用重投影误差来对共视窗口内对应的空间点线和当前帧位姿进行优化。下面是整体的优化函数
$\begin{aligned} \xi_{k, w}^{*}=\underset{\xi}{\operatorname{argmin}} &\left\{\sum_{i \in \mathcal{P}}\left\|r_{p}\left(\mathbf{T}_{k, w}, \mathbf{X}_{i, k}\right)\right\|^{2}\right.\\ &\left.+\sum_{j \in \mathcal{L}}\left\|r_{l}\left(\mathbf{T}_{k, w}, \mathbf{P}_{j, k}, \mathbf{Q}_{j, k}, \mathbf{l}_{j}\right)\right\|^{2}\right\} \end{aligned}$ξk,w∗​=ξargmin​​{i∈P∑​∥rp​(Tk,w​,Xi,k​)∥2+j∈L∑​∥rl​(Tk,w​,Pj,k​,Qj,k​,lj​)∥2⎭⎬⎫​​
这里线段的投影误差，使用线段端点到直线的距离来定义，如下所示
$r_{l}\left(T_{k, w}, \mathbf{P}_{j, k}, \mathbf{Q}_{j, k}, 1_{j}\right)=\left[\begin{array}{c}{1_{j} \cdot \boldsymbol{\pi}\left(\mathbf{T}_{k, w} \cdot \mathbf{P}_{j, k}\right)} \\ {1_{j} \cdot \boldsymbol{\pi}\left(\mathbf{T}_{k, w} \cdot \mathbf{Q}_{j, k}\right)}\end{array}\right]$rl​(Tk,w​,Pj,k​,Qj,k​,1j​)=[1j​⋅π(Tk,w​⋅Pj,k​)1j​⋅π(Tk,w​⋅Qj,k​)​]
在优化的过程更加鲁棒，使用了柯西鲁棒核函数，且优化过程使用高斯-牛顿法进行迭代优化。
$\rho(s)=\log (1+s)$ρ(s)=log(1+s)

建图线程

这一块儿主要进行关键帧的筛选和地图点逆深度滤波。论文没细讲参照SVO即可。