美团无人机团队ICRA 2022最佳论文奖，该研究不仅可使机器人在行进时的功耗大幅降低，更能提升计算的效率和准确度。

在视觉里程计领域，目前主要有两大类方法，即基于特征的间接方法和直接方法。长期以来，基于特征的间接计算方法一直主导着这一领域。不过最近，许多研究发现，直接方法表现出更高的精度和鲁棒性，即使是在对于间接方法来说很有难度的低纹理场景中也是如此。本文介绍的，就是一种使用点和线的效率更高的直接 VO 算法。

视觉里程计的直接方法中所采用的，通常是梯度足够大的像素点，包括线上的角点和点。在机器人工作的许多场景中，线上的点的数量是超过角点的数量的。而按照目前的算法，通过光流来跟踪角点虽然容易，但对于线上的点的跟踪却仍有困难，这主要是由于线上存在的一维模糊性造成的。如果直接放弃线上点的共线约束，则将导致对于深度的估计不够确切。

而在此前的许多研究工作中，这些线上点的共线约束问题不是像下图所示的那样被直接忽略，就是将这一问题转移给最终的优化系统来处理并给系统带来繁重的计算负担。

提出了一种新的方法来将 3D 共线点参数化，进而将此前仅仅针对点的光度误差扩展到了线。具体来说，该研究团队证明在一条 2D 线上的任意一个 3D 点，都可以由 2D 线的两个端点的逆深度来决定。这一方法不仅意味着可以显著减少变量的数量，而且可以在优化过程中更精确地满足共线约束，进而可以提高精度。

普吕克坐标系 Plucker Coordinates

令\(\pi\)表示由直线\(L\)和坐标原点组成的平面，\(\pi_{l}\)表示原点到直线的距离。则，直线\(L\)的普吕克坐标表示为一个六维向量.

\[ \mathbf{L} = [\mathbf{m};\mathbf{d}] \quad s.t. ||\mathbf{d}||_2 = 1, \mathbf{d}^T \mathbf{m} = 0 \]

其中\(\mathbf{d}\)是直线的方向，\(\mathbf{m}\)是垂直平面的向量且\(||\mathbf{m}||_2 = \pi_l\)。这里\(||\mathbf{d}||_2 = 1\)方便计算。

给出两个点\(\mathbf{p}_1, \mathbf{p}_2\),可以表示普吕克坐标系下的直线：

\[ \mathbf{L} = [\mathbf{m}; \mathbf{d}], \mathbf{m} = \mathbf{p}_1 \times \mathbf{d}, \mathbf{d} = \frac{\mathbf{p}_2 - \mathbf{p}_1}{ || \mathbf{p}_2 - \mathbf{p}_1 ||_2} \]

使用普吕克坐标系我们可以写车点\(\mathbf{x}\)到线\(\mathbf{L}\)的距离向量：

\[ e(\mathbf{L}, \mathbf{X}) = \mathbf{m} - \mathbf{X} \times \mathbf{d} \]

给出旋转平移\(\mathbf{R},\mathbf{t}\)可以得到普吕克变换矩阵：

\[ h(\mathbf{R},\mathbf{t}) = \begin{bmatrix} \mathbf{R} & [\mathbf{t}]_{\times} \mathbf{R} \\ 0 & \mathbf{R} \end{bmatrix} \]

并且可以对普吕克坐标系下直线做变换：

\[ \mathbf{L}^{'} = H(\mathbf{R}, \mathbf{t}) \mathbf{L} \]

光度误差 Photometric Error

对于reference image\(I_i\)和target image\(I_j\), 点\(\mathbf{x}\)在图像\(I_i\)中，并且具有逆深度\(d\),假设\(\mathbf{x}\)也被图像\(I_j\)观测到，在两者之间的关系为：

\[ \boldsymbol{x}^{\prime}=\Pi_{c}\left(\mathbf{R}_{i j} \Pi_{c}^{-1}(\boldsymbol{x}, d)+\mathbf{t}_{i j}\right) \]

其中\(\Pi_{c}\)和\(\Pi_{c}^{-1}\)表示投影和逆投影。

由此可定义光度误差为：

\[ E_{\boldsymbol{x} j}=\sum_{\boldsymbol{x} \in \mathbb{N}_{\boldsymbol{x}}} w_{\boldsymbol{x}}\left\|\left(I_{j}\left[\boldsymbol{x}^{\prime}\right]-b_{j}\right)-\frac{t_{j} e^{a_{j}}}{t_{i} e^{a_{i}}}\left(I_{i}[\boldsymbol{x}]-b_{i}\right)\right\|_{\gamma} \]

其中\(t_i, t_j\)为曝光时间，\(a_i, a_j, b_i, b_j\)为光度变换参数，\(w_x\)为权重，\(\mathbb{N}_x\)为\(\mathbf{x}\)领域内点集合，\(||.||_{\gamma}\)为huber鲁棒核函数。

---

Reference

[1] DPLVO: Direct Point-Line Monocular Visual Odometry [2] EDPLVO: Efficient Direct Point-Line Visual Odometry