CyberLoc: Towards Accurate Long-term Visual Localization

In this section, we introduce some image pre-processing steps before using images for sparse 3D map reconstruction.

We have pre-recorded database images with ground-truth 6-DoF camera poses in a world coordinate system. We perform image retrieval to find Top-K (K=$40$) similar database images. For each database image pair, we perform sparse feature matching and find 2D-2D matches. We further prune out wrong 2D-2D matches by enforcing the epipolar constraint using the ground-truth poses. With 2D-2D matches and ground-truth poses, we triangulate ²²2 SVD with post non-linear refinement. 3D points and perform structure-only bundle adjustment ³³3Fixing camera poses. to refine the positions of 3D points using colmap [12].

Given ground-truth 6-DoF camera poses of images, with SuperPoint feature points, we perform SLAM using the ptam [21]. Note that we skip the pose estimation stage in the SLAM and only optimize 3D points in the local map optimization stage.

Using the same retrieved pairs in Sec.2.3, we use dense feature matching method QTA [22] to build sparse 2D-2D matches and then triangulate 3D points in the same way as Sec.2.3. Since dense feature matching methods do not detect sparse 2D feature points and can generate unrepeatable 2D-2D matches ⁶⁶6Given 2D-2D matches for image pair A-B and A-C, 2D feature points in image A are different., we use the same quantization technique in Patch2Pix [23] to alleviate this problem.

For each image pair, we can estimate a dense disparity map using the pre-trained LEAStereo [25], thus obtaining a local 3D map for each database image. An example of the estimated disparity map is given in Figure 3.

Let $\mathbf{I}_{\mathsf{K}}=\{\mathbf{I}_{k}|k=1,\cdots,\mathsf{K}\}$ denotes a query image sequence up to timestamp $\mathsf{K}$. For each query image $\mathbf{I}_{k}$, we have $\mathsf{C}$ candidate 6-DoF poses $\mathcal{P}_{k}=\{\mathbf{P}_{k,i}=(\mathbf{R}_{k,i},\mathbf{t}_{k,i})|i=1,\cdots,\mathsf{C}\}$, one from a reference map.

We aim to find the most accurate 6-DoF pose $\mathbf{P}_{k,i^{*}}$ for $\mathbf{I}_{k}$.

For query images $\mathbf{I}_{m}$ and $\mathbf{I}_{n}$, we can obtain a set of 2D-2D matches $\mathcal{Q}_{m,n}$ via feature tracking in SLAM. Given query poses $\mathbf{P}_{m,i}$ and $\mathbf{P}_{n,j}$, we can compute the number of inlier 2D-2D matches subject to $\mathbf{P}_{m,i}$ and $\mathbf{P}_{n,j}$, by thresholding the Sampson errors [28] of 2D-2D matches $\mathcal{Q}_{m,n}$. We denote the number of inlier 2D-2D matches as $S_{m,i,n,j}$ and use it to measure the compatibleness of poses $\mathbf{P}_{m,i}$ and $\mathbf{P}_{n,j}$. We assume the best poses would generate the largest score $\sum S_{m,i,n,j}$, i. e., finding the largest consensus set. Though the problem is non-convex [29] and the original $\mathcal{Q}_{m,n}$ contains outlier matches, we found the score $\sum S_{m,i,n,j}$ describes the correctness of poses well. The consensus set maximization problem is solved by a integer programming process.

We use three frames $k,m,n$ to describe our integer programming process, as is given in Figure 4. We first build a densely connected graph for consecutive timestamps. For nodes $\mathbf{P}_{k,i}$ and $\mathbf{P}_{m,j}$, there is an edge $e_{k,i,j}\in\{0,1\}$ connecting them. The score of edge $e_{k,i,j}$ is $S_{k,i,m,j}$ (abbrev. $S_{k,i,j}$ for clarity), which is the number of inlier 2D-2D matches using poses $\mathbf{P}_{k,i}$ and $\mathbf{P}_{m,j}$. Our integer programming process is given by,

Eq.(2) enforces that exactly one edge should be selected for the connectivity of the graph. Eq.(3) enforces the following relationship: 1) if node $\mathbf{P}_{m,j}$ is selected, there must be an edge connecting $\mathbf{P}_{m,j}$ to other nodes in consecutive frames; 2) if node $\mathbf{P}_{m,j}$ is NOT selected, all edges connecting $\mathbf{P}_{m,j}$ to other nodes in consecutive frames should be removed.

There are a maximum ⁹⁹9If one map session fails to estimate a camera pose, the number of edges between consecutive timestamps is smaller than $\mathsf{C}\times\mathsf{C}$. of $(\mathsf{K}-1)\times\mathsf{C}\times\mathsf{C}$ edges (optimization variables) in our integer programming. The solution can be found very efficiently by off-the-shelf toolboxes. After optimization, nodes (poses) connected by edges $e_{k,i,j}=1$ are deemed as the best poses.

We validate the proposed consensus set maximization method on the oldtown and cityloop datasets, from the 4seasons dataset. The number of image sequences with ground-truth poses is four and three, for the oldtown and cityloop dataset, respectively. For each dataset, one sequence is used for testing and the rest sequences are used for mapping. The localization results are separately given in Table 2 and Table 3. The results show that the proposed consensus set maximization method is robust and consistently shows good performance on the two datasets. In contrast, the simple method of merging 2D-3D matches does not work on the cityloop dataset. The reason is that localization results using multi-session maps are not compatible, on the cityloop dataset.

We run our method on a PC equipped with a 2TB RAM and an AMD EPYC 7742 CPU. The running time of our method is given in Table 4. The implementation is based on Python and uses CBC solver for optimization. The running time can be significantly reduced using C++.

For each query image $\mathbf{I}_{k}$, we have obtained its best global pose $\mathbf{P}_{k}$ from the multi-session localization step (Sec. 4). Considering that the accuracy (even being an outlier) of $\mathbf{P}_{k}$ varies for different frames, we associate a weight variable $w_{k}\in[0,1]$ for each $\mathbf{P}_{k}$, describing the weight of $\mathbf{P}_{k}$ in our optimization process. We also retrieve 2D-3D matches $\mathcal{F}_{k}$ corresponding to $\mathbf{P}_{k}$, and denote the reprojection error at pose $\mathbf{X}_{k}$ as $\pi\left(\mathbf{X}_{k},\mathcal{F}_{k}\right)$.

For consecutive frames, we can obtain their local relative poses $\mathbf{Z}_{k-1,k}$ and 2D-2D matches $\mathcal{Q}_{k-1,k}$ through SLAM. Given query poses $\mathbf{X}_{k-1}$ and $\mathbf{X}_{k}$, we denote the two-view matching (Sampson) error as $\rho\left(\mathbf{X}_{k-1},\mathbf{X}_{k},\mathcal{Q}_{k-1,k}\right)$.

Given query images $\mathbf{I}_{\mathsf{K}}=\{\mathbf{I}_{k}|k=1,\cdots,\mathsf{K}\}$ up to timestamp $\mathsf{K}$, we aim to solve query poses $\mathcal{X}_{\mathsf{K}}=\{\mathbf{X}_{k}|k=1,\cdots,\mathsf{K}\}$ and latent weights $\mathcal{W}_{\mathsf{K}}=\{w_{k}|k=1,\cdots,\mathsf{K}\}$. The overall framework of our optimization problem is given in Fig. 5.

Our objective function is given by,

where $\left\|\cdot\right\|^{2}$ denotes the pose error, and $h\left(\mathbf{X}_{k-1},\mathbf{X}_{k}\right)$ denotes the relative pose between $\mathbf{X}_{k-1}$ and $\mathbf{X}_{k}$, $\lambda_{1}$ and $\lambda_{2}$ are two weights to balance the 2D-3D reprojection error and 2D-2D Sampson error.

Inspired by [30], we use Expectation-Maximization (EM) algorithm to solve Eq.(5.1). The latent weights $\mathcal{W}_{\mathsf{K}}$ and query poses $\mathcal{X}_{\mathsf{K}}$ are alternatively optimized until convergence (the change of $\mathcal{W}_{\mathsf{K}}$ is smaller than a threshold).

Based on the global localization results of Sec. 4.4, we run the pose refinement step. Our refinement objective function has four terms (Eq. (5.1)) and we conduct following ablation studies to validate the effectiveness of each term. Specifically,

1. 1.

   Pose Graph Optimization (PGO). By removing global 2D-3D reprojection term $\pi\left(\mathbf{X}_{k},\mathcal{F}_{k}\right)$ and 2D-2D Sampson term $\rho\left(\mathbf{X}_{k-1},\mathbf{X}_{k},\mathcal{Q}_{k-1,k}\right)$, the objective function becomes PGO.

2. 2.

   PGO_2D2D. By removing global 2D-3D reprojection term $\pi\left(\mathbf{X}_{k},\mathcal{F}_{k}\right)$, the objective function becomes PGO with 2D-2D sampson term.

3. 3.

   PGO_2D3D. By removing 2D-2D Sampson term $\rho\left(\mathbf{X}_{k-1},\mathbf{X}_{k},\mathcal{Q}_{k-1,k}\right)$, the objective function becomes PGO with 2D-3D reprojection term.

4. 4.

   PGBA. We keep all four terms and denote the objective function as PGBA (PGO with Bundle Adjustment).

The results of the above four methods are given in Table 5 and 6. For the oldtown dataset, both PGO_2D2D and PGO_2D3D outperforms PGO, showing the effectiveness of adding global 2D-3D reprojection and 2D-2D Sampson terms. The best performance is obtained by combining the two terms, resulting in PGBA. For the cityloop dataset, all methods have similar performance probably because this dataset is saturated.

We show estimated query positions before and after consensus set maximization and pose refinement steps in Fig. 6. It clearly shows that wrong estimated query positions are refined to correct positions, resulting in a smooth trajectory after refinement.

Our pose refinement step is implemented using C++, and the processing time is given in Table 7. For time-critical applications, methods PGO and PGO_2D3D are good candidates.

Based on estimated poses from Sec. 5.3, we test the pose polish module. We use refined poses $\mathcal{X}_{\mathsf{K}}$ to filter 2D-3D matches and recompute the pose for each session. A second round of consensus set maximization and pose refinement is performed on these poses. The results are given in Table 8. For both datasets, polishing helps to further improve pose accuracies, as the Recall@0.05m increases.