Efficient Map Sparsification Based on 2D and 3D Discretized Grids

To realize autonomous navigation for robots, localization in a pre-built map is a basic technique. Lots of algorithms have been proposed for mapping and localization using different sensors, including camera [20] and lidar [27]. These algorithms commonly work well in some small-scale environments now. When applied in large-scale environments or in long-term, however, new challenges appear and need to be settled for practical applications. Some of these problems are time and memory consuming.

When using cameras, visual simultaneous localization and mapping (SLAM) is a commonly used method to build maps. In visual SLAM, redundant features are extracted from images and then constructed as landmarks in a map, such that camera poses can be tracked robustly and accurately. These redundant landmarks promise good localization results in small-scale environments. As more and more images are received when working in large-scale environments or in long-term, however, memory consumption is increasing unboundedly. Localizing in such large maps will also be more time-consuming. These problems are especially severe for some low-cost robots.

Actually, not all landmarks are necessary for robots to localize in pre-built maps. In theory, even only 4 matched landmarks can determine a camera pose using EPnP [16] (more landmarks are commonly used for robust and accurate estimation). This reveals that maps can be compressed and still retain comparable performance for localization. Map compression can be classified into two types: descriptor compression [18, 22, 5] and landmark sparsification [17, 23]. The research of this paper falls in the latter one, which is to find a subset of an original map while maintaining comparable localization performance. A subset map is called compact map in this paper. For example, as indicated in  Fig. 1, only $3.91\%$ of the original landmarks are selected as the compact map, in which more than $96\%$ of the query images can still be localized successfully.

To select an optimal subset for localization, map sparsification is related to a $K$-cover problem [17], which means the number of landmarks in a compact map is minimized while keeping the number of associated landmarks in each image larger than a threshold (i.e., $K$). To solve the $K$-cover problem, it can be formulated as mixed-integer linear programming, through which an optimal subset is obtained. The original formulation only considers the number of landmarks for localization, while their distribution also affects localization performance. Therefore, some works design and add quadratic terms, formulating mixed-integer quadratic programming to enforce a more uniform distribution of selected landmarks [23, 11]. However, these quadratic terms slow down the optimization speed heavily. The required high memory capacity and heavy computation become severe limitations of these map sparsification methods. For example, in our experiments, the mixed-integer quadratic programming methods cannot be used for the maps containing more than 55K landmarks, because all computer memory has been consumed.

To select uniformly distributed landmarks and at the same time maintain the computation efficiency, we keep map sparsification formulation in a linear form in this paper. We firstly discretize images into 2D fix-sized grids. Then for all observed landmarks, we can find which cells they fall in. Therefore, more occupied cells reflect a more uniform distribution of landmarks. This can be formulated in a linear form easily. In this way, uniformly distributed landmarks are selected efficiently, and thus the localization performance in compact maps will be better.

Another severe limitation is that all of past works assume the spatial distribution of the query sequence is close to that of the mapping sequence. Then landmarks are selected only based on their association with the images of the mapping sequence. However, this assumption cannot be guaranteed in real robotic applications. The perspective difference between query and mapping sequences may cause the localization in compact maps to fail unexpectedly.

To ensure more query images from whole 3D space can be localized successfully in compact maps, we propose to select landmarks based on not only their association with mapping images but also their visibility in 3D space. The visibility region of a landmark is defined based on its viewing angle and distance. The 3D space is also discretized into a 3D discretized grid. For each 3D cell, all visible landmarks are collected, and a constraint on the number of visible landmarks is added into map sparsification formulation. In this way, landmarks are selected to maintain localization performance for query images from the whole space.

In summary, the contributions of this paper are as follows: 1) We propose an efficient map sparsification method formulating uniform landmark distribution in a linear form to keep the computation efficiency. 2) We propose to perform map sparsification involving the visibility of landmarks to achieve better localization results for the query images from the whole space. 3) We conduct exhaustive experiments in different datasets and compare with other state-of-the-art methods, showing the effectiveness and superiority of our methods for different kinds of query sequences.

Map compression or sparsification has received many research attention in SLAM and structure from motion (SfM) because of the burden of maintaining and utilizing a large map. Different methods are also proposed to solve this problem. In [18, 22, 5], the descriptors are quantized to compact forms to reduce memory consumption. Mera-Trujillo et al. [19] propose a constrained quadratic program method and design an efficient solver to select landmarks. Contreras et al. [8] develop a map compression method based on traveled trajectories, and they also propose an online system performing SLAM and map compression simultaneously [9]. Some SLAM systems [26, 28, 7] are also designed to select and build a compact map online, but much more landmarks are still remained compared with the offline methods. Recently, some learning-based map sparsification methods have also appeared and achieved good results [25, 4], but they require high-cost GPUs to train and struggle to be applied in different unseen scenes.

Map sparsification is more commonly regarded as a $K$-cover problem. Since $K$-cover is an NP-hard problem, Li et al. [17] propose to use a greedy algorithm to select the landmark observed by the largest number of not-yet-fully covered frames. Cao et al. [3] also use a greedy algorithm to select landmarks, considering both the coverage and distinctiveness. In [6], the original $K$-cover method is improved by predicting the parameter $K$. Similarly, the landmarks are scored and sampled based on the observation count, covariance, and descriptor stability in [12]. Moreover, the authors come up with more scoring factors to rank landmarks for selecting in [13]. These methods can provide compact maps for localization, but depend on a careful design of selecting methods and cannot get the optimal solution.

Another $K$-cover based map sparsification solution is to be formulated as a mixed-integer programming problem, which is introduced in detail in [23]. The linear formulation does not consider the distribution of the selected landmarks but can be solved efficiently, and therefore it is adopted in some later works [13, 24]. The mixed-integer quadratic programming formulation encourages the uniformly distributed landmarks but requires a high capacity of memory and heavy computation [23] and becomes impractical for large maps. To improve the computation efficiency, Dymczyk et al. [11] have to divide the whole map into several small parts and perform map sparsification separately. Camposeco et al. [2] keep the linear form of map sparsification but divide each image into $q$ uniformly-sized cells and let each cell be covered by $K/q$ landmarks. This method seems reasonable, but we find in the experiments that the number of corresponding selected landmarks in some images is much less than $K$ actually, and thus the localization results get worse. Similarly, the proposed method also makes use of discretized images, but the image is still considered as a whole and thus avoids the above problems.

All of the above methods are based on the existing association between landmarks and images, thus landmarks are selected only to ensure localization performance of these images. For an arbitrary query image with a different viewing perspective, localization may fail unexpectedly, especially when the compression ratio is high. To solve this problem, the proposed method considers landmark visibility in the 3D space and adds constraints to preserve enough visible landmarks for the whole space in compact maps.

The proposed method is designed mainly for the maps built from feature-based visual SLAM systems (e.g., ORB-SLAM [20]). Receiving sequences of images, the feature points observed by keyframes are constructed as landmarks. Thus, an original map $\mathcal{M}$ consists of landmarks $\left\{\boldsymbol{p}_{i}\right\}_{i=1,2,\dots,N}$, keyframes $\left\{\boldsymbol{F}_{j}\right\}_{j=1,2,\dots,M}$, and their association. The map sparsification aims to decrease the number of landmarks. We firstly introduce commonly used formulations of map sparsification in past works [23].

Using mixed-integer programming, map sparsification can be formulated as:

$$\begin{array}[]{cl}\underset{\mathbf{x},\boldsymbol{\xi}}{\operatorname{minimize}}&\mathbf{q}^{\top}\mathbf{x}+\lambda\mathbf{1}^{\top}\boldsymbol{\xi}\\ &\mathbf{Ax}\geq K\mathbf{1}-\boldsymbol{\xi}\\ \operatorname{subject\ to}&\mathbf{x}\in\left\{0,1\right\}^{N}\\ &\boldsymbol{\xi}\in\left\{0,\mathbb{Z}_{+}\right\}^{M}\end{array}$$

(1)

where $\mathbf{x}$ is a binary vector whose $i^{\operatorname{th}}$ element indicates whether the landmark $\boldsymbol{p}_{i}$ is selected or not. $\mathbf{q}$ is a weight vector to select landmarks. $\mathbf{A}$ is a binary matrix describing the association between landmarks and keyframes, i.e., the element $\mathbf{A}_{ij}$ indicates whether the landmark $\boldsymbol{p}_{j}$ is visible in the keyframe $\boldsymbol{F}_{i}$. $K$ is the desired minimum number of visible landmarks in each keyframe and can be used to adjust compression ratio. To handle the situation when the total number of associated landmarks of a keyframe is less than $K$, a slack variable $\boldsymbol{\xi}$ is used to relax the hard constraints, and $\lambda$ determines the hardness of the constraints.

Therefore, Eq. 1 describes the mixed-integer linear programming method for map sparsification. The distribution of selected landmarks is not considered in this method. To involve the distribution of landmarks, quadratic terms are commonly used:

$$\begin{array}[]{cl}\underset{\mathbf{x},\boldsymbol{\xi}}{\operatorname{minimize}}&\frac{1}{2}\mathbf{x}^{\top}\mathbf{Q}\mathbf{x}+\mathbf{q}^{\top}\mathbf{x}+\lambda\mathbf{1}^{\top}\boldsymbol{\xi}\\ &\mathbf{Ax}\geq K\mathbf{1}-\boldsymbol{\xi}\\ \operatorname{subject\ to}&\mathbf{x}\in\left\{0,1\right\}^{N}\\ &\boldsymbol{\xi}\in\left\{0,\mathbb{Z}_{+}\right\}^{M}\end{array}$$

(2)

where $\mathbf{Q}$ is a symmetric matrix. The element $\mathbf{Q}_{ij}$ indicates the relation between $\boldsymbol{p}_{i}$ and $\boldsymbol{p}_{j}$. For example, it can be the number of common observations for each pair of landmarks. Therefore, the landmarks that are observed together with others frequently would be discarded. As a result, selected landmarks are distributed more uniformly. Eq. 2 describes the commonly used map sparsification formulation in past works, which considers both the number and distribution of landmarks for localization.

Uniformly distributed landmarks commonly provide better localization results. Because of the quadratic terms, however, the computation complexity increases largely. Besides, Eq. 2 encourages selection of rarely observed landmarks, which may have larger errors and be detrimental for localization [11].

To solve these problems, we design a linear term to encourage a uniform distribution of selected landmarks. As illustrated in Fig. 2, the image is discretized into fixed-size grids (e.g., $C\times{R}$ cells). Each cell has two stats: empty or occupied. Based on the camera model, projected positions of landmarks can be computed easily. Then a cell is occupied if at least one landmark is projected into it; otherwise, it is empty. Therefore, to acquire uniformly distributed landmarks, we encourage selecting the landmarks that make more cells to be occupied. This concept can be easily formulated into Eq. 1, which becomes:

$$\begin{array}[]{cl}\underset{\mathbf{x},\boldsymbol{\xi},\boldsymbol{\phi}}{\operatorname{minimize}}&\mathbf{q}^{\top}\mathbf{x}+\lambda_{1}\mathbf{1}^{\top}\boldsymbol{\xi}+\lambda_{2}\mathbf{1}^{\top}\boldsymbol{\phi}\\ &\mathbf{Ax}\geq K\mathbf{1}-\boldsymbol{\xi}\\ &\mathbf{Bx}\geq\mathbf{1}-\boldsymbol{\phi}\\ \operatorname{subject\ to}&\mathbf{x}\in\left\{0,1\right\}^{N}\\ &\boldsymbol{\xi}\in\left\{0,\mathbb{Z}_{+}\right\}^{M}\\ &\boldsymbol{\phi}\in\left\{0,1\right\}^{M\times C\times R}\\ \end{array}$$

(3)

where $\mathbf{B}$ is a binary matrix describing the association between the landmarks and all cells, i.e., the element $\mathbf{B}_{ij}$ indicates whether the projected position of the landmark $\boldsymbol{p}_{j}$ falls within the cell $\boldsymbol{c}_{i}$. We encourage every cell to keep at least one corresponding landmark and use the slack variable $\boldsymbol{\phi}$ to relax this constraint. An example is illustrated in Fig. 2, the dotted points can be discarded without changing the number of occupied cells.

Eq. 3 formulates map sparsification as a mixed-integer linear programming problem and thus can be solved efficiently. The selected landmarks are directly encouraged to spread uniformly in all corresponding keyframes. Thus better localization performance will be achieved.

From the previous section, it is noticed that map sparsification is formulated based on the association between landmarks and keyframes. This association can be easily constructed from feature matching during mapping. All past works are based on such association and assume the spatial distribution of the query sequence will be close to that of the mapping sequence. Thus selected landmarks can be matched again in query images. Obviously, this assumption cannot be promised in real robotic applications. Some selected landmarks would not be matched in query images because of different viewing perspectives, and thus, the localization may fail unexpectedly.

In this section, we will introduce our proposed map sparsification method that involves landmark visibility in 3D space and thus improves localization performance for query images from the whole space.

The proposed map sparsification based on 2D grids (i.e., Eq. 3) is firstly compared with previous works to show its superiority in performance and efficiency for localizing query images close to mapping sequences. Then we demonstrate the effectiveness of map sparsification involving the visibility of landmarks in 3D space (i.e., Eq. 6) and its superiority when query images are not close to mapping sequences. More experimental results can be found in the supplementary material, including visualization results and the comparison between original and compact maps in terms of memory consumption and localization efficiency.

In this paper, we propose two novel terms for efficient map sparsification. The first term is to enforce a uniform distribution of selected landmarks based on 2D discretized grids. The second one adds a space constraint based on landmark visibility to weaken the assumption that the spatial distribution of the query sequence is close to that of the mapping sequence. Both two terms are formulated in efficient linear forms, and thus avoid heavy computation. The proposed terms can be chosen and set according to different conditions of query sequences. The exhaustive experiments have demonstrated the effectiveness and superiority of the proposed method, especially the computation efficiency compared with previous QP methods.