Point-line-based RGB-D SLAM and Bundle Adjustment Uncertainty Analysis

Many famous visual odometry and SLAM works have been developed based on point features, for example PTAM and ORB-SLAM. In these works, keypoints are extracted and exploited for pose estimation and mapping with different descriptors. Then the keypoints in different frames are matched according to their descriptor distances. Based on the feature correspondences, a sparse 3D map is built so that the subsequent camera poses are estimated by solving the PnP problems. Methods based on monocular cameras inevitably encounter the problem of scale drift due to depth ambiguity. In contrast, RGB-D and stereo cameras can easily avoid this issue [12], but stereo camera methods cost more time due to the feature matching between left and right images. In terms of robustness, low-textured scenes are considered as a main challenge for indirect methods.

Compared with feature-based methods, direct approaches [13] directly deal with the raw pixel intensities, which can significantly save the time of feature extraction and matching. Direct methods have been performed for different sensors, such as LSD-SLAM for monocular cameras and DVO [14] for RGB-D cameras. High computational efficiency is one of the advantages of direct methods. For example, as a typical semi-dense method, SVO [1] can run with hundreds of frames per second. The core of these methods is the minimization of the photometric errors for pixels of a certain size, so they can operate well in some texture-less scenes with few point features.

Considering that the detection of line features is less sensitive to lighting variations and the accuracy of line-based methods is usually not comparable with that of point features, combinations of point and line features have been proposed for VO/SLAM methods recently. For feature-based methods, lines are usually treated similarly to points, that is, the line features are detected first and then the corresponding descriptors are computed for feature matching [9]. The work [8] proposed a stereo point-line-based SLAM method with open source code, and lines are integrated into the process of loop closing. More recently, PL-VIO is presented in the work [15], which combines point and line features in a visual inertial system.

Besides the indirect methods, line features can also be integrated into direct methods. Based on the idea that the points on a line have large pixel gradient, DLGO [16] used a set of points in a line rather than two endpoints, which improved the performance of DSO.

In addition, the work [17] discussed the combined use of point and line features with several parameterizations for an EKF-SLAM system. Point-line-based methods have also been proposed for RGB-D cameras.

Lu et al. [18] proposed a RGB-D visual odometry and analyzed the uncertainty of motion estimation to show the benefit of fusing line features with point features. In terms of the relationship between features and the accuracy of pose estimation, the work in [19] has conducted experiments and shown that increasing the number of features leads to higher accuracy of a monocular SLAM system.

We define the variables to be optimized in the local bundle adjustment as follows:

which contains $m$ keyframe poses and $n$ landmark positions in the local map. Let the number of fixed keyframes in the local map be $d$, and the current keyframe be $C_{0}$. If the $j$-th landmark is observed in the $i$-th keyframe, the corresponding reprojection error $\bm{e}_{ij}(0\leq i\leq(m+d),1\leq j\leq n)$ can be computed from $\bm{\xi}_{i}$ and $\bm{P}_{j}$:

where $\bm{p}_{ij}$ is the observation of the landmark $\bm{P}_{j}$ on the $i$-th keyframe. Note that the reprojection error expression is different for different types of landmarks (e.g.,line landmarks). Next, we integrate all the reprojection errors in the local BA into an error function:

where the form of $h(\bm{x})$ varies with the observation relationships in the local map. In order to better illustrate the problem, we have shown a factor graph corresponding to the local BA in Fig. 3 as an example. The corresponding error function in Fig. 3 is $h_{1}(\bm{x})={[\bm{e}^{\top}_{01},\bm{e}^{\top}_{02},\bm{e}_{03}^{\top},\bm{e}_{12}^{\top},\bm{e}_{13}^{\top},\bm{e}_{14}^{\top},\bm{e}_{24}^{\top},\bm{e}_{34}^{\top}]}^{\top}$. For a more general representation, we define the error function as (8). According to the Maximum Likelihood Estimation (MLE), the core of local bundle adjustment is to solve the following least square problem:

where $\bm{\Sigma}_{h}=diag(\bm{\Sigma}_{01},\cdots,\bm{\Sigma}_{ij})$ and $\bm{\Sigma}_{ij}$ is the covariance of the observation $\bm{p}_{ij}$. Note that (9) is derived based on the Maximum a Posteriori (MAP) problem, which is equivalent to the Maximum Likelihood Estimation (MLE) in a typical visual SLAM problem. And observations of the landmarks need to satisfy independence assumptions.

Next, we define the following notations:

According to first order linear approximation and the back-propagation of covariance, the MLE of $\bm{x}$ has covariance [23]:

with $\bm{J}_{h}=\frac{\partial h}{\partial\bm{x}}=\begin{bmatrix}\frac{\partial h}{\partial\bm{x}_{c}}&\frac{\partial h}{\partial\bm{x}_{p}}\end{bmatrix}=\begin{bmatrix}\bm{J}_{A}&\bm{J}_{B}\end{bmatrix}$. We further derive $\bm{J}^{\top}_{h}\bm{\Sigma}^{-1}_{h}\bm{J}_{h}=\begin{bmatrix}\begin{array}[]{cc}\bm{J}^{\top}_{A}\bm{\Sigma}^{-1}_{h}\bm{J}_{A}&\bm{J}^{\top}_{A}\bm{\Sigma}^{-1}_{h}\bm{J}_{B}\\ \bm{J}^{\top}_{B}\bm{\Sigma}^{-1}_{h}\bm{J}_{A}&\bm{J}^{\top}_{B}\bm{\Sigma}^{-1}_{h}\bm{J}_{B}\end{array}\end{bmatrix}=\begin{bmatrix}\begin{array}[]{cc}\bm{H}_{A}&\bm{H}_{B}\\ \bm{H}_{B}^{\top}&\bm{H}_{C}\end{array}\end{bmatrix}$.

Generally, the accuracy of camera trajectory gets more attention than geometrical reconstruction (e.g. landmarks), especially for the indirect SLAM methods. As the first part of the state $\bm{x}$, the covariance of $\bm{x}_{c}$ corresponds to the upper left submatrix of $cov(\bm{x})$, with the size of $6m\times 6m$. Therefore, under Gaussian noise assumption, the MLE of $\bm{x}_{c}$ (multiple keyframe poses in the local map) has covariance:

If we solve the problem of local BA with $k$ new landmarks $\{\bm{P}^{\prime}_{1},\cdots,\bm{P}^{\prime}_{k}\}$ and the same keyframe states as mentioned in Section V-A, we can define these new landmarks as:

In this case, the variables to be optimized are written as:

Let the number of fixed keyframes be $d^{\prime}$ in this case. Define the error function as:

where $\bm{e^{\prime}}_{ij}(0\leq i\leq(m+d^{\prime}),1\leq j\leq k)$ represents the reprojection error corresponding to the $j$-th new landmark $\bm{P^{\prime}}_{j}$ in the $i$-th keyframe:

For example, the error function can be expressed as $f_{1}(\bm{x})={[\bm{e^{\prime}}^{\top}_{01},\bm{e^{\prime}}^{\top}_{11},\,\bm{e^{\prime}}^{\top}_{12},\bm{e^{\prime}}^{\top}_{22}]}^{\top}$ in Fig. 3.

The local BA solves the following least square problem:

where $\bm{\Sigma}_{f}=diag(\bm{\Sigma}^{\prime}_{01},\cdots,\bm{\Sigma}^{\prime}_{ij})$ and $\bm{\Sigma}^{\prime}_{ij}$ is the covariance of the observation $\bm{p}^{\prime}_{ij}$.

According to the back-propagation of covariance, the MLE of $\bm{x^{\prime}}$ has covariance:

where $\bm{J}_{f}=\frac{\partial f}{\partial\bm{x^{\prime}}}=\begin{bmatrix}\frac{\partial f}{\partial\bm{x}_{c}}&\frac{\partial f}{\partial\bm{x}_{p^{\prime}}}\end{bmatrix}=\begin{bmatrix}\bm{J}_{C}&\bm{J}_{D}\end{bmatrix}$. We further derive $\bm{J}^{\top}_{f}\bm{\Sigma}^{-1}_{f}\bm{J}_{f}=\begin{bmatrix}\begin{array}[]{cc}\bm{J}^{\top}_{C}\bm{\Sigma}^{-1}_{f}\bm{J}_{C}&\bm{J}^{\top}_{C}\bm{\Sigma}^{-1}_{f}\bm{J}_{D}\\ \bm{J}^{\top}_{D}\bm{\Sigma}^{-1}_{f}\bm{J}_{C}&\bm{J}^{\top}_{D}\bm{\Sigma}^{-1}_{f}\bm{J}_{D}\end{array}\end{bmatrix}=\begin{bmatrix}\begin{array}[]{cc}\bm{H}^{\prime}_{A}&\bm{H}^{\prime}_{B}\\ \bm{H}^{\prime\top}_{B}&\bm{H}^{\prime}_{C}\end{array}\end{bmatrix}$.

Therefore, in this case, under Gaussian noise assumption, the MLE of $\bm{x}_{c}$ has covariance:

If considering all the landmarks as mentioned in Section V-A and Section V-B, the states to be estimated in the local map is denoted as:

The error function can be expressed as:

Local BA solves the following problem:

where $\bm{\Sigma}_{g}=diag(\bm{\Sigma}_{01},\cdots,\bm{\Sigma}_{ij},\bm{\Sigma}^{\prime}_{01},\cdots,\bm{\Sigma}^{\prime}_{ij})$.

The MLE of $\bm{x^{\prime\prime}}$ has covariance:

where
$\bm{J}_{g}=\frac{\partial g}{\partial\bm{x^{\prime\prime}}}=\begin{bmatrix}\begin{array}[]{ccc}\frac{\partial h}{\partial\bm{x}_{c}}&\frac{\partial h}{\partial\bm{x}_{p}}&\bm{0}\\ \frac{\partial f}{\partial\bm{x}_{c}}&\bm{0}&\frac{\partial f}{\partial\bm{x}_{p^{\prime}}}\end{array}\end{bmatrix}=\begin{bmatrix}\begin{array}[]{ccc}\bm{J}_{A}&\bm{J}_{B}&\bm{0}\\ \bm{J}_{C}&\bm{0}&\bm{J}_{D}\end{array}\end{bmatrix}$. Next, we derive $\bm{J}^{\top}_{g}\bm{\Sigma}^{-1}_{g}\bm{J}_{g}=\begin{bmatrix}\begin{array}[]{c:cc}\bm{J}^{\top}_{A}\bm{\Sigma}^{-1}_{h}\bm{J}_{A}+\bm{J}^{\top}_{C}\bm{\Sigma}^{-1}_{f}\bm{J}_{C}&\bm{J}^{\top}_{A}\bm{\Sigma}^{-1}_{h}\bm{J}_{B}&\bm{J}^{\top}_{C}\bm{\Sigma}^{-1}_{f}\bm{J}_{D}\\ \hdashline\bm{J}^{\top}_{B}\bm{\Sigma}^{-1}_{h}\bm{J}_{A}&\bm{J}^{\top}_{B}\bm{\Sigma}^{-1}_{h}\bm{J}_{B}&\bm{0}\\ \bm{J}^{\top}_{D}\bm{\Sigma}^{-1}_{f}\bm{J}_{C}&\bm{0}&\bm{J}^{\top}_{D}\bm{\Sigma}^{-1}_{f}\bm{J}_{D}\end{array}\end{bmatrix}=\begin{bmatrix}\begin{array}[]{c:cc}\bm{H}_{A}+\bm{H}^{\prime}_{A}&\bm{H}_{B}&\bm{H}^{\prime}_{B}\\ \hdashline\bm{H}^{\top}_{B}&\bm{H}_{C}&\bm{0}\\ \bm{H}^{\prime\top}_{B}&\bm{0}&\bm{H}^{\prime}_{C}\end{array}\end{bmatrix}$.

Similarly, it can be deduced that, under Gaussian noise assumption, the MLE of $\bm{x}_{c}$ has covariance:

With (12), (19) and (24), the following relationship is derived:

If matrices $\bm{M}$ and $\bm{M}^{\prime}$ are real symmetric and $\bm{M}-\bm{M}^{\prime}$ is positive definite, we can define $\bm{M}\succ\bm{M}^{\prime}$. Hence, the covariance matrices $\bm{C}_{h}$ and $\bm{C}_{f}$ satisfy: $\bm{C}_{h}\succ\bm{0}$, $\bm{C}_{f}\succ\bm{0}$.

According to matrix theory, we have:

which means the $i$-th largest eigenvalue of $\bm{C}_{g}$ is smaller than the $i$-th largest eigenvalue of $\bm{C}_{h}$ and $\bm{C}_{f}$ [24].

According to (25), the following conclusions can be drawn: first, adding more new landmarks with their observations in the local BA leads to smaller uncertainty in the MLE of keyframe poses; second, the more accurate observations of the new landmarks, the smaller uncertainty in the MLE of keyframe poses, that is, the smaller $\bm{C}_{f}$, the smaller $\bm{C}_{g}$.

In our system, we sample $N$ points on a 2D line feature and back-project them to generate the map line landmarks. Each map line is represented by $N$ 3D points (named line guidance features). Let $\bm{\theta}$ be the states (including local keyframe poses and landmark positions) to be estimated in the local BA. The local BA aims to minimize the reprojection errors between the projections of the 3D landmarks and their corresponding 2D observations in the local keyframes:

where $\mathcal{K}$, $\mathcal{P}$ and $\mathcal{L}$ respectively refer to the sets of local keyframes, local map points, local map lines and $\bm{e}_{ik}=[\bm{e}^{\top}_{ik1},\cdots,\bm{e}^{\top}_{ikN}]^{\top}.$

The expression of reprojection error $\bm{e}_{ij}$ is similar to (7). The observation parameterization of an ORB feature $\bm{P}_{ij}$ is defined as $\bm{P}_{ij}=[u,v,u-\frac{f_{x}b}{d}]^{\top}$ based on original pixel measurement $\bm{p}_{uv}=[u,v]^{\top}$ and depth measurement $d$, where $b$ is the baseline of camera. Assuming pixel coordinates and $d$ satisfy the zero-mean Gaussian distribution with the standard deviation $\sigma_{p}$ and $\sigma_{d}$, the covariance of original measurements is:

where $\sigma_{p}$ is related to the extraction layer of ORB feature in the Gaussian pyramid, and $\sigma_{d}$ is modeled by a quadratic function of $d$ [25]. The covariance of $\bm{P}_{ij}$ is defined as:

where $\bm{J}_{ij}=\frac{\partial\bm{P}_{ij}}{\partial(\bm{p}_{uv},d)}=\begin{bmatrix}1&0&0\\ 0&1&0\\ 1&0&\frac{f_{x}b}{d^{2}}\end{bmatrix}$.

The reprojection error $\bm{e}_{iks}(s=1,\cdots,N)$ is obtained by the distance between the projected 2D position of 3D line-guidance feature $\bm{P}_{iks}$ and the corresponding straight line $\bm{l}_{ik}$ (i.e. observation) in the image plane:

Compared to point-based methods, our method is based on point and line guidance features, and expands the number of landmarks to be estimated in the local BA, which will result in smaller estimation uncertainty of keyframe poses according to the uncertainty analysis in this section. Line features are often at the edges of objects, such that the corresponding depth measurements are noisy. Considering the noisy depth measurements and the mismatch of line features, oversampling points on a line will introduce too many landmarks with poor initial values and have a bad impact on the optimization problem. A good SLAM algorithm should carefully balance the quantity and quality of landmarks in the bundle adjustment.

We first compare the proposed method against some state-of-the-art approaches, including ORB-SLAM2, DVO-SLAM [14] and L-SLAM [28]. Artificial noise was used in the image data to simulate realistic sensor noise in this dataset, meanwhile every sequence has a version without noise.

The evaluation results are shown in TABLE I for all the sequences with noise. The Absolute Trajectory Error (ATE) is used for measuring the root mean squared error (RMSE) between the estimated trajectory and ground truth. Though L-SLAM performs best in three noise sequences, it is limited to planar environments. Note that our method outperforms the other methods in half of all the noise sequences. We show the 788th frame (low-textured scene) from sequence lr-1 during the tracking process in Fig.4(a) and the estimated trajectories comparison between ORB-SLAM2 and Ours in Fig. 7. In addition, we also conduct experiments for ORB-SLAM2 and Ours in all the sequences without noise, as shown in TABLE I. According to the results of ORB-SLAM2 and Ours in TABLE I, it can be found that the higher quality of newly added landmarks with their measurements leads to more accurate pose estimation, which is basically consistent with our analysis in section V-D.

We choose fr3/office for mapping visualization, as shown in Fig. 4(b). Line features can provide more spatial structural information of the environments than point features. Fig. 5 shows the difference of sparse map between ORB-SLAM2 and Ours, which indicates that our method can construct a more accurate map structure.

In this dataset, we compare our algorithm with eight state-of-the-art SLAM methods: ORB-SLAM2, an indirect point-line-based method PL-SLAM [9], a direct point-line-based method DLGO [16], RKD-SLAM [29], Kintinuous [30], ElasticFusion [31], DVO-SLAM and RGBD SLAM [32].

The ATE results are shown in TABLE II. From the table, we can find that: (i) the methods with combination of points and lines lead to higher accuracy than those only using points; (ii) our method achieves better performance than other methods in more than half of the sequences. In dynamic environments (fr3/sit_static, fr3/sit_half, fr3/walk_half), the performance of ORB-SLAM2 is worse than the point-line-based methods, since the line features are basically on the static objects. PL-SLAM fails in the sequence fr3/nstr_tex_far and the main reason is that the images are blurred due to the fast camera motion. Additionally, we show the estimated trajectories (ORB-SLAM2 and Ours) corresponding to the sequences fr1/room and fr3/sit_half in Fig. 7. The performance of ORB-SLAM2 drops in the low-textured (lr-1) and low dynamic (fr3/sit_half) environments. We also show the comparison of trajectory errors in different axes for sequence fr1/360 in Fig. 6.

We test the ATE of our method with different number of sampling points and the same parameter setting in the sequences fr1/room, fr3/nstr_tex_far, lr-1_noise and of-3_noise, and the results are shown in Fig. 8. In our opinion, the quantity and quality of newly added landmarks need to be balanced and paid enough attention. Sampling too many points will lead to reduction of the quality of line landmarks due to the noisy depth measurements and result in an increase of the number of outliers. It would be better to sample a small number of points and we choose five sampling points in our implementation.

The point-line-based visual SLAM methods improve the accuracy and robustness, but at the same time lead to higher computational complexity. In particular, the extraction and matching of line features increase a lot of time consumption. We summarize the results of mean tracking time per frame for ORB-SLAM2 and Ours in TABLE III.