FusionLoc: Camera-2D LiDAR Fusion Using Multi-Head Self-Attention for End-to-End Serving Robot Relocalization

There have been a lot of studies on deep learning for localization using cameras. The visual localization studies include place recognition and end-to-end camera relocalization. Given a query image, place recognition performs localization by image retrieval, i.e., transforming the query image into a descriptor by a deep network and then searching the most similar descriptor with the input descriptor in a database consisting of descriptors and their location information. NetVLAD [15] is a representative study belonging to visual place recognition. On the other hand, end-to-end camera relocalization directly predicts the pose of the mobile robot from the input image. PoseNet [4] was a breaking ground work that tried to directly regress 6-DoF camera pose from a single image using a CNN. It was noted that it was robust to motion blur, darkness, and unknown camera intrinsics compared to conventional methods based on the scale-invariant feature transform (SIFT [16]). Probabilistic PoseNet [5], an extension of PoseNet, adopted a Bayesian CNN with Monte Carlo dropout sampling to handle uncertainty in pose estimation. In [6], LSTM was presented as a substitute for the fully-connected layer before the pose regression layer to prevent overfitting and to perform a structured dimensionality reduction. In [7], loss function was also a consideration to improve relocalization performance. To this end, two alternatives were presented and tested in the study. One was a learnable balancing parameter between position and orientation, and the other was the reprojection error between predicted and actual camera poses. In [8], VidLoc was proposed. It leveraged a bidirectional LSTM to utilize temporal information in successive images. The trial led to some reduction in the relocalization error. In [9], the authors employed an encoder-decoder CNN architecture, which was called hourglass, for fine-grained information restoration. In the encoder, ResNet-34 [17] was adopted instead of GoogLeNet [18] used in PoseNet. In [10], MapNet was proposed using an additional loss term related to the relative poses between image pairs together with the loss term of the absolute poses of images. It was meaningful concerning encoding geometric constraints between consecutive poses into the loss function. Moreover, in the study, the logarithm of the unit quaternion was also proposed as the representation of the camera orientation instead of the unit quaternion. The logarithm of the unit quaternion has been popular in most follow-up studies. In [19] and [20], a Siamese architecture was presented to reduce the relocalization error with the relative poses of the image pairs. ViPNet [21] utilized the squeeze-and-excitation blocks and a Bayesian CNN with ResNet-50 to deal with uncertainty in predicting a camera pose like Probabilistic PoseNet [5]. AtLoc [11] adopted a self-attention module to focus on geometrically rigid objects rather than dynamic ones. VMLoc [22] encoded the features of depth images (or projection of sparse LiDARs to RGB images) and RGB images in each CNN branch. Then, the two types of features were fused by Product-of-Experts.

There have been a few studies on deep learning for relocalization with other sensors. Compared to camera relocalization, those studies have recently been published. The 3D LiDAR point clouds have more geometric information because they can see with a wide angle of 360 degrees and vertical field of view. In [23], a point cloud odometry method was proposed, which took two consecutive 3D point clouds as input and predicted the transformation between them. Each point cloud was first encoded to a panoramic depth image, and then the two depth images were stacked. The DeepPCO network estimated 3D translation and 3D orientation from the stacked depth image using its two sub-networks. Different from [23], PointLoc [12] regressed a 6-DoF pose of a 3D LiDAR sensor directly from a point cloud. In PointLoc, the PointNet++ [24] architecture was employed to extract features from unordered and unstructured 3D point clouds, and a self-attention module was also employed to remove outliers. Unlike DeepPCO and PointLoc, StickyLocalization [25] relocalized input 3D point cloud within a pre-built map. In the pillar layer where PointPillar [13] method was utilized, local and global key points were extracted from a current point cloud and the global point cloud corresponding to a map, respectively. Then, self-attention and cross-attention modules in the graph neural network layer aggregated context information to improve robustness. The final optimal transport layer output the pose of the current point cloud by a matching process between the outputs of the graph neural network layer. Although its task was not to estimate the pose of a mobile robot, 2DLaserNet [26] processed 2D laser scan data with a neural network developed from PointNet++ [24] to classify the location of the mobile robot as one of room, corridor, and doorway.

In addition to camera and LiDAR, IMU has also been used for relocalization. RoNIN [27] utilized the backbone networks such as ResNet, LSTM, and temporal convolutional network to estimate human 3-DoF poses from a sequence of IMU sensor data. IDOL [28] regressed 5-DoF poses with a two-stage procedure consisting of an orientation module using extended Kalman filters (EKFs) and LSTMs and a position module using bidirectional LSTMs. In NILoc [29], a neural inertial navigation technique was presented to convert IMU sensor data to a sequence of velocity vectors. The methods leveraged a Transformer-based neural network architecture to reduce high uncertainty in IMU data. Moreover, in [30], a neural network framework was proposed to handle laser scan data and IMU sensor data together for mobile robot localization. For feature extraction in the method, a stack of two laser scans and a IMU data sequence between the two laser scans were passed through CNN and LSTM, respectively. Then, another LSTM regressed the 3-DoF robot pose from the fused feature.

Since Transformer [14] made a great success in the literature on natural language process, its self-attention, one of the essential elements in Transformer, has been utilized in many studies for computer vision applications. In particular, it was shown in [11] that a self-attention on CNN features was effective in camera pose regression. We use the same self-attention, but its input consists of image features computed from an input image and point cloud features computed from an input 2D point cloud. The input of the self-attention $\mathbf{f}_{i}$, which is a column vector, is first projected to generate query $\mathbf{W}_{q}\mathbf{f}_{i}$, key $\mathbf{W}_{k}\mathbf{f}_{i}$, and value $\mathbf{W}_{v}\mathbf{f}_{i}$ by three learnable projections. Then, the value is weighted based on the normalized correlations between the query and the key. The correlations are calculated using the softmax function $\phi$, and these procedures can be represented as

Here, $\mathbf{f}_{Att}$ is the output of scaled dot-product attention in [14]. The output of the self-attention $\mathbf{f}_{o}$ is computed based on a linear projection of $\mathbf{f}_{Att}$ and a residual connection as

According to [11], it was illustrated that this self-attention makes activation more intense in fixed objects like buildings and furniture in its input image than in dynamic ones like moving vehicles and pedestrians, which leads to robust relocalization. It was also shown that the self-attention is effective in capturing the correlations among the elements of $\mathbf{f}_{i}$ in camera relocalization as well as in other computer vision applications [31], [32].

The proposed network consists of two feature extraction modules, an MHSA module, and a regression module as shown in Fig. 1. Receiving two types of data obtained from different sensors, an image captured by a camera and a point cloud captured by a 2D LiDAR, it provides a robot pose, which consists of a position $\mathbf{p}$ and an orientation $\mathbf{q}$. We assume that they are synchronized in time.

At first, in the feature extraction modules, the image and point cloud features are computed from the image and the point cloud, respectively. The image feature $\mathbf{f}_{I}$ is computed by the feature extractor of AtLoc [11], which consists of a CNN based on ResNet-34 [17] followed by the self-attention module described above. On the other hand, the point cloud feature $\mathbf{f}_{P}$ is computed by the feature extractor of the PointLoc [12], which consists of the set abstraction layers, the self-attention layer, and the group all layer. Except for the self-attention layers, the other layers in the network were presented in PointNet++ [24], which was proposed for 3D point cloud classification and semantic segmentation. Since the input point cloud is two-dimensional, not three-dimensional, we modified the feature extraction module so that it receives a 2D point cloud as input. As in a CNN where a basic block consisting of convolution, nonlinear activation, and pooling operations is consecutively performed, several successive set abstraction layers extract features from a 2D LiDAR scan to give a feature matrix. The self-attention layer in PointLoc is slightly different from the one in AtLoc described above. In the layer, a weight matrix is computed by passing the feature matrix through a shared multi-layer perceptron (MLP) layer, a sigmoid function, and a broadcasting operation. Then, the output of the self-attention layer is obtained by multiplying the feature matrix and the weight matrix element-wise. Note that the input and output of the self-attention layer in PointLoc also have the same dimension as in AtLoc. The group all layer takes the output of the self-attention layer as its input and provides the point cloud feature $\mathbf{f}_{P}$ by conducting an MLP and max-pooling operations. More details are referred to as [12].

From the perspective of sensor fusion, it may be important to make data measured from different sensors interact with each other. For the relocalization using both camera and 2D LiDAR, the most straightforward way to use their data together in a network is to combine the two features by summation or concatenation as in [33], [34], [35] and perform a pose regression using the combined feature, which is called the fusion feature below. In our network, we chose concatenation to make the fusion feature from the image and point cloud features because summation is impossible but concatenation allows them to have different dimensions. However, we figured out from many experiments that those simple concatenation is not enough to effectively fuse different information obtained from the two sensors. To overcome this limitation, we propose to apply additional MHSA as shown in Fig. 2 to the fusion feature. Since the self-attention effectively captures the correlations between its input elements, it can allow different information contained in $\mathbf{f}_{I}$ and $\mathbf{f}_{P}$ to interact with each other. In other words, we utilize the MHSA module for information fusion.

In [14], MHSA was presented to capture different correlations among input elements by performing several scaled dot-product attentions in parallel. To this end, the outputs of all attentions are integrated and a linear projection is performed as

where $\mathbf{f}_{Att_{j}}$ is the output of the $j$-th scaled dot-product attention, $N_{h}$ is the number of the attention heads, and $\left[\mathbf{a}_{1}^{T},\ldots,\mathbf{a}_{n}^{T}\right]$ is the concatenation of $\{\mathbf{a}_{i}^{T}\}_{i=1}^{n}$. In this operation, each attention is scaled by a scaling factor $N_{h}$ so that its output has the same dimension as the input. Like the Transformer encoder in [14], a normalization layer is applied before MHSA, and a residual connection is attached after MHSA. We employ batch normalization (BN, [36]) instead of layer normalization (LN, [37]) different from [14]. It was demonstrated in [36] that LN is more effective than BN for recurrent networks. However, we find out from experiments that BN is more effective than LN in this work. Also, we do not use the positional encoding, another input of the Transformer encoder, because the order of elements in the fusion feature is not important in this task, unlike a sequence. This MHSA block with identical architecture repeats $N_{l}$ times as in [14]. It will be demonstrated in experiments that the repetition of MHSA improves the accuracy of the relocalization based on the camera-2D LiDAR fusion.

Finally, the regression module predicts the pose, the position $\mathbf{p}=\left[x,y\right]^{T}$ and the orientation $\mathbf{q}=\left[\cos\theta,\sin\theta\right]^{T}$ from the output of the MHSA module. It consists of a position branch and an orientation branch as in [11], [12]. Each branch is composed of consecutive MLPs. In [12], a leaky ReLU activation function was used after each MLP except for the last one in its regression head, but we replace it with the ReLU activation function in our network. Different from most of the previous studies for end-to-end relocalization, both the position and the orientation are two-dimensional under the assumption that typical serving robots move on planar space. To take into account the continuity of the rotation angle [38], we present the rotation $\mathbf{q}$ as $\left[\cos\theta,\sin\theta\right]^{T}$ rather than $\theta$. To our best knowledge, this work is the first study addressing the end-to-end relocalization for a serving robot based on the camera-2D LiDAR fusion in two-dimensional planar space.

In order to train and evaluate the neural networks for serving robot relocalization, we constructed a dataset using a commercial serving robot Polaris3D ereon as shown in Fig. 3. The dataset contains two sets, Set-01 and Set-02. We gathered the data samples in Set-01 by moving the robot around an area with tables and chairs in our testbed as shown in the left of Fig. 4a. Set-01 originally consisted of a single sequence of 3,964 lengths. We split it into ten shorter ones as in the right of Fig. 4a. Seven sequences and the others among them were used for training and evaluation, respectively. In addition, we collected the data samples for Set-02 by operating the robot in a relatively wider area with long corridors as shown in the left of Fig. 4b. Set-02 consists of three sequences with lengths of 4,820, 7,805, and 6,377 as in the right of Fig. 4b. Two sequences and the other one among them were used for training and evaluation, respectively. Table I summarizes the ereon dataset described above.

As shown in Fig. 3, ereon has two cameras, Intel RealSense D435 and one 2D LiDAR, SLAMTEC RPLiDAR A1M8. The cameras are installed at the side of the upper and lower serving tray, and the LiDAR is mounted in the center of the drive unit located under the lower serving tray. In order to capture the whole body of people around the robot, the lower and upper cameras face upwards and downwards, respectively, rather than facing straight ahead. We gathered images and point clouds obtained from the lower camera and the LiDAR because the upper camera was affected by vibration during robot movement. The obtained RGB images have the size of 420$\times$240 pixels with a frequency of 1.5 Hz. It may be seen that the image resolution and the saving frequency are low. The serving robot is equipped with a single-board computer (SBC) instead of a PC or a laptop due to the unit cost of production. It is not easy in practice to capture and store high-resolution images and 2D LiDAR point clouds at a high frequency on the SBC performing a localization to obtain ground truth pose information mentioned below. The LiDAR sensor captures 2D point clouds with a range radius of up to 12 meters (m) and a field of view of 360 degrees (^∘). Although it can measure point clouds at a frequency of 8 Hz, we acquired only the point clouds synchronized with images. Since the angular resolution of the LiDAR sensor is equal to or greater than 0.313^∘, the number of 2D points in a point cloud is up to about 1,150. Fig. 5 shows the example RGB images and 2D point clouds captured by our sensors. Together with the sensor data, the robot poses corresponding to images and point clouds are necessary to train and evaluate relocalization algorithms. To do this, we first constructed a map of our testbed using a 2D LiDAR-based SLAM technique and then measured the poses on the constructed map by its localization mode using only the 2D LiDAR. Since the SBC in ereon has not powerful as mentioned above, 2D LiDAR-based mapping and localization is a better solution than visual SLAM and camera-based localization for real-time navigation. The localization mode provides a 6-DoF pose with the quaternion representation for orientation. However, we used the x- and y-axis values for position and the yaw angle value for orientation under the consideration that typical serving robots operate in a flat environment. Given a quaternion $\left[q_{x},q_{y},q_{z},q_{w}\right]$, the yaw angle $\theta$ is calculated as

We implemented and trained our proposed network and other networks presented for the same task under the setting below. The Adam method was employed as the solver or optimizer. The learning rate was set to 0.0001, and the weight decay was determined to be the same value. The networks were trained up to 1000 epochs using the ereon dataset with a batch size of 256 on a single GPU of NVIDIA GeForce RTX 3090. As in AtLoc [11] and PointLoc [12], we adopted $\mathcal{L}_{1}$ distances to measure the dissimilarity between the ground truth pose and the estimated pose in our loss function as

where $\beta$ and $\gamma$ are learnable parameters to balance the position and orientation loss terms. Their initial values $\beta_{0}$ and $\gamma_{0}$ were set to $0.0$ and $-3.0$, respectively, as in [11] and [12]. In the above loss function, we employed the $\mathcal{L}_{1}$ distance between the predicted angle and the ground truth angle instead of the $\mathcal{L}_{1}$ distance between two logarithms of their unit quaternions. This replacement came from the dimension of the space in which our robot operates.

For the purpose of comparison, AtLoc and PointLoc were selected as the image- and the point cloud-based baseline methods for end-to-end serving robot relocalization, respectively. We also utilized their networks as backbones for image and point cloud feature extraction. For image feature extraction, as in AtLoc, we scaled the short side of the image to have 256 pixels, then randomly cropped it to 256$\times$256 pixels and normalized the cropped image when training. The random cropping was replaced by the center cropping when testing. For data augmentation, the color jittering method was also conducted by setting the brightness, contrast, and saturation values to 0.7 and the hue value to 0.5. The pre-trained ResNet34 with the ImageNet dataset was chosen as the backbone for image feature extraction. We also applied a dropout operation with a probability of 0.5 whereas no dropout was applied when using BN. Table II shows the parameter values of the set abstraction layers in the point cloud feature extractor used in our experiments. We reduced the numbers of layers and neurons in MLP compared to the original PointLoc setting because the 2D point cloud has fewer input points than the 3D point cloud. We also employed ReLU instead of LeakyReLU as the activation function in the fully connected layers. After the feature extractions mentioned above, the image and the point cloud features were concatenated into the fusion feature. Then, the fusion feature was fed into the several MHSA blocks described in the previous section and the final regression head to provide the pose estimate. In these procedures, both the image and the point cloud features could be set to 256-, 512-, 1024-, and 2048-dimensional giving 16 combinations of fusion. Also, we conducted experiments applying MHSA with 1, 2, 4, and 8 heads and 1, 2, 4, and 6 layers.

Baselines. In the original AtLoc and PointLoc, the image and point cloud features were 2048- and 1024-dimensional, respectively. We first tried to determine the dimensions of the image and point cloud features represented as $d_{I}$ and $d_{P}$, respectively. To do this, we trained the two methods using the data in the seven training sequences in Set-01 in Table I. Then, we measured the median and mean of the position (m) and orientation errors (^∘) using the data in each evaluation sequence in the same set for each value of the $d_{I}$ and $d_{P}$. Most previous end-to-end relocalization studies reported only the median errors. However, we report the mean errors with the median errors since the latter can not reflect a few large errors. We computed the averages of the median and mean errors, and Table III shows the average median and mean errors of position and orientation. We can see from the table that AtLoc gives lower position errors than PointLoc whereas PointLoc yields lower orientation errors than AtLoc. Another interesting point is that the tendency between the error values and their feature dimension is opposite, i.e., the error values of AtLoc and PointLoc decrease and increase as their dimension increases, respectively. Also, in the case of position error, AtLoc provided similar error values varying the dimension of the image features, and the difference was up to 0.04 m and 3.58^∘ for position and orientation, respectively. However, the error values of PointLoc had a large difference, the maximum of which was 0.51 m and 3.24^∘.

In order to validate the proposed method in a larger place, we compared the position and orientation errors using Set-02 data. For efficiency, we set to $d_{I}$ = $d_{P}$ = 256. As in the above experiments using the data in Set-01, we trained every neural network aforementioned using the training sequences in Set-02, and we measured the median and mean errors of position and orientation using the evaluation sequence. Fig. 8 shows the median and mean errors of AtLoc, PointLoc, the simple concatenation method, FusionLoc using LN, and FusionLoc using BN, which were computed at every 100 up to 1000 epochs. Different from the results of Set-01 in Table III that AtLoc was better than PointLoc in terms of the position error whereas PointLoc was better than AtLoc in terms of the orientation error, we can see in Fig. 8 that AtLoc provided lower position and orientation errors than PointLoc. This inconsistency may come from the fact that i) Set-02 was collected by making the robot move similar paths multiple times unlike Set-01 as shown in Fig. 4, and ii) the maximum range of the used LiDAR sensor is relatively lower than the area of the place where Set-02 was gathered. Note that using only the fusion feature could decrease the position and the orientation errors if the learning progresses enough. Actually, it yielded a more accurate result than AtLoc by 0.24 m and 1.09^∘ in terms of the mean errors at 1000 epochs. We can also see that FusionLoc using LN provided higher errors than the simple concatenation method. However, FusionLoc using BN gave the minimum errors in position and orientation, which were lower than the concatenation method by 0.1 m and 0.5^∘ in terms of the mean error at 1000 epochs. It corresponds to 15% and 18% reductions in the mean position and orientation errors. Fig. 9 shows a visualization of the position and orientation errors represented as color using Set-02. We also figure out from the figure that FusionLoc is more effective than the other methods in the relocalization task. In summary, our experimental results demonstrate that MHSA can be an effective solution to fuse the features captured by different sensors, and BN is more appropriate than LN in MHSA for robot relocalization based on the camera-2D LiDAR fusion.