Deep Robust Multi-Robot Re-localisation in Natural Environments

A range of algorithms has been proposed for LPR. Conventional approaches [12, 13, 14, 15, 16] encode point clouds into either a global descriptor representing the entire point cloud or several local descriptors by segmenting the point cloud into patches. However, these handcrafted methods are often rotation dependent and are not effective in generating discriminative descriptors for unstructured environments.

Deep LPR has demonstrated outstanding results in the past few years. These methods process point clouds through a deep neural network to extract local features. Features are either directly used for place recognition, such as works in [17, 18] or aggregated using either a first-order pooling technique, e.g., GeM [19], NetVLAD [20] or second order pooling employed in [21, 7], to generate a global descriptor of the point cloud [7, 6, 8, 5, 22]. Methods such as EgoNN [10] and LCDNet [23] estimate relative pose between two point clouds upon place recognition. EgoNN computes keypoint coordinates, local descriptors, and saliency in a local head. It later estimates 6DoF relative transformation between the query and top-candidate point clouds by matching keypoints and employing RANSAC to remove outliers. LCDNet trains local features end-to-end utilising the Optimal Transport (OT) theory for matching features and finally estimating the relative pose using Singular Value Decomposition (SVD), allowing the entire pipeline to be differentiable, therefore, learnable. However, at test-time, LCDNet employs RANSAC for relative pose estimation prone to divergence in natural environments. Focusing on re-ranking top-k retrieval candidates, SpectralGV [24] introduces a computationally efficient spectral re-ranking method to improve localisation.

There are PR-related works that aim to enhance place recognition by leveraging lidar scans and images captured in the same place. Works such as [25, 26, 27] integrates lidar and visual measurements at an early stage of multi-modal fusion to encode them into a global descriptor using a projection technique; however, at the cost of dimension loss. In contrast, works such as [28, 29, 30] encode lidar and visual data separately (late fusion) into image and point cloud embeddings and later aggregate them to create the bimodal global descriptor. To deal with lighting conditions (affecting the quality of image features), AdaFusion [31] employs an attention mechanism avoiding two modalities to be considered equally important when image quality is poor for recognition and vice versa.

In computer vision, works such as I2P [32] and 2D3D-MatchNet [33] have been proposed with a focus on image-to-lidar registration. I2P trains a network to estimate the pose between a pair of images and point clouds in two steps of classification and inverse camera projection. I2P uses an attention mechanism to classify lidar points in and out of the camera frustum. It optimises pose in the lidar frame using inverse camera projection and classification prediction. 2D3D-MatchNet learns 2D image and 3D point cloud descriptors in a triplet loss (anchor image, positive and negative point clouds) as similar image-lidar descriptors are pushed closer while negative pairs are pushed apart. Recently, SLidR [34] proposed to find similarities between point cloud and image pairs based on locally similar regions on 2D images and their corresponding 3D patches obtained knowledge distillation.

Our deep re-localisation module is based upon EgoNN [10]. Using a light 3D CNN network, EgoNN trains a global descriptor $d_{\mathcal{G}}\in\mathbb{R}^{256}$ and several local embeddings $d_{\mathcal{L}_{t}}\in\mathbb{R}^{128}$, where $t\in\{1,...,M\}$ is the number of keypoints detected by USIP [37], in each point cloud. Global descriptors are the aggregation of feature maps $\mathcal{F}_{\mathcal{G}}\in\mathbb{R}^{K\times 128}$ elements in the global head utilising GeM pooling [19]. $K$ is the number of local features in the global head. Keypoint descriptors are generated in the local head by processing the elements of a local feature map $\mathcal{F}_{\mathcal{L}}\in\mathbb{R}^{M\times 64}$. A two-layer Multi-Layer Perceptron (MLP) followed by a $\tanh{}$ functional module is used to compute the local embeddings’ coordinates in each point cloud. Global descriptors are used for PR, while local descriptors for localisation.

To accept or reject the re-localisation module output, we leverage cross-modal perception to compare the image $\mathcal{I}^{q}$ captured at the time of the query point cloud $\mathcal{S}^{q}$ and the top candidate point cloud $\mathcal{S}^{t1}$ estimated by re-localisation module. For this, the top candidate needs to be projected onto the query image using the relative pose $\mathbf{T}_{t1,q}$ estimated by local branch. If the pose estimate is correct, the projected points must overlay with their corresponding image pixels. To evaluate this, corresponding 2D and 3D features must be extracted and matched.

Handcrafted approaches such as [38] are not, however, suitable for feature extraction on lidar point clouds due to their sparsity and for the detection of similar features on images to establish accurate point-to-pixel matches. Point-wise deep feature descriptors, e.g., [33, 32], despite outperforming conventional techniques, can be affected in the presence of occlusion or motion blur, which is inevitable in robotics. Hence, we leverage a deep image-to-lidar self-supervised distillation approach called Superpixel-driven Lidar Representations (SLidR) [34], which relates a group of pixels with a group of points.

SLidR trains a 3D point representation using visual features for semantic segmentation and object detection. Cross-modal representation learning is motivated by the scarcity of annotated 3D point data and the abundance of image labels. SLidR transfers feature knowledge from super-pixels, i.e., regions of the image with visual similarity, to superpoints, i.e., groups of points segmented through superpixels back-projection. The image $\mathcal{I}_{q}$ is segmented into, at most, 250 superpixels using SLIC [39]. Importantly, SLidR requires no data labels for pre-training the 3D network. Given a synchronous lidar and camera data stream and the calibration parameters, SLidR extracts features of superpixels and their corresponding superpoints. The 2D features extracted from a pre-trained ResNet-50 backbone trained using  [40], serve as a supervisory signal for training a 3D sparse residual U-Net backbone [41] using a contrastive loss to align the pooled 3D points and 2D pixel features.

Employing SLidR, our approach compares the extracted features of superpixels ${sp}^{\mathcal{I}_{q}}_{i}$, where $i$ is the number of superpixels in image $\mathcal{I}_{q}$, with that of superpoints ${sp}^{\mathcal{S}_{t1}}_{j}$, where $j$ is the number of superpoints in point cloud $\mathcal{S}_{t1}$, using cosine similarity:

here $\mathbf{f}$ and $\mathbf{g}$ denote superpixel and superpoint features, respectively, after average pooling. Symbol $\langle.,.\rangle$ denotes inner product and $\|.\|$ L2 norm.

Now, we define two metrics, one in feature space and one in Euclidean space, to accept or reject re-localisation. First, we use the Mean Cosine Similarity (MCS) of corresponding superpixels and superpoints features, i.e., $\frac{1}{L}\sum_{i=j}{cs_{ij}}$ to decide whether the point clouds $\mathcal{S}^{q}$ and $\mathcal{S}^{t1}$ represent the same place. $L$ is the total number of superpixel-superpoint pairs on the main diagonal of the similarity matrix computed from Eq. (1). Low MCS values are an indicator of false-positive cases from our re-localisation module.

Second, to evaluate the accuracy of the relative pose estimated by EgoNN, we identify the top-5 candidate superpoints, denoted as $sp^{\mathcal{S}_{t1}}$@5, for each superpixel $sp^{\mathcal{I}_{q}}$. We project the centroid of each of these top-5 superpoints $sp^{\mathcal{S}_{t1}}$@5 onto the image $\mathcal{I}_{q}$. We find the superpoint $sp_{c}^{\mathcal{S}_{t1}}$ whose projected centroid is closest to the centroid of $sp^{\mathcal{I}_{q}}$, and we select it as the pair of $sp^{\mathcal{I}_{q}}$. We check whether the projected centroid of $sp_{c}^{\mathcal{S}_{t1}}$ falls within $sp^{\mathcal{I}_{q}}$, and we count it as a match if it does and a mismatch if it does not. We calculate the percentage of superpixel-superpoint mismatched pairs over the entire set of pairs to determine whether to reject or accept the re-localisation. We then define the alignment ratio as follows:

where $n$ is the number of superpixel-superpoint mismatched pairs computed from the abovementioned procedure. Defining two similarity and alignment metrics, we train a simple multi-class Support Vector Classifier (SVC), $y_{i}=\mathcal{K}(\text{MCS}_{i},\nu_{i})$, to predict if the pair $i$ belongs to matched, mismatched or unmatched category, where $y_{i}\in\{\text{matched, mismatched, unmatched}\}$.

Fig. 3 illustrates the top-K Recall curves between EgoNN and Scan Context. As seen, the performance of EgoNN is almost twice higher than Scan Context, indicating the limitation of Scan Context to produce distinctive and rotation-invariant descriptors in cluttered environments such as forests. To evaluate re-localisation accuracy, we compare the estimated relative transformation with ground truth and compute a success rate if the rotation and translation errors are within $5^{\circ}$ and $2$ m, respectively. This evaluation was not performed for Scan Context due to the inability of this approach to estimate 6DoF rotation and translation only based on global descriptors.

The success rate for EgoNN, when the relative transformation was only estimated using keypoints and via RANSAC, is about $40\%$. However, after refining the estimated transformation using ICP (we downsampled point clouds to 40 cm spatial resolution for online registration), the success rate increased to $78\%$. This indicates that although EgoNN achieved high performance in place recognition, the extracted keypoints are not well-repeated in unstructured environments for accurate re-localisation.

Fig. 4 illustrates an example of matched, unmatched and mismatched pairs in the top, middle and bottom rows, respectively. As seen, the similarity matrices (second column) and the projection vectors (third column) obtained from the procedure described in Sec. III-B are good measures to distinguish between matched, mismatched and unmatched pairs. Fig. 5 shows the boxplots over MCS and $\nu$ computed for about 250 matched pairs and 230 mismatched and unmatched pairs on the validation set (above 700 pairs in total). The substantial difference in MCS between unmatched and matched/mismatched pairs allows classifying unmatched ones with high confidence. Additionally, the large $\nu$ for the matched pairs helps classify them from the other pairs. We, however, observed if both MSC and $\nu$ are used together, it improves generalisation when training and testing environments are different. Hence, we trained a multi-class fifth-degree polynomial SVC model, $\mathcal{K}(\text{MCS},\nu)$, to predict if a pair belongs to matched, mismatched or unmatched categories.

To evaluate our pipeline, a tracked robot (as seen in Fig. 2), equipped with a lidar sensor and four cameras, was teleoperated in an unstructured area once as the initial session and once as the revisit session at QCAT. Time difference between the two sessions was reasonably chosen large, allowing us to evaluate the performance of our verification under various lighting conditions. For the cross-modal perception, we only used the camera frames of the front camera. Submaps were generated by Wildcat, and Robot Operating System (ROS) was used for communication between different components. Our re-localisation pipeline is triggered through a rosservice command. Upon requesting re-localisation, the query submap and the submaps existing in the prior map were fed into the already trained model of EgoNN. By performing a forward pass using the weights and benefiting from kd-tree, the top candidate was selected and the relative pose was estimated. Since PR is only performed based on root nodes, there were at most 20 submaps in the prior map from initial session. To thoroughly test the pipeline, the recorded data of the revisit session played back, and the re-localisation was requested for every root node spawned out, resulting in testing the entire pipeline 20 times (i.e., 20 “wake-up” locations). Following this process, the average Recall@1 of EgoNN is $100\%$. However, the success rate for re-localisation was $70\%$, evidencing the necessity of the hypothesis verification.

The pose estimate is not transferred to our lidar-inertial SLAM unless it passes through the hypothesis verification. For this, the top-candidate submap and the query image (which is already rectified) are fed into our pre-trained verification models. For the QCAT dataset, after 20 trials, the proposed hypothesis verification detected all the mismatched pairs for which EgoNN could not produce an accurate pose estimate. Fig. 6 shows a sample matched (top) and mismatched (bottom) scenario. The proposed verification pipeline, including the pre-trained feature matching and the SVC model $\mathcal{K}$, successfully separated these cases and detected the re-localisation failure.

Upon a verified re-localisation, the pose graph generated by the revisit session is safely merged into the existing map as shown in Fig. 7. Fig. 8 presents qualitative results after merging revisit session robot into the map generated from the initial session, evidencing the proposed pipeline feasibility for multi-agent re-localisation.

To demonstrate that our presented system can run online, we evaluated the computation time for each component. The timing results are collected by running the pre-trained models on a single NVIDIA Quadro T2000 GPU and the rest of the pipeline on a unit with an Intel Xeon W-10885M CPU.  Table I reports a breakdown of individual modules’ runtime in our pipeline. Together, the total runtime (for the QCAT experiment with the scale shown in Fig. 1) is less than a second, allowing the system to run for online operation.