Fast loop closure detection using probabilistic integration of pose and appearance similarity

Abstract

Loop closure detection is a key technique for robots to minimize the accumulated localization and mapping errors after long-time explorations of simultaneous localization and mapping. However, the requirement for efficiency and accuracy performance for mobile robot applications is not well satisfied. In this article, we propose a fast and accurate loop closure detection method by exploiting both pose-based and appearance-based information in a probabilistic manner, inspired by the complementarity between the pose-based and the appearance-based information. Our approach formulates a probability framework combing the pose-based loop closure detection probability and the appearance-based loop closure detection probability. In the proposed framework, the pose-based loop closure detection model is firstly derived from the nonlinear optimization model of odometry. Then the appearance-based loop similarity and the pose-based loop similarity are combined into a joint framework to improve the loop closure detection performance. We implemented our approach using C++ and ROS and thoroughly tested it on the publicly available datasets. The experiments presented in this article suggest that the proposed method can achieve high efficiency and accuracy performance on loop closure detection.

Introduction

Visual loop closure detection is “the revisiting problem” for a robot to recognize a previously visited area. ¹ So the loop closure detection (LCD) issue is mostly regarded as a scene matching or image retrieval problem. ² An image acquired by a camera is matched to a map, where the map contains a set of previously acquired images. Once the match is successful, a loop closure event is confirmed. However, in contrast to general scene matching issues, the LCD method must deal with a growing dataset as the robot explores more scenes, leading to a linear increase in the computational cost, as shown in Figure 1(a). Therefore, the LCD algorithm must be sufficiently efficient in consideration that there are always newly acquired images waiting to be processed with the ongoing application of real-time simultaneous localization and mapping (SLAM).

Figure 1.

Time consumption of loop closure detections. (a) The computational cost of traditional loop closure detection will linearly increase with ongoing SLAM. While the proposed method maintains stable and low. (b) Different procedures of loop closure detection consume different length of time when processing a loop candidate. The proposed pose-based probability module (PP) runs faster than the other procedures (SC is the sequential consistency check, and GC is the geometrical consistency check). SLAM: simultaneous localization and mapping.

To perform a faster and more accurate LCD, we presented a novel approach that exploits both pose-based and appearance-based information, inspired by the complementarity between these two kinds of information. The image similarity works well in most cases but may confuse different places with similar scenes. Think about how human beings recognize previously visited places. People can recognize a previously visited place on the basis of the same location, even if the scene has changed greatly. On the contrary, people will not confuse two different places with similar scenes during the journey. In other words, both kinds of information contribute to the LCD task, and they are complementary to each other.

Inspired by the complementary of pose- and appearance-based information, we propose a novel pose-appearance-based probabilistic LCD method. This study is the first trial combining these two kinds of information into a joint framework to improve the LCD performance.

Consequently, this work is an actual “loop” closure detection method, rather than a general re-localization method or scene matching method. In other words, the kidnapping issue is not taken into account.

The joint framework of the proposed method is illustrated in Figure 2. The pose-based method calculates the probability of loop closure using the proposed probability model. And in this process, a fast pose-based filter is also designed to efficiently acquire the loop closure candidates. The appearance-based method is a replaceable component based on the appearance-based LCD method, such as bag-of-words (BoW) and convolutional neural networks (CNN). Then the product of the pose-based probability and the appearance-based similarity is then used as the final loop probability. A sequential consistency (SC) check and a geometrical consistency (GC) check are also introduced into the proposed framework as optional modules which are commonly used for LCD applications. The SC check is very efficient while the GC check has a remarkable precision performance with quite high time consumption.

Figure 2.

The joint LCD framework combining the pose-based method and the appearance-based method. The first block indicates the proposed pose-based loop closure probability calculation. The appearance-based method is based on other appearance-based LCD algorithms, such as BoW and CNN. The dashed blocks are optional for different applications. LCD: loop closure detection; BoW: bag-of-words.

The main contribution of this article is the probability combination of pose and appearance in LCD. This allows the proposed method to achieve higher efficiency and accuracy performance on LCD. In sum, we make two key claims:

The introduction of pose-based information into the framework will accelerate the LCD process.

The proposed LCD pipeline is compatible with many other methods and easy to be used in real-time SLAM applications. The rest of this article is mainly focused on the probability derivations of pose similarity, so that the appearance similarity and pose similarity can be combined and form a novel LCD method.

The remainder of this article is structured as follows. The related work is introduced in section “Related works,” followed by the relevant mathematical concepts and notations in section “Preliminaries.” As the main mathematical derivation of this article, in section “Pose-based loop closure probability,” the pose similarity is derived probabilistically, where the accumulative pose covariance matrix is incrementally calculated and used for this pose similarity. Then, in section “Pose-appearance-based LCD,” the pose similarity is introduced in the new LCD pipeline, named as the pose-appearance-based LCD. Finally, section “Experimental evaluation” presents the experimental settings and results to validate the contributions of the proposed method, and this article is concluded in section “Conclusions.”

Related works

After years of research, LCD methods have formed four main types: traditional feature based, learning based, sequence based, and pose based.

Methods based on traditional feature

Visual features can well represent the appearance information of an image frame and are widely used for image matching. So it is a natural idea to detect loop closures using visual features. ³ Visual features can also be coded as words, named BoW, to accelerate the feature matching process. ⁴ It represents image features as words of a dictionary, which is generated offline using a large number of visual features with the k-means algorithm. ⁵ The key idea is then to accelerate the image matching procedure by comparing words instead of raw feature descriptors.

Based on the BoW model, appearance-based LCD methods proposed by Angeli et al. ⁶ , Cummins and Newman ⁷ , Schöps and Cremers ⁸ , Gálvez-López and Tardós ⁹ , and Labbé and Michaud ¹⁰ are widely used. incremental appearance-based mapping (IAB-MAP) ⁶ extends the BoW method by adding color histogram features together with keypoint features in the dictionary. The epipolar constraint between the similar images is checked and such a procedure is referred to as the GC check. fast appearance-based mapping (FAB-MAP) ^7,8 also adopts the BoW idea and uses the Chow–Liu Tree ¹¹ to model the distributions of words, which in turn provides a way of calculating the probability of a loop closure event. DLoopDetector ⁹ employs both sequential and GC checks after the BoW image match. The sequential check keeps a serial of images matching the same scene in the map as a candidate loop closure, while the GC check employs random sample consensus (RANSAC) to find a fundamental matrix between the image pair. The loop closure is confirmed only if both consistency checks are verified. real-time appearance-based mapping (RTAB-MAP) ¹⁰ focuses on the memory management aspect for large-scale and long-term online SLAM application, it also proposes a Bayes estimation approach for LCD problems. ORB-SLAM ¹² improves the DLoopDetector by replacing the binary robust independent elementary features (BRIEF) feature with the rotation invariant oriented fast and rotated brief (ORB) features, which can better deal with perspective changes when performing LCD. Bampis et al. ¹³ use the BoW to produce a general vector representing the whole scene and improve the ability to deal with the LCD for image sequences.

The BoW model can be extended to be incremental, to avoid the vocabulary training stage. ¹⁴ Then the robot may online and incrementally detect loop closures for image sequences. ^{15 –17}

Methods based on learning

In addition to the methods based on traditional features, with the fast development of machine learning, LCD based on deep learning is also gaining in popularity. ^{18 –20} For example, Jégou et al. proposed the vector of locally aggregated descriptors (VLAD) algorithm ²¹ based on the Fisher kernel, which computes the gradients of the input features in a trained model and generates a joint fixed-size vector as the representation of the input image. ^22,23 NetVLAD ²⁴ applies soft assignment of VLAD descriptors to the offline learned clusters as an end-to-end visual place recognition method. Merrill and Huang proposed the CALC ²⁵ method by training a network comprising a semantic segmentation and auto-encoder to extract descriptors. Based on the traditional histogram-of-oriented-gradients (HOG) LCD mehtod, ²⁶ Zaffar et al. proposed the CoHOG ²⁷ by integrating the convolutional scanning and regions-based feature extraction of CNN. Zhang et al., on the other hand, proposed a CNN-based LCD method named CNN-LCD. ²⁸ An et al. combined the CNN global features and CNN local features to improve the LCD performance. ²⁹

Methods based on sequential information

Since the image frames are captured sequentially, the loop closure images are also sequentially consistent. Then the SC check or temporal consistency check can be employed to filter the LCD results to improve the accuracy.

SeqSLAM ³⁰ is a typical representative method using sequential information. Instead of matching to a single image, SeqSLAM calculates matching location within every local navigation sequence to enhance the performance in challenging environments.

DOseqSLAM ^31,32 uses a dynamical instead of fixed size of sequence to check the SC, achieving a better adaptability for different applications. Similar to the usage of the sequential information, the incremental LCD methods are also proposed. ^29,33,34

Methods based on pose information

Although the appearance-based LCD methods are powerful and successful, however, they suffer from high computational demands, especially when the computational cost linearly increases with the map size. In contrast to that, Pose-driven methods, on the other side of the spectrum, can provide an efficient solution with less computational demand. ^35,36 For example, Neira and Tardos ³⁵ recognized the importance of the pose-based information and used the pose-based information for the data association in the mapping process. Li et al. ³⁶ model the visual odometry accumulative error based on the Kalman Filter and the RGB-D camera model. The error in the pose is used as a constraint in LCD. The disadvantage of these two approaches is the limiting Kalman filter model, which is not applicable to nonlinear-optimization-based SLAM, such as the sparse methods ^{37 –40} and dense methods. ^{41 –43}

In this article, we introduce a novel pose-appearance-based method by exploiting both pose-based and appearance-based information in a probabilistic manner, which performs fast and accurate LCD.

Preliminaries

This work combines the pose-based loop probability and the appearance-based loop probability. The calculation of the pose-based loop probability needs the estimation of the covariance matrix of poses, where the pose is represented as Lie algebra, and its covariance matrix is calculated using nonlinear optimization. So in this section, some basic formulas are introduced as preliminaries.

Lie algebra

Consider the 3D rotation matrix of R , that is, $\mathit{R}\in \mathit{SO}3$ , and ${\mathit{\varphi }}^{\wedge }$ as its corresponding Lie algebra, that is, ${\mathit{\varphi }}^{\wedge }\in \mathit{so}3$ . Then, the relationship between R and $\mathit{\varphi }$ is

$\mathit{R}=\text{exp}{\mathit{\varphi }}^{\wedge },\hspace{1em}\hspace{0.17em}\mathit{\varphi }=(\text{ln}\mathit{R}{)}^{\vee }$

1

where ${}^{\wedge }$ is the antisymmetric matrix operator, and ${}^{\vee }$ is its inverse operator, as

${\mathit{\varphi }}^{\wedge }={\left[\begin{array}{c}{\varphi }_1\\ {\varphi }_2\\ {\varphi }_3\end{array}\right]}^{\wedge }\doteq \left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$

2

The operator ${}^{\wedge }$ has the property as

${\mathit{a}}^{\wedge }\mathit{b}=-{\mathit{b}}^{\wedge }\mathit{a},\hspace{0.17em}\hspace{1em}({\mathit{a}}^{\wedge }\hspace{0.17em}\text{and}\hspace{0.17em}{\mathit{b}}^{\wedge }\in \mathit{so}3)$

3

If a small increment is added to $\mathit{\varphi }$ , the corresponding Lie group has the linear approximation as

$\text{exp}{\left(\mathit{\varphi }+\delta \mathit{\varphi }\right)}^{\wedge }=\text{exp}{\left({\mathit{J}}_l\delta \mathit{\varphi }\right)}^{\wedge }\text{exp}{\mathit{\varphi }}^{\wedge }$

4

On the contrary, the Lie algebra has the linear approximation in equation (5) with the small increment of R

${(\text{ln}(\Delta \mathit{R}\cdot \mathit{R}))}^{\vee }=\mathit{\varphi }+{\mathit{J}}_l^{-1}\delta \mathit{\varphi }$

5

where J _l is the left Jacobian matrix of the Lie algebra. If $\mathit{\varphi }$ is represented in norm and direction form as $\mathit{\varphi }=\varphi \mathit{a}$ , the left Jacobian matrix together with its inverse can be calculated as

$\{\begin{array}{l}{\mathit{J}}_l=\frac{\text{sin}\varphi }{\varphi }\mathit{I}+\frac{1-\text{cos}\varphi }{\varphi }{\mathit{a}}^{\wedge }+\left(1-\frac{\text{sin}\varphi }{\varphi }\right)\mathit{a}{\mathit{a}}^{\text{T}}\hfill \\ {\mathit{J}}_l^{-1}=\frac{\varphi }{2}\text{cot}\frac{\varphi }{2}\mathit{I}-\frac{\varphi }{2}{\mathit{a}}^{\wedge }+\left(1-\frac{\varphi }{2}\text{cot}\frac{\varphi }{2}\right)\mathit{a}{\mathit{a}}^{\text{T}}\hfill \end{array}$

6

Nonlinear optimization for pose estimation

Consider a six-dof camera pose T written as vector form $\mathit{\xi }$

$\mathit{T}=\text{exp}{\mathit{\xi }}^{\wedge },\hspace{0.17em}\hspace{0.17em}{\mathit{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathit{\varphi }}^{\wedge }& \mathbf{\rho }\\ {\mathbf{0}}^{\text{T}}& 0\end{array}\right]\in \mathit{se}3,\hspace{0.17em}\hspace{0.17em}{\mathit{\varphi }}^{\wedge }\in \mathit{so}3$

7

The landmarks observed by the camera are 3D points in the map, ${\mathit{x}}_{wj}$ , and the corresponding observations in the image are $x_{oj}$ . Then, the camera pose can be estimated based on the nonlinear optimization model as

$\begin{array}{l}\underset{\mathit{\xi }}{{\displaystyle \text{arg} \text{max}}} p(\left(x_{o1},{\mathit{x}}_{w1}\right),\left(x_{o2},{\mathit{x}}_{w2}\right),\dots ,\left(x_{oN},{\mathit{x}}_{wN}\right)|\mathit{\xi })\\ =\underset{\mathit{\xi }}{{\displaystyle \text{arg} \text{max}}} \text{ln}\left\{{\displaystyle \prod _{j=1}^N}\frac{1}{{\sigma }_{oj}\sqrt{2\pi }}\text{exp}\left(-\frac{e{(x_{oj},\pi \left(\mathit{\xi },x_{wj}\right))}^2}{2{\sigma }_{oj}{}^2}\right)\right\}\\ =\underset{\mathit{\xi }}{{\displaystyle \text{arg} \text{min}}}{\displaystyle \sum _{j=1}^N}\frac{e{(x_{oj},\pi \left(\mathit{\xi },x_{wj}\right))}^2}{{\sigma }_{oj}{}^2}\end{array}$

8

The $\pi ()$ is the observation function, and $e(x_{oj},\pi \left(\mathit{\xi },x_{wj}\right))$ is the observation error, which is simply written as e_j . ${\sigma }_{oj}$ is the covariance of e_j , which can be calculated online based on the observation errors. N is the total number of observed landmarks.

Then, equation (8) can be rewritten in the matrix form as follows

$\underset{\mathit{\xi }}{{\displaystyle \text{arg} \text{min}}}f=\underset{\mathit{\xi }}{{\displaystyle \text{arg} \text{min}}}{\mathit{e}}^{\text{T}}\mathit{We}$

9

where $\mathit{e}\doteq \left[e_1,e_2,\dots ,e_N\right]$ , and W is a diagonal matrix of $\text{diag}\{{\sigma }_{o1}^{-2},{\sigma }_{o2}^{-2},\dots ,{\sigma }_{oN}^{-2}\}$ .

Based on the Gauss–Newton nonlinear optimization algorithm, equation (9) can be iteratively solved, and the t-th iterative increment of $\mathit{\xi }$ is calculated as in equation (10) ⁴⁴

$\delta {\mathit{\xi }}_t=-{({\mathit{J}}_t^{\text{T}}{\mathit{W}}_t{\mathit{J}}_t)}^{-1}{\mathit{J}}_t^{\text{T}}{\mathit{W}}_t{\mathit{e}}_t$

10

where J _t is the Jacobian matrix ${\mathit{J}}_t=\partial {\mathit{e}}_t/\partial {\mathit{\xi }}_t^{\text{T}}$ .

After several iterations, the optimal pose of $\widehat{\mathit{\xi }}$ can be obtained when $\delta {\mathit{\xi }}_t\to \mathbf{0}$ .

Covariance of pose

The ${\mathit{J}}_t^{\text{T}}{\mathit{W}}_t{\mathit{J}}_t$ in equation (10) can be written as H _t , which is an approximate Hessian matrix for the optimization. Then the covariance matrix of the optimal estimation ${\mathbf{\Sigma }}_{\mathit{\xi }}$ can be obtained from the final H

${\mathbf{\Sigma }}_{\mathit{\xi }}={\mathit{H}}^{-1}=({\mathit{J}}^{\text{T}}\mathit{W}\mathit{J}{)}^{-1}$

11

The ${\mathbf{\Sigma }}_{\mathit{\xi }}$ is the relative pose covariance against the previous frames. So the accumulative covariance ${\mathbf{\Sigma }}_i$ for the i-th frame should be calculated based on the propagation law of uncertainty

${\mathbf{\Sigma }}_i=\frac{\partial {\mathit{\xi }}_i}{\partial {\mathit{\xi }}_{\text{inc}}^{\text{T}}}{\mathbf{\Sigma }}_{\text{inc}}{\left(\frac{\partial {\mathit{\xi }}_i}{\partial {\mathit{\xi }}_{\text{inc}}^{\text{T}}}\right)}^{\text{T}}+\frac{\partial {\mathit{\xi }}_i}{\partial {\mathit{\xi }}_{i-1}^{\text{T}}}{\mathbf{\Sigma }}_{i-1}{\left(\frac{\partial {\mathit{\xi }}_i}{\partial {\mathit{\xi }}_{i-1}^{\text{T}}}\right)}^{\text{T}}$

12

where ${\mathit{\xi }}_{\text{inc}}$ is the incremental pose between ${\mathit{\xi }}_i$ and ${\mathit{\xi }}_{i-1}$ , and ${\mathbf{\Sigma }}_{\text{inc}}$ is its covariance matrix from the incremental pose estimation, that is, the ${\mathbf{\Sigma }}_{\mathit{\xi }}$ in equation (11).

Equation (11) is a fast but low accurate first-order estimation of the incremental covariance. There are other accurate methods ⁴⁵ as alternatives. For the joint optimization of sequential frames, there are also some methods calculating the pose covariance, ^46,47 which can be used for global trajectory optimization.

In this work, the covariance of pose is employed for LCD, but it is not a major concern about how to calculate the covariance in this work. Users may adopt any method mentioned above for a specific SLAM system. But the computational efficiency should be taken into consideration when utilizing covariance calculation methods, since the LCD process is executed in real time along with the progress of SLAM.

Pose-based loop closure probability

In this section, we proposed the pose-based method to calculate the loop closure probability of any two poses, shown as the first block in Figure 2.

Two images with exactly the same pose definitively indicate a loop closure. However, in most cases, these two poses are not exactly the same, but they share similar views. In this case, they are called images of covisibility, and they can generate a loop.

Consider the current pose as ${\widehat{\mathit{\xi }}}_i$ , and the candidate loop pose as ${\widehat{\mathit{\xi }}}_j$ , then the loop closure probability of the two poses will be

$p_j(\text{Loop})=\frac{|{\mathbf{A}}^{-1}{|}^{1/2}}{|{\mathbf{\Sigma }}_i{|}^{1/2}}\text{exp}\left\{\frac{1}{2}{\mathit{b}}^{\text{T}}\mathit{A}\mathit{b}-\frac{1}{2}{\widehat{\mathit{\xi }}}_j^{\text{T}}{\mathbf{\Sigma }}_{\text{co}}^{-1}{\widehat{\mathit{\xi }}}_j-\frac{1}{2}{\widehat{\mathit{\xi }}}_i^{\text{T}}{\mathbf{\Sigma }}_i^{-1}{\widehat{\mathit{\xi }}}_i\right\}$

13

All the variables in equation (13) are related to poses and covariance matrix. The derivation is detailed as follows.

Probability distribution of pose

To calculate the pose-based loop closure probability, the probabilistic distribution model of the pose needs to be derived first. For any pose of ${\mathit{\xi }}_i$ with the optimal estimation of ${\widehat{\mathit{\xi }}}_i$ and the covariance matrix of ${\mathbf{\Sigma }}_i$ , basing on the assumption of zero mean Gaussian noise, the probability distribution of the pose ${\mathit{\xi }}_i$ is fully parametrized as

${\rho }_{{}_{{\widehat{\mathit{\xi }}}_i,{\mathbf{\Sigma }}_i}}({\mathit{\xi }}_i)=\frac{1}{{(2\pi )}^{\text{dim}/2}|{\mathbf{\Sigma }}_i{|}^{1/2}}\text{exp}\left(-\frac{1}{2}{\left({\mathit{\xi }}_i-{\widehat{\mathit{\xi }}}_i\right)}^T{\mathbf{\Sigma }}_i^{-1}\left({\mathit{\xi }}_i-{\widehat{\mathit{\xi }}}_i\right)\right)$

14

Covisibility of two poses

The covisibility of two poses is the basis for visual odometry to calculate their incremental transformation. So the covisibility matrix can be estimated during the odometry process, and then be employed to detect pose-based loop closure.

The covisibility matrix ${\mathbf{\Sigma }}_{\text{co}}$ is calculated using a group of covisible poses during the odometry, as

${\mathbf{\Sigma }}_{\text{co}}=\frac{1}{n}{\displaystyle \sum _{i=1}^n}{\mathit{\xi }}_{\text{co},i}{\mathit{\xi }}_{\text{co},i}^{\text{T}}$

15

The ${\mathit{\xi }}_{\text{co},i}$ indicates the relative pose of any two images with covisibility, which are used for odometry. And ${\mathbf{\Sigma }}_{\text{co}}$ is incrementally updated online when new covisible image pairs are acquired with the progress of the odometry.

Pose-based loop closure probability

Then we can calculate the loop closure probability of any two poses based on the covariance matrix of pose and the covisibility matrix. In other words, the two poses are considered as loop closure only when they are covisible under the uncertainty of ${\mathbf{\Sigma }}_i$ and ${\mathbf{\Sigma }}_{\text{co}}$ .

Based on the marginal probability model, the probability of loop closure, that is, the probability of covisibility, can be calculated as

$p_j(\text{Loop})=p_j(\text{Covis})={\displaystyle {\int }_{-\infty }^{\infty }}{p}_{{}_{{\widehat{\mathit{\xi }}}_j,{\mathbf{\Sigma }}_{\text{co}}}}(\text{Covis}|\mathit{\xi }){\rho }_{{}_{{\widehat{\mathit{\xi }}}_i,{\mathbf{\Sigma }}_i}}(\mathit{\xi })\hspace{0.17em}\text{d}\mathit{\xi }$

16

Note the difference between $p_{*}$ and ${\rho }_{*}$ , where $p_{*}$ indicates the probability and ${\rho }_{*}$ indicates the probability density. $p_{{}_{{\widehat{\mathit{\xi }}}_j,{\mathbf{\Sigma }}_{\text{co}}}}(\text{Covis}|\mathit{\xi })$ is the probability of the covisibility of ${\widehat{\mathit{\xi }}}_j$ and $\mathit{\xi }$ . ${\rho }_{{}_{{\widehat{\mathit{\xi }}}_i,{\mathbf{\Sigma }}_i}}(\mathit{\xi })$ is the probability density function of $\mathit{\xi }$ calculated in equation (14).

Based on Bayes’ theorem of $P(A|B)=P(B|A)P(A)/P(B)$ , we can get $p_j(\text{Covis}|\mathit{\xi })=c_1{\rho }_j(\mathit{\xi }|\text{Covis})$ , where c ₁ is a constant factor and ${\rho }_j(\mathit{\xi }|\text{Covis})$ is the probability density function of $\mathit{\xi }$ under the constraint of covisibility. With the Gaussian distribution, we can obtain

$p_j(\text{Covis}|\mathit{\xi })=\frac{c_1}{{(2\pi )}^3|{\text{Σ}}_{\text{co}}{|}^{1/2}}\text{exp}\left(-\frac{1}{2}{\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_j\right)}^{\text{T}}{\mathbf{\Sigma }}_i^{-1}\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_j\right)\right)$

17

Considering that the probability of covisibility should be normalized to be 1 when the two poses are exactly the same, that is, $p_j(\text{Covis}|\mathit{\xi }={\widehat{\mathit{\xi }}}_j)=1$ , we can obtain

$p_j(\text{Covis}|\mathit{\xi })=\text{exp}\left(-\frac{1}{2}{\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_j\right)}^{\text{T}}{\mathbf{\Sigma }}_i^{-1}\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_j\right)\right)$

18

Finally, the probability of loop closure can be calculated as

$\begin{array}{l}{p}_j(\text{Loop})\\ ={\displaystyle {\int }_{-\infty }^{\infty }}{p}_j(\text{Covis}|\mathit{\xi }){\rho }_{{}_{{\widehat{\mathit{\xi }}}_i,{\mathbf{\Sigma }}_i}}(\mathit{\xi })\hspace{0.17em}\text{d}\mathit{\xi }\\ =\frac{1}{8{\pi }^3|{\mathbf{\Sigma }}_i{|}^{1/2}}{\displaystyle {\int }_{-\infty }^{\infty }}\text{exp}\{-\frac{1}{2}{\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_j\right)}^{\text{T}}{\Sigma }_{\text{co}}^{-1}\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_j\right)\\ \hspace{1em}\hspace{1em}-\frac{1}{2}{\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_i\right)}^{\text{T}}{\Sigma i}_{-1}\left(\mathit{\xi }-{\widehat{\mathit{\xi }}}_i\right)\}\hspace{0.17em}\text{d}\mathit{\xi }\end{array}$

19

Let $\mathbf{A}={\mathbf{\Sigma }}_{\text{co}}^{-1}+{\mathbf{\Sigma }}_i^{-1}$ and $\mathit{b}={\mathbf{A}}^{-1}\left({\mathbf{\Sigma }}_{\text{co}}^{-1}{\widehat{\mathit{\xi }}}_j+{\mathbf{\Sigma }}_i^{-1}{\widehat{\mathit{\xi }}}_i\right)$ . According to the symmetry of ${\mathbf{\Sigma }}_{ij}$ and ${\mathbf{\Sigma }}_{co}$ , equation (19) can be rewritten as

$\begin{array}{l}{p}_j(\text{Loop})\\ =\frac{1}{8{\pi }^3|{\mathbf{\Sigma }}_i{|}^{1/2}}{\displaystyle {\int }_{-\infty }^{\infty }}\text{exp}\{-\frac{1}{2}{\left(\xi -b\right)}^{\text{T}}A\left(\mathit{\xi }-\mathit{b}\right)+\frac{1}{2}{b}^{\text{T}}\mathit{A}\mathit{b}\\ -\frac{1}{2}{\widehat{\mathit{\xi }}}_j^{\text{T}}{\mathbf{\Sigma }}_{\text{co}}^{-1}{\widehat{\mathit{\xi }}}_j-\frac{1}{2}{\widehat{\mathit{\xi }}}_i^{\text{T}}{\mathbf{\Sigma }}_i^{-1}{\widehat{\mathit{\xi }}}_i\}\text{d}\mathit{\xi }\\ =\frac{|{\mathbf{A}}^{-1}{|}^{1/2}}{|{\mathbf{\Sigma }}_i{|}^{1/2}}\text{exp}\left\{\frac{1}{2}{\mathit{b}}^{\text{T}}\mathit{A}\mathit{b}-\frac{1}{2}{\widehat{\mathit{\xi }}}_j^{\text{T}}{\mathbf{\Sigma }}_{\text{co}}^{-1}{\widehat{\mathit{\xi }}}_j-\frac{1}{2}{\widehat{\mathit{\xi }}}_i^{\text{T}}{\mathbf{\Sigma }}_i^{-1}{\widehat{\mathit{\xi }}}_i\right\}\end{array}$

20

Fast loop closure candidate filtering

To improve the efficiency of the LCD procedure, the loop closure candidates are selected using a simple threshold operation based on the pose and the covariance matrix, before calculating the probability of equation (20).

We set a probabilistic threshold of 95%. According to the Gaussian integration function, the 95% confidence interval of ${\mathit{\xi }}_i$ is $[{\widehat{\mathit{\xi }}}_i-1.96{\mathit{\sigma }}_i,{\widehat{\mathit{\xi }}}_i+1.96{\mathit{\sigma }}_i]$ , and the 95% confidence interval of covisibility with ${\widehat{\mathit{\xi }}}_j$ is $[{\widehat{\mathit{\xi }}}_j-1.96{\mathit{\sigma }}_{\text{co}},{\widehat{\mathit{\xi }}}_j+1.96{\mathit{\sigma }}_{\text{co}}]$ . The covariance matrices of ${\mathit{\sigma }}_i$ and ${\mathit{\sigma }}_{\text{co}}$ are the 6D vectors that are the square roots of the diagonal elements of ${\mathbf{\Sigma }}_i$ and ${\mathbf{\Sigma }}_{\text{co}}$ , respectively.

Thus, the current pose of ${\widehat{\mathit{\xi }}}_i$ may be a loop closed with ${\widehat{\mathit{\xi }}}_j$ in the map only if it satisfies the relationship ${\widehat{\mathit{\xi }}}_j\in [{\widehat{\mathit{\xi }}}_i-1.96{\mathit{\sigma }}_i-1.96{\mathit{\sigma }}_{\text{co}},{\widehat{\mathit{\xi }}}_i+1.96{\mathit{\sigma }}_i+1.96{\mathit{\sigma }}_{\text{co}}]$ .

Then, we can obtain the final 95% confidence interval of loop closure according to $[{\widehat{\mathit{\xi }}}_j-1.96{\mathit{\sigma }}_i-1.96{\mathit{\sigma }}_{\text{co}},{\widehat{\mathit{\xi }}}_j+1.96{\mathit{\sigma }}_i+1.96{\mathit{\sigma }}_{\text{co}}]$ . Only the poses within this interval are considered loop closure candidates, and then be used for the pose-appearance-based LCD.

Pose-appearance-based LCD

Based on the pose-based loop closure probability, the pose-appearance-based LCD framework can be built, as illustrated in Figure 2. The appearance similarity method, such as BoW-based or CNN-based appearance similarity, can be employed as the appearance-based model in the joint framework.

Pose-appearance-based loop closure probability

To detect the loop closures using both the appearance and the pose information, the appearance similarity of s is multiplied by the pose-based loop closure probability of p_p , after which we can obtain the pose-appearance-based loop closure probability: $p_{\text{pa}}=p_p\cdot s$ . The p_p can be calculated using equation (20), and the s can be calculated using any other appearance-based LCD method, such as the BoW-LCD or the CNN-LCD.

For the BoW-based appearance similarity, let v and w be the two BoW vectors that are pre-normalized. Then, the fast $\ell 1$ -norm of the two BoW vectors ⁴ is calculated as

$||\mathit{v}-\mathit{w}{||}_1=2+{\displaystyle \sum _{v_i\ne 0w_i\ne 0}}(|v_i-w_i|-|v_i|-|w_i|)$

21

Since $||\mathit{v}-\mathit{w}{||}_1\sim [0,2]$ , the similarity of the two images can be calculated based on the $\ell \mathbf{1}$ -norm of the two BoW vectors as

$s=1-0.5||\mathit{v}-\mathit{w}{||}_1,\hspace{0.17em}\hspace{1em}s\sim [0,1]$

22

For CNN-based appearance similarity, the output of CNN, that is, the output of the activation function, can be also normalized to be the substitution of appearance similarity s.

Finally, the pose-appearance-based loop closure probability of $p_{\text{pa}}$ is used to detect loop closures. A threshold can be set for this probability to refine the loop closure candidates, and then use the consistency check to determine the loop closure.

SC check and the GC check

Similar to the appearance-based LCD method, the SC check and the GC check are introduced in the proposed method. These two checks are optional and commonly used for LCD methods. ⁴⁸

The SC check is designed to work very efficiently but with limited performance. A loop candidate is kept only if two or more sequential images are detected to be the same loop closure candidates. The SC check is an optional module for most LCD methods, as well as the proposed method. It can only work for continuous SLAM without kidnapping. Considering the proposed method relies on the continuous pose estimation, and do not deal with the kidnapping case, the SC check is recommended.

The SC check is mostly considered as the prepossess of the global optimization after LCD, instead of part of the loop detection. During the GC check, the fundamental matrix of the image pair is checked based on their keypoint matches. The loop closure candidate is confirmed only if there are enough inliers for the fundamental matrix. Then the keypoint matches are employed as the constraints of the global optimization. In this reason, no matter whether the GC check is part of the LCD methods, it is always necessary for LCD tasks. The GC check has a remarkable performance at filtering out false loops, albeit with a high computational cost.

Experimental evaluation

The main focus of this work is to improve the efficiency and accuracy of LCD process using the complementarity of pose and appearance. Our experiments are designed to show the capabilities of our method and to support our key claims, which are: (i) The introduction of pose-based information into the framework will accelerate the LCD process. (ii) The proposed LCD pipeline is compatible with many other methods and easy to be used in real-time SLAM applications.

Methodology

Datasets

Two publicly available datasets are used in the experiments, namely, the KITTI dataset ⁴⁹ and the TUM dataset. ⁵⁰ The KITTI dataset is a dataset of outdoor driving, and the TUM dataset is captured in an indoor environment with a handheld RGB-D camera. Note that only some image sequences of the two datasets consist of loop closures: KITTI00, KITTI02, KITTI05, TUMfreiburg2desk, and TUMfreiburg3longoffice. These image sequences are employed for the LCD experiments. All loop image pairs of the ground truth are manually labeled; some image pairs are illustrated in Figure 3.

Figure 3.

Some image pairs of the loop closure ground truth, where the ground truth is manually labeled. (a) KITTI dataset and (b) TUM dataset.

Metrics

The performance of the proposed method is evaluated with the precision and recall metrics. The precision is defined as the ratio of true positive LCDs to the total number of detections. The recall is defined as the ratio of true positive LCDs to the number of ground truth loop closures. An LCD result is considered to be truly positive only if the loop image pair is listed in the ground truth; otherwise, this LCD result is considered to be false. The threshold of the pose-appearance-based probability is continuously tuned during the experiments, and the precision–recall curves are drawn and analyzed.

For the precision–recall curve, the area under the curve (AUC) represents the comprehensive performance of the method. So the AUC is calculated for every comparing LCD method. The average time consumption per frame is also recorded to evaluate the real-time performance.

Program and settings

We implement the proposed LCD method in the framework of ORB-SLAM2, ³⁹ where the LCD process is replaced by the proposed method. The proposed method and all methods used for comparison run together in the program, and the trajectory optimization of the loop closure is disabled in the program to maintain consistency within the experimental conditions among all methods. The program runs on a computer with a 2.4 GHz quad-core processor. The ORB feature is used in all experiments, and the number of features per image is set to be 1000 in consideration of the computational capability of the computer. The program runs at 30 FPS with these settings. The code is open source and can be downloaded from https://github.com/nubot-nudt/PALoop

Efficiency

The first experiment is designed to show the efficiency of our approach and to support the claim that the introduction of pose-based information will accelerate the LCD process. The time consumption of each of the four components is recorded during the LCD, and the average time consumption per loop image pair is illustrated in Figure 1(b).

As shown in Figure 1(b), the time consumption of the GC check is excessively high. Thus, the total time consumption will linearly increase with the amount of the loop candidates when detecting loop closures, as shown in Figure 1(a). Note that most LCD methods do not take the GC check module in themselves. However, the GC check is necessary for LCD of real-time SLAM, because it provides the observation constraints for the global optimization after the loop closure. So there are as many GC checks as the loop candidates. Thats why the time consumption of GC checks is taken into consideration in this real-time LCD experiment.

With the introduction of pose-based information into the framework, the loop candidates are efficiently refined, and the time consumption of the proposed method remains much lower.

The pose-based loop candidates together with the appearance-based loop candidates for one current frame are illustrated in Figure 4. Both the pose-based and the appearance-based modules detect many loop candidates. But the two kinds of candidates are distributed differently with complementarity: The pose-based candidates congregate around the current pose, while the appearance-based candidates disperse over the whole map. The two kinds of candidates overlap each other around the ground truth. Then with the help of the pose-based fast loop closure candidate filtering, only part of the map is necessary for the loop closure consistency check. And thus, the proposed method combining the pose-based information and the appearance-based information has a good efficiency performance.

Figure 4.

Loop closure candidates of one demonstrative query frame using the pose-based probability module (PP) and the appearance-based appearance similarity module (AP). The pose-based candidates and the appearance-based candidates are distributed differently with complementarity. (a) Query frame, (b) correct frame of GT, (c) wrong frame of BoW, and (d) wrong frame of PP. GT: ground truth; BoW: bag-of-words.

The pose covariance matrix is calculated along with the odometry. We visualize the calculation results of the pose variance to validate the algorithm. The 95% translation confidence interval is drawn in Figure 5.

Figure 5.

The 95% confidence intervals of the pose illustrated as cubes along the trajectory.

The pose variance increases with the progression of the odometry, as shown in Figure 5. This means the more loop candidates will be kept by the pose-based module which result in the increasing of time-cost. But the time-cost is still low comparing to the appearance-based module, as shown in Figure 1. Note that if the robot estimates the pose variance with a low precision in some challenging environments, the 95% threshold can be tuned higher to get a larger confidence interval. Caution that the higher thresh will result in an increase of the time-cost, and an extreme case is the infinite confidence interval which would result in the traditional appearance-based global LCD. This case mostly occurs with the failure of localization which results in the infinite localization error. On the other hand, a smaller threshold may cause the confidence interval too small to detect any loop closures. All the experiments in this work use 95% as the threshold.

Accuracy

The next set of experiments is designed to show the accuracy of our approach and to support the claim that the proposed LCD pipeline is compatible with many other methods and easy to be used in real-time SLAM applications. We, therefore, tested our approach on two public datasets and is running in real time together with a SLAM method of state-of-the-art, the ORB-SLAM. ³⁹

Comparing LCD methods are employed as the appearance-based module in the proposed framework, which are the FAB, ⁵¹ DBOW, ⁹ CNN-LCD, ²⁸ NetVLAD, ²⁴ AlexNet, ²⁵ CALC, ²⁵ HOG, ²⁶ and CoHOG. ²⁷ And the LCD experiments are conducted for the three pose-appearance-based examples compared with the original methods. The experimental results are illustrated in Table 1.

Table 1.

The area under curve (AUC) and the average time consumption performance of different LCD methods with/without the proposed LCD pipeline.

Method	KITTI00		KITTI02		KITTI05		TUMfreiburg2desk		TUMfreiburg3longoffice		Mean Average
	AUC	TpF (ms)	AUC	TpF (ms)	AUC	TpF (ms)	AUC	TpF (ms)	AUC	TpF (ms)	AUC	TpF (ms)
DBOW	0.60	112.67	0.39	405.66	0.53	231.20	0.17	3.05	0.20	4.86	0.38	151.49
DBOW+SC	0.66	103.98	0.43	375.18	0.61	213.44	0.18	2.52	0.24	4.03	0.43	139.83
DBOW+PP	0.71	4.05	0.63	10.77	0.63	6.24	0.43	0.20	0.60	0.71	0.60	4.40
DBOW+PP+SC	0.71	3.58	0.57	9.48	0.65	5.68	0.39	0.18	0.61	0.58	0.59	3.90
FAB-MAP	0.26	30.76	0.06	31.31	0.20	25.72	0.05	13.28	0.04	13.63	0.12	22.94
FAB-MAP+SC	0.27	29.87	0.04	30.38	0.21	24.50	0.04	10.09	0.51	10.03	0.22	20.97
FAB-MAP+PP	0.56	1.11	0.21	0.77	0.51	0.64	0.50	0.41	0.49	0.87	0.46	0.76
FAB-MAP+SC+PP	0.59	0.99	0.45	0.71	0.54	0.57	0.45	0.34	0.68	0.58	0.54	0.64
CNN	0.00	16.29	0.01	16.11	0.02	18.05	0.09	57.93	0.09	63.33	0.04	34.34
CNN+SC	0.00	15.72	0.01	15.52	0.08	17.29	0.40	57.06	0.06	58.76	0.11	32.87
CNN+PP	0.01	0.53	0.01	0.40	0.03	0.37	0.52	2.22	0.45	5.82	0.20	1.87
CNN+SC+PP	0.01	0.49	0.01	0.37	0.10	0.35	0.53	2.02	0.53	5.26	0.24	1.70
VLAD	0.78	1007.79	0.72	1010.93	0.68	1006.51	0.54	1012.97	0.42	996.90	0.63	1007.02
VLAD+SC	0.75	983.27	0.67	987.24	0.68	983.53	0.67	982.66	0.41	956.08	0.64	978.56
VLAD+PP	0.78	38.96	0.72	28.66	0.74	30.93	0.62	57.30	0.56	146.82	0.68	60.53
VLAD+SC+PP	0.75	36.54	0.67	26.60	0.76	29.70	0.73	52.84	0.53	137.16	0.69	56.57
AlexNet	0.71	693.70	0.72	686.02	0.66	682.31	0.15	679.13	0.27	710.75	0.50	690.38
AlexNet+SC	0.68	688.01	0.66	679.93	0.64	676.91	0.19	670.95	0.24	698.51	0.48	682.86
AlexNet+PP	0.72	23.66	0.72	17.27	0.67	16.08	0.48	26.31	0.59	71.19	0.64	30.90
AlexNet+SC+PP	0.70	22.56	0.67	16.24	0.66	15.67	0.52	24.57	0.58	67.55	0.62	29.32
CALC	0.69	50.24	0.64	51.74	0.62	49.34	0.18	46.42	0.23	45.62	0.47	48.67
CALC+SC	0.65	49.95	0.58	51.40	0.60	49.05	0.13	46.14	0.28	45.34	0.45	48.38
CALC+PP	0.71	1.67	0.64	1.30	0.65	1.11	0.53	1.78	0.57	4.37	0.62	2.05
CALC+SC+PP	0.67	1.60	0.59	1.23	0.63	1.08	0.50	1.68	0.61	4.20	0.60	1.96
CoHOG	0.38	3680.42	0.52	4284.12	0.50	2349.37	0.59	326.02	0.51	288.77	0.50	2185.74
CoHOG+SC	0.33	3678.99	0.50	4282.21	0.49	2348.25	0.58	325.32	0.49	287.73	0.48	2184.50
CoHOG+PP	0.45	119.87	0.53	103.88	0.67	49.81	0.70	12.44	0.75	27.37	0.62	62.67
CoHOG+SC+PP	0.41	115.13	0.50	98.63	0.67	48.86	0.69	11.81	0.75	26.39	0.61	60.16
HOG	0.17	93.25	0.41	107.49	0.39	59.37	0.10	13.56	0.04	12.20	0.22	57.17
HOG+SC	0.10	93.03	0.41	107.30	0.35	59.24	0.06	13.53	0.24	12.16	0.23	57.05
HOG+PP	0.40	3.03	0.45	2.62	0.44	1.27	0.40	0.51	0.32	1.14	0.40	1.72
HOG+SC+PP	0.31	2.91	0.43	2.48	0.40	1.25	0.41	0.48	0.52	1.10	0.41	1.64

TpF: time consumption per frame; LCD: loop closure detection; HOG: histogram-of-oriented-gradients. The bold values indicate the outstanding results.

Some precision and recall curves are illustrated in Figures 6 and 7. The precision curves and the recall curves are also provided together with the precision–recall curves to analyze the details of the precision and recall performance, respectively.

Figure 6.

Precision, recall, and precision–recall curves of different methods on the KITTI00 image sequence (PP indicates the pose-based probability. SC indicates the sequential consistency check).

Figure 7.

Precision, recall, and precision–recall curves of the CNN-based method on the TUMfreiburg2desk (the top row) and the TUMfreiburg3longoffice (the bottom row) image sequences.

Note that the SC check and GC check module are optional and are widely used in many other methods. These two modules are not the contributions of this work so that they are not discussed in this experiment. Moreover, the GC module is a strict but time-consuming module. The introduction of GC will perform a 100% precision for most methods with a heavy computational burden, which is already illustrated in Figure 3.

As shown in Table 1, the introduction of the pose-based probability can greatly improve the LCD performance. The AUC (i.e., precision and recall performance) is greatly increased for all the experimental methods. The time consumption per frame (TpF) is decreased by orders of magnitude, showing the extraordinary real-time performance. Note that this TpF is the average time during the whole SLAM, and the time consumption is increasing with the increase of map, as shown in Figure 1. On the other hand, the introduction of the SC check can also improve the LCD performance, but the improvement is limited comparing to the pose-based probability.

The precision curves in Figures 6 and 7 show that the pose-based probability and the SC check can improve the precision performance of the appearance-based method. However, the recall performance, as shown in the recall curves in Figures 6 and 7, will not be better than the appearance-based results because the introduction of any other component after the appearance-based component can filter out some candidates and cannot add candidates. Finally, the large precision increase and small recall decrease results in an improvement of the precision–recall performance.

In summary, our evaluation suggests that our method provides competitive efficiency and accuracy in LCD. The introduction of pose-based information into the framework will greatly accelerate the LCD process. And meanwhile, the proposed method achieves good precision–recall performance in real-time SLAM applications.

Conclusions

In this article, we presented a novel approach to detect loop closures for SLAM. Our approach exploited both pose-based and appearance-based information in a probabilistic manner, inspired by the complementarity between these two kinds of information. This allows the proposed method to achieve higher efficiency and accuracy performance in LCDs. We implemented and evaluated our approach on different datasets and provided comparisons to other existing techniques. The experimental evaluation suggests that our method provides competitive efficiency and accuracy in LCDs.

However, there are certain aspects of the proposed method that need to be improved. First, the introduction of the pose information limits the application of the method, which makes it unable to deal with the kidnapping of the robot. Our plan is to introduce more powerful but less fast appearance method, such as deep learning based method, to re-localize in the global map. Second, the trend of online incrementally update of the appearance vocabularies or other appearance models can be adopted to enhance the adaptability of long-time SLAM.