Keeping an Eye on Things: Deep Learned Features for Long-Term Visual Localization

We start by detecting sparse 2D keypoints, $\mbf{q}=\begin{bmatrix}u_{\ell}&v_{\ell}\end{bmatrix}^{T}$, at sub-pixel locations in the left stereo image and computing descriptor vectors, $\mbf{d}\in\mathbb{R}^{496}$, and scores, $s\in[0,1]$, for all pixels in the image. The descriptor and score for a given keypoint is found using bilinear interpolation. The score determines how important a point is for pose estimation. We pass an image to an encoder-decoder neural network following the design from [28], illustrated in Figure 3. After the bottleneck, the network branches into two decoder branches, one to compute keypoint coordinates and one for the scores. We divide the image into size $16\times 16$ windows and detect a keypoint for each one by taking the weighted average of the pixel coordinates. We get the weights by computing the softmax over the network output for each window. Applying a sigmoid to the output of the second decoder branch gives us the scores. Finally, the descriptors are found by resizing and concatenating the feature maps of each encoder block, leaving us with a length 496 descriptor vector for each pixel.

We have a set of $N$ keypoints for the source image and we need to perform data association between these and points in the target image. Descriptors are compared using zero-normalized cross correlation (ZNCC), which for real matrices is the same as cosine distance and the resulting value will be in the range $[-1,1]$. For each keypoint in the source image, $\mbf{q}_{s}^{i},i\in[1,N]$, we compute a matched point, $\hat{\mbf{q}}_{t}^{i}$, in the target image. This point is the weighted sum of all image coordinates in the target image, where the weight is based on how well descriptors match:

$M$ is the total number of pixels in the target image, $f_{\rm zncc}(\cdot)$ computes the ZNCC between the descriptors, and $\sigma(\cdot)$ takes the temperature-weighted softmax with $\tau$ as the temperature, which is determined empirically by trying a range of values. The keypoint matching is differentiable. We found that using all target pixels worked better in practice than only including keypoints detected in the target image. Finally, we find the descriptor, $\hat{\mbf{d}}_{t}^{i}$, and score, $\hat{s}_{t}^{i}$, for each computed target point using bilinear interpolation.

In order to estimate the pose from matched 2D keypoints, we need to get their corresponding 3D coordinates, which is straightforward with a stereo camera. The camera model, $\mbf{g}(\cdot)$, for a pre-calibrated stereo rig maps a 3D point, $\mbf{p}=\begin{bmatrix}x&y&z\end{bmatrix}^{T}$, in the camera frame to a left stereo image coordinate, $\mbf{q}$, together with disparity, $d=u_{\ell}-u_{r}$, as follows:

where $f_{u}$ and $f_{v}$ are the horizontal and vertical focal lengths in pixels, $c_{u}$ and $c_{v}$ are the the camera’s horizontal and vertical optical centre coordinates in pixels, and $b$ is the baseline in metres. We use the inverse stereo camera model to get each keypoint’s 3D coordinates:

We perform stereo matching to obtain disparity, $d$, by using [30] as implemented in OpenCV.

Given the correspondences between the source keypoints, $\mbf{q}_{s}^{i}$, and matched target keypoints, $\hat{\mbf{q}}_{t}^{i}$, we can compute the relative pose from the source to the target, $\mbf{T}_{ts}=\left[\begin{matrix}\mbf{C}_{ts}&\mbf{r}^{st}_{t}\\ \mbf{0}&1\end{matrix}\right]$, where $\mbf{r}^{st}_{t}$ is the translation from the target frame to the source frame given in the target frame. As described in Section III-C, we use the inverse stereo camera model (3) to compute 3D coordinates, $\mbf{p}_{s}^{i}$ and $\hat{\mbf{p}}_{t}^{i}$, from the corresponding 2D keypoints. This allows us to minimize the following cost:

The minimization is implemented using Singular Value Decomposition (SVD) (more details can be found in [31]), which is differentiable. The weight, $w^{i}\in[0,1]$, for a matched point pair is a combination of the learned point scores, $s_{s}^{i}$ and $\hat{s}_{t}^{i}$, and how well the descriptors match:

We additionally remove large outliers at training time based on ground truth keypoint coordinate error and using Random Sample Consensus (RANSAC) at inference.

Barnes and Posner [28] supervise training using only a loss on the estimated pose. We found this was insufficient for our approach and also include a loss on the 3D coordinates of the matched keypoints, similar to [32]. We generate the keypoint ground truth from the poses and do not require additional keypoint correspondence data. These losses are sufficient and we do not need to add losses for descriptors or scores. For our training datasets, we only use a subset of the pose degrees of freedom (DOF) for supervision due to accuracy variability in the remaining DOF for some sequences. Specifically, we use the robot longitudinal direction, $\alpha$, lateral offset, $\beta$, and heading, $\gamma$. For this reason, we modify the keypoint loss to only use these DOFs.

Using (3), we can compute the 3D coordinates of the keypoints in the source and target camera frames. Given the ground truth pose, $\mbf{T}_{ts}$, we form a pose

that we use to transform the source keypoints. Because we transform the source points in the plane, we only compare the $x$ and $y$ point coordinates:

Forming $\mbf{T}_{ts}^{\prime}$ from the ground truth pose and $\hat{\mbf{T}}_{ts}^{\prime}$ from the estimated pose, we get the following pose loss:

where $\lambda$ is used to balance rotation and translation and $\mbf{1}$ is the identity matrix. The total loss is a weighted sum of $\mathcal{L}_{\rm keypoint}$ and $\mathcal{L}_{\rm pose}$, where the weight is determined empirically to balance the influence of the two loss terms.

The training data were previously collected using a Clearpath Grizzly robot with a Bumblebee XB3 camera, see Figure 1. By using multi-experience VT&R [1], the robot repeats paths accurately across large lighting and seasonal change with the help of intermediate bridging experiences and Speeded-Up Robust Features (SURF) [33]. We can use the resulting data as ground truth for supervised learning. In VT&R, stereo image keyframes are stored as vertices in a spatio-temporal pose graph. Edges contain the relative pose between temporally adjacent vertices and between a repeat vertex and the teach vertex to which it has been localized. We sample image pairs and poses from the pose graph.

We use data from two different paths for training¹¹1Data at http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset. The In-The-Dark dataset contains 39 runs of a path collected at our campus in summer 2016 along a road and on grass. The path was repeated once per hour for 30 hours systematically capturing lighting change. The Multiseason dataset contains 136 runs of a path in an area with more vegetation and undulating terrain. It was repeated from January until May 2017 capturing varying seasons and weather. Overhead path views can be seen in Figure 4. We generate two separate datasets from these two paths that each have 100,000 training samples and 20,000 validation samples.

We train the network by giving it pairs of images and the ground truth relative pose from the training dataset. Large outliers are removed based on keypoint error using the ground truth pose during training and with RANSAC during inference. The network is trained using the Adam optimizer [34] with a learning rate of $10^{-5}$ and other parameters set to default values. We determine the number of training epochs with early stopping based on the validation loss. The network is trained on an NVIDIA Tesla V100 DGXS GPU. Feature extraction on this server using PyTorch takes on average 14.8 ms, while it takes 7.3 ms using C++ in the VT&R system running on a ThinkPad P52 laptop with an Intel ® Core^™ i7-8850H CPU, 32 GB of RAM, and an NVIDIA Quadro P2000 4 GB GPU. The code is made available online ²²2Code at https://github.com/utiasASRL/deep_learned_visual_features.

In order to test the performance of the learned features, we add them to the VT&R system. While a user manually drives the robot to teach a new path, VT&R creates a spatio-temporal pose graph that serves as the map. The path is repeated by alternating between VO and localizing keyframes. VT&R does not compute global poses, but only finds the relative offset to a keyframe in the map. In multi-experience localization, poses and detected features for each repeat is stored in the pose graph. More detail on VT&R can be found in [35], and the VT&R code base is available online³³3VT&R code at http://utiasasrl.github.io/vtr3. We insert the learned feature detector and descriptors without making substantial changes to VT&R. Instead, we use the existing sparse descriptor matcher and do not add the keypoint scores used in training, but plan to include dense descriptor matching and keypoint scores in future work. Note that in the experiments, we match live images directly to the map without using intermediate experiences. Since VT&R still relies on SURF for VO, the learned features introduce additional computation and we had to reduce the maximum speed of the robot to 0.8 m/s in order to run the experiment.

We complete three experiments. We start by comparing to other state-of-the-art methods with code available online, specifically SuperPoint [19], D2-Net [20], and R2D2 [21]. This comparison is done offline in PyTorch (not in VT&R) for localization only on held-out repeats from the Multiseason dataset, see Figure 6. Our method uses a network trained on the Multiseason and In-The-Dark datasets, while we use weights provided with the other methods.

The last two experiments are run in closed loop on the robot. First we train a network on the In-The-Dark dataset and teach a new path by physically driving the robot, rather than using held-out data from the training dataset. The new path is similar to the one form the training data (see Figure 5), albeit five years later. We will refer to this as the ‘lighting-change experiment’. We taught the approximately 265 m path at 12:14 on August 2nd and repeated it on August 3rd and 9th covering lighting conditions from 3 a.m. til 10.30 p.m., see Figure 7. For this experiment, the network had a higher number of features for each layer (first layer of size 32 instead of 16). We later found this was unnecessary and all other experiments use the architecture from Figure 3.

In the final closed-loop experiments, which we refer to as the ‘generalization experiment’, we test the learned features’ ability to generalize to new areas. We train a network using both the Multiseason and In-The-Dark datasets and teach three new paths. The first is approximately 760 m and, as shown in Figure 5, includes two new areas that are not observed by training data. The larger of the two new sections is driven in an area with more vegetation, trees, and taller grass than seen in the most relevant Multiseason dataset. Moreover, the experiment is conducted in August, while the Multiseason dataset only contains data until May. This path was taught on August 14th and repeated on August 15th, 16th, and 20th covering lighting change from 4 a.m. til 9 p.m., see Figure 8. Additionally, we include a second path taught in a field at 09:52 on November 11th (275 m) and a third path taught in a parking lot at 11:34 on November 11th (220 m). Neither path has any overlap with the training data, see Figure 5. The paths are repeated from morning until dark on November 11th and 12th, see Figure 9.

We compared our method to state-of-the-art learned features on localization only in PyTorch. Keypoints, descriptors, and scores were extracted using the code provided by the authors, and the same sparse nearest-neighbour feature matching was used for all methods for fairness. For D2-Net and R2D2, we used multi-scale detection for best results. All conditions from Figure 6 were localized against each other. We did this test as a sanity check as our method trained on relevant data should outperform the others ⁴⁴4Since our data is different from more commonly used urban datasets, retraining the other methods with the same data would be more fair. This limits the usefulness of our comparison..

The median number of inliers as determined by RANSAC are listed in Table I. R2D2 gets the best result localizing the first three conditions to each other. Our method, however, is the only one with a high number of inliers across all experiments, while the others deteriorate as the seasonal change increases. Table I shows that the run from 14/02 with snow and high brightness is the most difficult. Since our target application runs on a laptop in real-time, memory consumption and speed also matter. R2D2 (1531 MB) and our method (2443 MB) used the lowest memory, while SuperPoint (5305MB) and D2-Net (9521MB) would be too large to fit on the 4 GB GPU. Our method was the fastest with 14.8 ms for feature extraction, and does not rely on NMS to pick a subset of keypoints as SuperPoint (37.3 ms) and R2D2 (38.7 ms). D2-Net was the slowest with 477.4 ms, though it would likely be faster without multi-scale detection.

We repeated the path described in Section IV-C 40 times with a 100% autonomy rate. The repeats span lighting change from 3 a.m. until 10.30 p.m., see Figure 7. All days had sunny weather with only occasional clouds. The robot repeated the path accurately for all conditions including driving in the dark with headlights. For this experiment only, we collected the path-tracking error measured with RTK-GPS. The root mean squared error (RMSE) across all repeats is 0.049 m with 0.070 m being the highest RMSE for an individual repeat. We provide box plots of the number of matched feature inliers in Figure 10. For every repeat, we get enough inliers to localize. Localization failure happens for frames with less than 6 inliers, which is the case for only three frames, see Table II. In this case, we rely on VO until localization recovers. As expected, we get the highest number of inliers in the middle of the day and the lowest number when it is dark. The dips in numbers during sunrise and sunset are caused by sun flares. Before sunrise there was fog, explaining the lower number of inliers compared to driving after dark at night.

In the previous experiment, we showed that we can learn features based on data collected in 2016, teach a new path five years later and repeat it across large lighting change. We want to make sure that the network is not just memorizing known locations from the training data. This experiment tests generalization to areas and seasonal appearance not included in the training datasets. We train a network with data from both the In-The-Dark and Multiseason datasets. Both the on-road and off-road sections of the first path have one new unseen area, as shown in Figure 5, while the second and third paths are set in entirely new locations. The first path was repeated 26 times with a 100% autonomy rate spanning lighting change from 4 a.m. until 9 p.m., see Figures 8. Table II shows localization failures for a small part of the path driven during night-time. We provide box plots of the number of inliers for all repeats in Figure 11 (a) and get the highest number of inliers during the day, some dips at sunrise and sunset due to sun flares, and the lowest inlier counts in the dark. In the same plot, we compare to localization using SURF with colour-constant images [1]. In eleven examples SURF fails to localize at the beginning of the path. For the remaining repeats, the number of inliers is much lower than for the learned features. Since a large part of the path falls in an area with tree cover and other vegetation, we were unable to collect RTK GPS ground truth. In the new off-road area the robot swayed slightly at a few spots during the dark or strong sun flares, but quickly recovered and never drove off the path. In the new on-road area, for one repeat at 10:20, the robot turned too sharply and was off the path by roughly 1.5 meters, but recovered when exiting the turn.

We also present two plots in Figure 11 (b) and (c) that compare the number of matched feature inliers when driving in the new areas versus the areas already seen in the training dataset. We divide the path into on-road and off-road sections since the on-road section generally gets more inliers. The new off-road area consistently gets fewer inliers than the area contained in the training data, but at the same time it gets enough inliers for driving. Moreover, the inlier numbers for the new area exhibit the same variation over time as those for the known area. In the new on-road area, the median number of inliers remains similar or higher compared to the known area. The reason is the new area is more similar to the rest of the path in the on-road case. Moreover, the new part of the path is less affected by sun flare at sunrise and sunset than the known on-road section, explaining why it has higher values at these times.

Finally, the second path collected in a field and the third path collected in a parking lot were both repeated 7 times with a 100% autonomy rate from morning until dark. Figure 11 (d) and (e) show a high number of inliers for both experiments, which means that the learned features generalized to these new places outside the training data. Localization failures only occur for the path in the field, see Table II.