Monocular Visual SLAM with Points and Lines for Ground Robots in Particular Scenes: Parameterization for Lines on Ground

Abstract

Visual simultaneous localization and mapping (V-SLAM) has attracted a lot of attention lately from the robotics communities due to its vast applications and importance. This paper addresses the problem of V-SLAM with points and lines in particular scenes where there are many lines on an approximately planar ground. All the lines in these particular scenes are treated as 3D lines with four degree-of-freedom (DoF) in most V-SLAM systems with lines. However, lines on ground only have two DoF. The redundant parameters will increase the estimation uncertainty of lines on ground. In order to restrict the lines on ground to the correct solution space, we propose two parameterization methods for it. The first method still treats lines on ground as 3D lines, and then we propose a planar constraint for the representation of 3D lines to loosely constrain the lines to the ground plane. Further, to strictly constrain the lines on ground to the ground plane, the second method treats these lines as 2D lines in a plane, and then we propose the corresponding parameterization method and geometric computation method from initialization to bundle adjustment. After that, to better exploit lines on ground during localization and mapping by using the proposed parameterization methods, we propose the graph optimization-based monocular V-SLAM system with points and lines to deal with lines on ground differently from general 3D lines. Assisted by wheel encoders, the proposed system generates a structural map. We perform experiments on both simulated data and real-world data to demonstrate that the proposed two parameterization methods can better exploit lines on ground than 3D line parameterization method that is used to represent the lines on ground in the state-of-the-art V-SLAM works with lines.

Appendix: Preintegrated Wheel Odometer Measurements

Each wheel encoder measures the traveled displacement ${\Delta } \tilde {d}_{k}$ of wheel between consecutive time-steps k − 1 and k at time-step k, which is assumed to be affected by a discrete-time zero-mean Gaussian noise η_w with varaince σ_w:

$$ {\Delta} \tilde{d}_{l_{k}} = {\Delta} d_{l_{k}} + \eta_{w_{l}} , \ \ {\Delta} \tilde{d}_{r_{k}} = {\Delta} d_{r_{k}} + \eta_{w_{r}} $$

(41)

where subscript $\left (\cdot \right )_{l}$ and $\left (\cdot \right )_{r}$ represent the left and right wheel respectively.

We assume that robot undergos the planar motion between consecutive wheel encoder readings. Based on the circular motion constraint of each wheel, the relative rotation vector and translation between two consecutive wheel frames {O_k− 1} and {O_k} measured by wheel encoders are:

$$ \begin{array}{ll} \tilde{\boldsymbol{\theta}}^{O_{k-1}}_{O_{k}} &= \left[ \begin{array}{c} 0 \\ 0 \\ {\Delta} \tilde{\theta}_{k} \end{array} \right] = \boldsymbol{\theta}^{O_{k-1}}_{O_{k}} + \boldsymbol{\eta}_{\theta_{k}}\\ \tilde{\mathbf{p}}^{O_{k-1}}_{O_{k}} &= \left[ \begin{array}{c} {\Delta} \tilde{d}_{k} \cos \frac{\Delta \tilde{\theta}_{k}}{2} \\ {\Delta} \tilde{d}_{k} \sin \frac{\Delta \tilde{\theta}_{k}}{2} \\ 0 \end{array} \right] = \mathbf{p}^{O_{k-1}}_{O_{k}} + \boldsymbol{\eta}_{p_{k}} \end{array} $$

(42)

where ${\Delta } \tilde {\theta }_{k} = \frac {\Delta \tilde {d}_{r_{k}} - {\Delta } \tilde {d}_{l_{k}}}{b}$ and ${\Delta } \tilde {d}_{k} = \frac {\Delta \tilde {d}_{r_{k}} + {\Delta } \tilde {d}_{l_{k}}}{2}$ are the rotation angle measurement and traveled distance measurement, b is the baseline length of wheels.

We define the transformation increment between non-consecutive frames i and j in wheel frame {O_i} as:

$$ \begin{array}{ll} \boldsymbol{\Delta} \mathbf{R}_{ij} &= \prod\limits_{k=i+1}^{j} \text{Exp}\left( \boldsymbol{\theta}^{O_{k-1}}_{O_{k}} \right) \\ \boldsymbol{\Delta} \mathbf{p}_{ij} &= \sum\limits_{k=i+1}^{j} \boldsymbol{\Delta} \mathbf{R}_{ik-1} \mathbf{p}^{O_{k-1}}_{O_{k}} \end{array} $$

(43)

From Eq. 43, we can obtain the preintegrated wheel odometer measurements as:

$$ \begin{array}{ll} \boldsymbol{\Delta} \tilde{\mathbf{R}}_{ij} &= \prod\limits_{k=i+1}^{j} \!\text{Exp}\left( \tilde{\boldsymbol{\theta}}^{O_{k-1}}_{O_{k}} \right) \\ \boldsymbol{\Delta} \tilde{\mathbf{p}}_{ij} &= \sum\limits_{k=i+1}^{j} \boldsymbol{\Delta} \tilde{\mathbf{R}}_{ik-1} \tilde{\mathbf{p}}^{O_{k-1}}_{O_{k}} \end{array} $$

(44)

Then, we can obtain the iterative propagation of the preintegrated measurements noise in matrix form as:

$$ \begin{array}{ll} &\left[ \! \begin{array}{c} \boldsymbol{\delta} \boldsymbol{\xi}_{ik+1} \\ \boldsymbol{\delta} \boldsymbol{p}_{ik+1} \end{array} \! \right] = \left[ \! \begin{array}{cc} \boldsymbol{\Delta} \tilde{\mathbf{R}}_{kk+1}^{\text{T}} \! & \! \mathbf{0}_{3 \times 3} \\ -\boldsymbol{\Delta} \tilde{\mathbf{R}}_{ik} \left[ \tilde{\mathbf{p}}^{O_{k}}_{O_{k+1}} \right]_{\times} \! & \! \mathbf{I}_{3 \times 3} \end{array} \! \right] \left[ \! \begin{array}{c} \boldsymbol{\delta} \boldsymbol{\xi}_{ik} \\ \boldsymbol{\delta} \boldsymbol{p}_{ik} \end{array} \! \right] \\ &+ \left[ \! \begin{array}{cc} \mathbf{J}_{r_{k+1}} \! & \! \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} \! & \! \boldsymbol{\Delta} \tilde{\mathbf{R}}_{ik} \end{array} \! \right] \left[ \! \begin{array}{c} \boldsymbol{\eta}_{\theta_{k+1}} \\ \boldsymbol{\eta}_{p_{k+1}} \end{array} \! \right] = \mathbf{A}_{k} \mathbf{n}_{ik} + \mathbf{B}_{k} \boldsymbol{\eta}_{k+1} \end{array} $$

(45)

Therefore, given the covariance $\boldsymbol {\Sigma }_{\eta _{k+1}} \in \mathbb {R}^{6 \times 6}$ of the measurements noise η_k+ 1, we can compute the covariance of the preintegrated wheel odometer meausrements noise iteratively:

$$ \boldsymbol{\Sigma}_{O_{ik+1}} = \mathbf{A}_{k} \boldsymbol{\Sigma}_{O_{ik}} \mathbf{A}_{k}^{\text{T}} + \mathbf{B}_{k} \boldsymbol{\Sigma}_{\eta_{k+1}} \mathbf{B}_{k}^{\text{T}} $$

(46)

with initial condition $\boldsymbol {\Sigma }_{O_{ii}} = \mathbf {0}_{6 \times 6}$.