Sequence-based sparse optimization methods for long-term loop closure detection in visual SLAM

Abstract

Loop closure detection is one of the most important module in Simultaneously Localization and Mapping (SLAM) because it enables to find the global topology among different places. A loop closure is detected when the current place is recognized to match the previous visited places. When the SLAM is executed throughout a long-term period, there will be additional challenges for the loop closure detection. The illumination, weather, and vegetation conditions can often change significantly during the life-long SLAM, resulting in the critical strong perceptual aliasing and appearance variation problems in loop closure detection. In order to address this problem, we propose a new Robust Multimodal Sequence-based (ROMS) method for robust loop closure detection in long-term visual SLAM. A sequence of images is used as the representation of places in our ROMS method, where each image in the sequence is encoded by multiple feature modalites so that different places can be recognized discriminatively. We formulate the robust place recognition problem as a convex optimization problem with structured sparsity regularization due to the fact that only a small set of template places can match the query place. In addition, we also develop a new algorithm to solve the formulated optimization problem efficiently, which guarantees to converge to the global optima theoretically. Our ROMS method is evaluated through extensive experiments on three large-scale benchmark datasets, which record scenes ranging from different times of the day, months, and seasons. Experimental results demonstrate that our ROMS method outperforms the existing loop closure detection methods in long-term SLAM, and achieves the state-of-the-art performance.

Appendix Proof of Lemma 1:

For any vector $\tilde{\mathbf {v}}$ and $\mathbf {v}$, the following inequality holds: $\Vert \tilde{\mathbf {v}}\Vert _2 - \frac{\Vert \tilde{\mathbf {v}}\Vert _2^2}{2\Vert \mathbf {v}\Vert _2} \le \Vert \mathbf {v}\Vert _2 - \frac{\Vert \mathbf {v}\Vert _2^2}{2\Vert \mathbf {v}\Vert _2}$.

Proof

Obviously, the inequality $-(\Vert \tilde{\mathbf {a}}\Vert _2 - \Vert \mathbf {a}\Vert _2)^2 \le 0$ holds. Thus, we have:

$$\begin{aligned}&-(\Vert \tilde{\mathbf {v}}\Vert _2 - \Vert \mathbf {v}\Vert _2)^2 \le 0 \Rightarrow 2\Vert \tilde{\mathbf {v}}\Vert _2\Vert \mathbf {v}\Vert _2 - \Vert \tilde{\mathbf {v}}\Vert _2^2 \le \Vert \mathbf {v}\Vert _2^2 \\&\Rightarrow \Vert \tilde{\mathbf {v}}\Vert _2 - \frac{\Vert \tilde{\mathbf {v}}\Vert _2^2}{2\Vert \mathbf {v}\Vert _2} \le \Vert \mathbf {v}\Vert _2 - \frac{\Vert \mathbf {v}\Vert _2^2}{2\Vert \mathbf {v}\Vert _2} \end{aligned}$$

This completes the proof. $\square$

1.1 Proof of Theorem 1:

Algorithm 1 monotonically decreases the objective value of the problem in Eq. 8 in each iteration.

Proof

Assume the update of $\mathbf {A}$ is $\tilde{\mathbf {A}}$. According to Step 6 in Algorithm 1, we know that:

$$\begin{aligned} \tilde{\mathbf {A}} =\,&\underset{\mathbf {A}}{\mathrm {arg\,min}}\; Tr((\mathbf {DA} - \mathbf {B})\mathbf {U}(\mathbf {DA}-\mathbf {B})^\top ) \nonumber \\&+ \lambda _1 Tr(\mathbf {A}^\top \mathbf {VA}) + \lambda _2 \sum _{i=1}^{s} \mathbf {a}_i^\top \mathbf {W}^i \mathbf {a}_i \,, \end{aligned}$$

(12)

where $Tr(\cdot )$ is the trace of a matrix. Thus, we can derive

$$\begin{aligned}&Tr((\mathbf {D}\tilde{\mathbf {A}} - \mathbf {B})\mathbf {U}(\mathbf {D}\tilde{\mathbf {A}}-\mathbf {B})^\top ) \nonumber \\&\qquad + \lambda _1 Tr(\tilde{\mathbf {A}}^\top \mathbf {V}\tilde{\mathbf {A}}) + \lambda _2 \sum _{i=1}^{s} \tilde{\mathbf {a}}_i^\top \mathbf {W}^i \tilde{\mathbf {a}}_i \nonumber \\&\quad \le Tr((\mathbf {DA} - \mathbf {B})\mathbf {U}(\mathbf {DA}-\mathbf {B})^\top ) \nonumber \\&\qquad + \lambda _1 Tr(\mathbf {A}^\top \mathbf {VA}) + \lambda _2 \sum _{i=1}^{s} \mathbf {a}_i^\top \mathbf {W}^i \mathbf {a}_i \end{aligned}$$

(13)

According to the definition of $\mathbf {U}$, $\mathbf {V}$, and $\mathbf {W}$, we have

$$\begin{aligned}&\sum _{i=1}^{s} \left( \frac{\Vert \mathbf {D}\tilde{\mathbf {a}}_i - \mathbf {b}_i\Vert ^2_2}{2\Vert \mathbf {D}\mathbf {a}_i - \mathbf {b}_i\Vert _2} + \lambda _1 \frac{\Vert \tilde{\mathbf {a}}\Vert _2^2}{2\Vert \mathbf {a}\Vert _2} + \lambda _2 \sum _{j=1}^{k} \frac{\Vert \tilde{\mathbf {a}}_i^j\Vert ^2_2}{2\Vert \mathbf {a}_i^j\Vert _2} \right) \nonumber \\&\quad \le \sum _{i=1}^{s} \left( \frac{\Vert \mathbf {D}\mathbf {{a}}_i - \mathbf {b}_i\Vert ^2_2}{2\Vert \mathbf {D}\mathbf {a}_i - \mathbf {b}_i\Vert _2} + \lambda _1 \frac{\Vert \mathbf {{a}}\Vert _2^2}{2\Vert \mathbf {a}\Vert _2} + \lambda _2 \sum _{j=1}^{k} \frac{\Vert \mathbf {{a}}_i^j\Vert ^2_2}{2\Vert \mathbf {a}_i^j\Vert _2} \right) \end{aligned}$$

(14)

According to Lemma 1, we can obtain the following inequalities:

$$\begin{aligned}&\sum _{i=1}^{s} \left( \Vert \mathbf {D}\tilde{\mathbf {a}}_i - \mathbf {b}_i\Vert _2 - \frac{\Vert \mathbf {D}\tilde{\mathbf {a}}_i - \mathbf {b}_i\Vert ^2_2}{2\Vert \mathbf {D}\mathbf {a}_i - \mathbf {b}_i\Vert _2} \right) \nonumber \\&\quad \le \sum _{i=1}^{s} \left( \Vert \mathbf {D}\mathbf {{a}}_i - \mathbf {b}_i\Vert _2 - \frac{\Vert \mathbf {D}\mathbf {{a}}_i - \mathbf {b}_i\Vert ^2_2}{2\Vert \mathbf {D}\mathbf {a}_i - \mathbf {b}_i\Vert _2} \right) \nonumber \\&\sum _{i=1}^{s} \left( \Vert \tilde{\mathbf {a}}\Vert _2 - \lambda _1 \frac{\Vert \tilde{\mathbf {a}}\Vert _2^2}{2\Vert \mathbf {a}\Vert _2} \right) \le \sum _{i=1}^{s} \left( \Vert \mathbf {{a}}\Vert _2 - \lambda _1 \frac{\Vert \mathbf {{a}}\Vert _2^2}{2\Vert \mathbf {a}\Vert _2} \right) \nonumber \\&\sum _{i=1}^{s}\sum _{j=1}^{k} \left( \Vert \tilde{\mathbf {a}}_i^j\Vert _2 - \frac{\Vert \tilde{\mathbf {a}}_i^j\Vert ^2_2}{2\Vert \mathbf {a}_i^j\Vert _2} \right) \le \sum _{i=1}^{s}\sum _{j=1}^{k} \left( \Vert \mathbf {{a}}_i^j\Vert _2 - \frac{\Vert \mathbf {{a}}_i^j\Vert ^2_2}{2\Vert \mathbf {a}_i^j\Vert _2} \right) \end{aligned}$$

(15)

Computing the summation of the three equations in Eq. 15 on both sides (weighted by $\lambda$s), we obtain:

$$\begin{aligned}&\sum _{i=1}^{s} \Vert (\mathbf {D}\tilde{\mathbf {a}}_i - \mathbf {b}_i)^\top \Vert _{2} + \lambda _1 \Vert \tilde{\mathbf {a}}\Vert _{2} + \lambda _2 \Vert \tilde{\mathbf {a}}\Vert _{2} \nonumber \\&\quad \le \sum _{i=1}^{s} \Vert (\mathbf {D}\mathbf {{a}}_i - \mathbf {b}_i)^\top \Vert _{2} + \lambda _1 \Vert \mathbf {{a}}\Vert _{2} + \lambda _2 \Vert \mathbf {{a}}\Vert _{2} \end{aligned}$$

(16)

Therefore, Algorithm 1 monotonically decreases the objective value in each iteration.$\square$