DeepRelativeFusion: Dense Monocular SLAM using Single-Image Relative Depth Prediction

In LSD-SLAM, the trajectory of the camera poses and the 3D location of the map points are stored in a list of keyframes. Each keyframe $\mathcal{K}_{i}$ contains an image $I_{i}:\Omega\rightarrow\mathbb{R}$, a semi-dense inverse depth map $D_{i,\textup{semi-dense}}:\Omega_{i}\rightarrow\mathbb{R}^{+}$, a semi-dense inverse depth variance map $V_{i,\textup{semi-dense}}:\Omega_{i}\rightarrow\mathbb{R}^{+}$, and a camera pose $S_{i}\in\textup{Sim}(3)$. Note that $\Omega_{i}\subset\Omega$ is a subset of pixels extracted from the texture-rich image regions for the structure and camera motion estimation, and a Sim(3) camera pose $S_{i}$ is defined by:

$$S_{i}=\begin{bmatrix}sR&t\\ 0&1\end{bmatrix},$$

(1)

where $R\in SO(3)$ is the rotation matrix, $t\in\mathbb{R}^{3}$ the translation vector and $s\in\mathbb{R}^{+}$ the scaling factor.

For every new keyframe $\mathcal{K}_{i}$, we obtain a relative inverse depth map, hereinafter referred to as relative depth map, from MiDaS [23] for the densification of the semi-dense depth map. Because the depth prediction $D_{i,\textup{CNN}}$ is a relative depth map, the predicted depth map needs to be scale- and shift-corrected before it can be used in the densification step. The scale- and shift-correction can be performed as follows:

$$D_{i,\textup{CNN}}^{\prime}=aD_{i,\textup{CNN}}+b,$$

(2)

where $a\in\mathbb{R}^{+}$ and $b\in\mathbb{R}$ are the scale and shift parameters, respectively. Let $\vec{d_{n}}=\begin{pmatrix}d_{n}&1\end{pmatrix}^{T}$ and $h^{\textup{opt}}=\begin{pmatrix}a&b\end{pmatrix}^{T}$, and the parameters $a$ and $b$ can be solved in closed-form as follows [23]:

$$h^{\textup{opt}}=\left(\sum_{n\in\Omega_{i}}\vec{d_{n}}\vec{d_{n}}^{T}\right)^{-1}\left(\sum_{n\in\Omega_{i}}\vec{d_{n}}d_{n}^{\prime}\right),$$

(3)

where $d_{n}\in D_{i,\textup{semi-dense}}$ and $d_{n}^{\prime}\in D_{i,\textup{CNN}}$ are the inverse depth values of the semi-dense depth map and relative depth map, respectively.

Our adaptive filtering is built upon bilateral filtering [16]. A bilateral filter is designed to combine the local pixel values according to the geometric closeness $w_{d}(.,.)$ and the photometric similarity $w_{s}(.,.)$ between the centre pixel $x$ and a nearby pixel $x_{n}$ within a window $\mathcal{N}$ of an image $I$, which is defined by:

$$\begin{gathered}I_{\textup{filtered}}(x)=\frac{1}{W_{\mathcal{N}}}\sum_{n\in\mathcal{N}}\bigg{(}I(x_{n})\\ \underbrace{\exp\Big{(}-\frac{(I(x)-I(x_{n}))^{2}}{2\sigma_{s}^{2}}\Big{)}}_{=:w_{s}(x,x_{n})}\underbrace{\exp\Big{(}-\frac{\left\|x-x_{n}\right\|^{2}}{2\sigma_{d}^{2}}\Big{)}}_{=:w_{d}(x,x_{n})}\bigg{)}\end{gathered}$$

(4)

with

$$W_{\mathcal{N}}=\sum_{n\in\mathcal{N}}w_{s}(x,x_{n})w_{d}(x,x_{n}).$$

(5)

In the context of semi-dense depth map filtering, we introduce two additional weighting schemes, namely, CNN depth consistency $w_{c}(.,.)$ and depth uncertainty $w_{u}(.)$, to remove the semi-dense depth pixels that have large local variance compared to their corresponding CNN depth as well as large depth uncertainty:

$$\begin{gathered}w_{c}(x,x_{n})=\exp\left(-\frac{\Big{(}\frac{D_{i,\textup{semi-dense}}(x)}{D_{i,\textup{semi-dense}}(x_{n})}-\frac{D_{i,\textup{CNN}}^{\prime}(x)}{D_{i,\textup{CNN}}^{\prime}(x_{n})}\Big{)}^{2}}{2\sigma_{c}^{2}}\right)\\ w_{u}(x_{n})=\exp\left(-\frac{\sigma_{u}V_{i,\textup{semi-dense}}(x_{n})}{D_{i,\textup{semi-dense}}(x_{n})^{4}}\right),\end{gathered}$$

(6)

where the squared ratio difference in $w_{c}(.,.)$ computes the scale-invariant error [27], and $D_{i,\textup{semi-dense}}(x_{n})$ in $w_{u}(.)$ detects spurious depth pixels. $\sigma_{s}$, $\sigma_{d}$, $\sigma_{c}$ and $\sigma_{u}$ are the smoothing parameters in their respective spatial kernels. Therefore, a filtered semi-dense depth map $D_{i,\textup{semi-dense}}^{\prime}$ can be computed as follows:

$\displaystyle\begin{split}D_{i,\textup{semi-dense}}^{\prime}(x_{n})=\frac{1}{W_{\mathcal{N}}^{\prime}}\sum_{n\in\mathcal{N}}\Big{(}D_{i,\textup{semi-dense}}(x_{n})\\ w_{s}(x,x_{n})w_{d}(x,x_{n})w_{c}(x,x_{n})w_{u}(x_{n})\Big{)}\end{split}$

(7)

with

$$W_{\mathcal{N}}^{\prime}=\sum_{n\in\mathcal{N}}w_{s}(x,x_{n})w_{d}(x,x_{n})w_{c}(x,x_{n})w_{u}(x_{n}),$$

(8)

and, with the updated $D_{i,\textup{semi-dense}}^{\prime}$, we re-estimate its semi-dense depth variance map $V_{i,\textup{semi-dense}}^{\prime}$ by taking the average of squared deviations within the local window for all the semi-dense depth pixels:

$\displaystyle\begin{split}V_{i,\textup{semi-dense}}^{\prime}(x_{n})=\frac{\lvert\mathcal{N}\rvert}{n_{\textup{valid}}}\frac{1}{W_{\mathcal{N}}^{\prime}}\sum_{n\in\mathcal{N}}\Big{(}W_{\mathcal{N}}^{\prime}(x_{n})\\ \left(D_{i,\textup{semi-dense}}^{\prime}(x)-D_{i,\textup{semi-dense}}(x_{n})\right)^{2}\Big{)},\end{split}$

(9)

where $\lvert\mathcal{N}\rvert$ is the total number of pixels within the window, $n_{\textup{valid}}$ the number of pixels containing depth values, and $W_{\mathcal{N}}^{\prime}(.)$ the weight computed at a nearby pixel. To remove the noisy depth pixels, we only include filtered depth whose variance is lower than a threshold $\gamma$. To ensure similar weighting effect of the semi-dense depth maps in densification, we rescale the semi-dense depth variance $V_{i,\textup{semi-dense}}^{\prime}$:

$$V_{i,\textup{semi-dense}}^{\prime}=\frac{\overline{V_{i,\textup{semi-dense}}}}{\overline{V_{i,\textup{semi-dense}}^{\prime}}}V_{i,\textup{semi-dense}}^{\prime},$$

(10)

where $\overline{\cdot}$ is the mean operator.

Consider the densification of $D_{i,\textup{semi-dense}}^{\prime}$ of $\mathcal{K}_{i}$ using $D_{i,\textup{CNN}}^{\prime}$ as initial values: the estimated inverse dense depth map $D_{i,\textup{opt}}$ can be obtained through the minimization of the cost function given by:

$$E_{\textup{total}}=E_{\textup{CNN\_grad}}+\lambda E_{\textup{semi-dense}}.$$

(11)

The first term, CNN depth gradient regularization $E_{\textup{CNN\_grad}}$, enforces depth gradient consistency between $D_{i,\textup{CNN}}$ and $D_{i,\textup{opt}}$:

$$E_{\textup{CNN\_grad}}=\frac{1}{\lvert\Omega\rvert}\sum_{n\in\Omega}\frac{\left(E_{\textup{CNN\_grad},x}(n)\right)^{2}+\left(E_{\textup{CNN\_grad},y}(n)\right)^{2}}{\left(1/{D_{i,\textup{CNN}}^{\prime}(n)}\right)^{2}},$$

(12)

with

$$\begin{gathered}E_{\textup{CNN\_grad},x}=\partial_{x}\ln D_{i,\textup{opt}}-\partial_{x}\ln D_{i,\textup{CNN}}^{\prime}\\ E_{\textup{CNN\_grad},y}=\partial_{y}\ln D_{i,\textup{opt}}-\partial_{y}\ln D_{i,\textup{CNN}}^{\prime},\end{gathered}$$

(13)

where $\lvert\Omega\rvert$ is the cardinality of $\Omega$, and $\partial$ the gradient operator. This error term is similar to the scale-invariant mean squared error in log space used in [27]. The denominator $\left(1/{D_{i,\textup{CNN}}^{\prime}}\right)^{2}$ in Equation \tagform@12 simulates the variance of the depth prediction, which provides stronger depth gradient regularization to closer objects than farther objects.

The second term, semi-dense depth consistency $E_{\textup{semi-dense}}$, minimizes the difference between the optimized depth map and the semi-dense depth map from LSD-SLAM (similar to [12]):

$$E_{\textup{semi-dense}}=\frac{1}{\lvert\Omega_{i}\rvert}\sum_{n\in\Omega_{i}}\rho\left(\frac{\left(D_{i,\textup{opt}}(n)-D_{i,\textup{semi-dense}}^{\prime}(n)\right)^{2}}{V_{i,\textup{semi-dense}}^{\prime}(n)}\right),$$

(14)

where $\lvert\Omega_{i}\rvert$ is the cardinality of $\Omega_{i}$. We add the generalized Charbonnier penalty function [28], $\rho(.)$, to improve reconstruction accuracy.

To incorporate the optimized semi-dense and dense structure into improving the keyframe poses while at the same time minimizing the influence of erroneous regions within the structure, we introduce a two-view consistency check step to filter the inconsistent depth regions between the current keyframe $\mathcal{K}_{i}$ and the last keyframe $\mathcal{K}_{i-1}$. To check for structural consistency, we project the last keyframe semi-dense $D_{i-1,\textup{semi-dense}}^{\prime}$ and densified $D_{i-1,\textup{opt}}$ depth maps to the current keyframe’s viewpoint:

$$\begin{gathered}\hat{\dot{x}}=KS_{i-1\rightarrow i}D_{i-1,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}(x)K^{-1}\dot{x}\qquad\textup{with}\quad\{x|D_{i-1,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}(x)>0\}\\ \hat{D_{i,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}}(\hat{x})=\left[\hat{\dot{x}}\right]_{3},\end{gathered}$$

(15)

where $D_{i-1,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}$ is a placeholder for $D_{i-1,\textup{semi-dense}}^{\prime}$ and $D_{i-1,\textup{opt}}$ and $\hat{D_{i,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}}(\hat{x})$ the warped $D_{i-1,\textup{semi-dense}}^{\prime}$ and $D_{i-1,\textup{opt}}$ in $\mathcal{K}_{i}$’s viewpoint. To retain the semi-dense structure in LSD-SLAM, the semi-dense depth regions in $D_{i-1,\textup{semi-dense}}^{\prime}$ has been excluded when warping $D_{i-1,\textup{opt}}^{\prime}$. $\dot{x}$ is $x$ in homogeneous coordinates, and $K$ is the camera intrinsics. The two-view consistent depth map $D_{i,\textup{c}}$ is given by:

$$D_{i,\textup{c}}(x)=\begin{cases}D_{i,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}(x)&\text{if $\left|D_{i,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}(x)-\hat{D_{i,\mathchoice{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\displaystyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\textstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptstyle\bullet$}}}}}{\mathbin{\vbox{\hbox{\scalebox{0.5}{$\scriptscriptstyle\bullet$}}}}}}}(x)\right|<\tau_{e}$}\\ 0&\text{otherwise}\end{cases}.$$

(16)

Next, we propagate the pose uncertainty $\Sigma_{\xi,i}\in\mathbb{R}^{7\times 7}$ estimated by LSD-SLAM to approximate the uncertainty $V_{i,opt}$ associated with $D_{i,\textup{opt}}$:

$$V_{i,\textup{opt}}\approx J_{d}\Sigma_{\xi,i}J_{d}^{T}$$

(17)

where $J_{d}\in\mathbb{R}^{1\times 7}$ is the Jacobian matrix containing the first-order partial derivatives of the camera projection function with respect to the camera pose [29]. As the two-view consistent depth $D_{i,c}$ contains a mixture of semi-dense depth regions and densified depth regions, the corresponding variance $V_{i,c}$ is sampled from $V_{i,\textup{semi-dense}}^{\prime}$ in the semi-dense depth regions and from $V_{i,\textup{opt}}$ otherwise. After obtaining two-view consistent depth $D_{i,c}$ and depth variance $V_{i,c}$ maps, we update the Sim(3) constraints in the pose-graph to refine the keyframe poses.

To evaluate our system, we use ICL-NUIM [10] and TUM RGB-D [9] datasets, which contain ground truth depth maps and trajectories to measure the reconstruction accuracy. We use the reconstruction accuracy metric proposed in [11], which is defined as the percentage of the depth values with relative errors of less than $10\%$. Also, we use absolute trajectory error (ATE) to measure the error of the camera trajectories. Since our system does not produce absolute scale scene reconstruction, and therefore each depth map needs to be scaled using the optimal trajectory scale (calculated with the TUM benchmark script⁴⁴4https://vision.in.tum.de/data/datasets/rgbd-dataset/tools) and its corresponding Sim(3) scale for depth correctness evaluation.

We compare our reconstruction accuracy against the state-of-the-art dense SLAM systems, namely CNN-SLAM [11], DeepFusion [12], and DeepFactors [13]. Table I shows a comparison of the reconstruction accuracy: the first three columns show the reconstruction accuracy of the state-of-the-art systems and the last two columns show a comparison between using VNLNet (an absolute depth prediction CNN) and MiDaS (a relative depth prediction CNN) in our optimization framework (see Section V-D). Owing to the similarity of the optimization frameworks between our system and DeepFusion, we also include the results for running dense reconstruction with an additional CNN depth consistency error term in the cost function (labelled ”$\dagger$” in Table I)⁵⁵5DeepFusion is not open source, and therefore the results are based on the implementation of our optimization framework (see Section IV). Our implementation of CNN depth consistency term is similar to that of DeepFusion except we use CNN depth for providing depth uncertainty (similar to Equation \tagform@12).. Note that the reconstruction accuracy of our method is taken with an average of 5 runs. Our method outperforms the competitors except for the ICL/office0 sequence, as LSD-SLAM is unable to generate a good semi-dense structure under rotational motion, hence the degraded reconstruction performance in the densification of the semi-dense structure. The reconstruction results demonstrate the superiority of our system by comparing the last column with all other columns in Table I. Figure 3 shows the use of our optimization framework to obtain more accurate densified depth maps from less accurate predicted relative depth maps.

We notice that the semi-dense structure from LSD-SLAM contains spurious map points, which may worsen the dense reconstruction performance. Figure 4 shows a qualitative comparison between the semi-dense depth maps by LSD-SLAM and the filtered depth maps, demonstrating the effectiveness of the adaptive filter in eliminating noisy depth pixels while preserving the structure of the scene. Quantitatively, the second row and the third row of Table II (labelled ”f”) shows about $5\%$ improvement on using adaptive filtering in dense reconstruction (see also the last four rows).

Table II shows the reconstruction results using different combinations of error terms in the cost function. To ensure consistent measurement of the reconstruction accuracy using different cost functions, the keyframes—i.e., the semi-dense depth and depth variance maps, and the camera poses—are pre-saved so that the densification process is not influenced by the inconsistency between runs from LSD-SLAM. Consistent with the finding in DeepFusion, incorporation of CNN depth gradient consistency and CNN depth consistency improves the reconstruction accuracy dramatically, although our CNN does not explicitly predict depth gradient and depth gradient variance maps (see the second and last row). However, removing the CNN depth consistency term (the the third and fourth last row), in our case, leads to better reconstruction accuracy (see also the third last and last column of Table I); the added generalized Charbonnier function (the second row, and labelled ”c”) also increases the accuracy.

To illustrate the advantage of using relative depth prediction CNNs (e.g., MiDaS), we perform the same densification step with an absolute depth prediction CNN, VNLNet⁶⁶6One important consideration in selecting a competing absolute depth prediction CNN is the runtime memory requirements. VNLNet is considered state-of-the-art at the time of experimental setup with a reasonable memory footprint. [20], and then compare the reconstruction accuracy between them. To promote a fair comparison, neither MiDaS nor VNLNet has been trained on the TUM RGB-D and ICL-NUIM datasets. In Table I, we show that, in general, using scale- and shift-corrected relative depth prediction (labelled ”MiDaS”) instead of absolute depth prediction (other columns) has superior dense reconstruction performance, as a result of more accurate depth prediction from MiDaS than depth prediction from VNLNet (last and second last column of Table III); Laina (second column of Table III), another absolute depth prediction CNN being used in CNN-SLAM, is significantly less accurate than MiDaS, which indicates that the outperformance of our system may just simply be due to the fact that MiDaS provides more accurate depth prediction for densification. Not only are the scale- and shift-corrected relative depth maps from MiDaS metrically more accurate than the absolute depth maps from VNLNet, but the relative depth maps also appear to be smoother (see Figure 5).

Table IV shows the camera tracking accuracy between our method and CNN-SLAM⁷⁷7Only CNN-SLAM has the ATEs on the evaluation datasets.. From the first two columns, we can see that our camera tracking performance, even without the pose-graph refinement, reduces the ATE of CNN-SLAM by almost $50\%$. Since both of the systems are built upon LSD-SLAM, the performance difference could be due to our configuration settings in LSD-SLAM (see Section IV). To evaluate the effectiveness of pose-graph refinement, the last column shows a baseline performance of refining the pose-graph using ground truth depth. In general, pose-graph refinement reduces the ATE significantly to the extent that, in certain sequences, it is similar to that obtained by pose-graph refinement using the ground truth depth.

On average, the CNN depth prediction and optimization require 0.15 s and 0.2 s, respectively, to complete. The measurements are taken on a laptop computer equipped with an Intel 7820HK CPU and an Nvidia GTX 1070 GPU.