Online Domain Adaptation for Occupancy Mapping

An occupancy model is typically represented as a parameterized function that models the occupancy probability of each location in the environment. The objective is to learn the model parameters $\theta$ given a set of observations from LIDAR beams. Once the parameters are estimated, it is possible to query $y_{*}=p(\mathrm{occupied}|\mathbf{x}_{*},\theta)\in[0,1]$ anywhere in the 2D space¹¹1We limit our discussion to 2D for simplicity. All theory are readily extensible to 3D. $\mathbf{x}_{*}\in\mathbb{R}^{2}:=(x_{1},x_{2})$. Labeling LIDAR hits as $y=1=\mathrm{occupied}$ and randomly sampled points between each LIDAR hit and the LIDAR sensor as $y=0=\mathrm{free}$, a dataset $\mathcal{D}=\{(\mathbf{x}_{n},y_{n})\}_{n=1}^{N}$ can be generated. Here, $\mathbf{x}_{n}\in\mathbb{R}^{2}$ are the corresponding spatial locations of $y_{n}\in\{0,1\}$.

Various models have been proposed for the occupancy function. Gaussian process occupancy maps (GPOMs) [20, 21] have been presented as an alternative to improve occupancy grid mapping (OGM) [22, 23] and Hilbert maps [24]. In addition to considering neighborhood information for accurate occupancy predictions, kernel methods used in GPOMs come with the flexibility of incorporating other aspects such as dynamics into occupancy mapping [25, 26]. On the other hand, GPOMs account for uncertainty as they are based on a Bayesian nonparametric model. Regardless of their attractive theoretical properties, GPOMs are impractical for real-world usage because of the $\mathcal{O}(N^{3})$ run-time and memory complexity. Recently proposed Bayesian Hilbert maps (BHMs) [27], on the other hand, encompass all positive traits of GPOMs but at a cost of $\mathcal{O}(M^{3})$ where $M\ll N$ is the number of features that correlates with the accuracy. Since ABHM considers the full Bayesian treatment over parameters of [27] to account for local spatial changes in the environment, it achieves a significantly higher accuracy.

BHM can be summarized as performing Bayesian logistic regression in a high-dimensional feature space $\mathbb{R}^{M}$ using kernels [28, 29]. BHM uses the same kernel for the entire map. ABHM is an extension to BHM to learn all location-dependent nonstationary kernel parameters (Appendix I-C). While BHM can be run in near real-time in an online fashion, ABHM is computationally expensive as it requires learning thousands of parameters offline. In ABHM, the occupancy probability of a point $\mathbf{x}_{*}$ is given by,

where $w,\mathbf{h},$ and $\gamma$ are parameters learned from data $\mathcal{D}$. The inner part of the equation is a $w$ weighted sum of $M$ kernels placed in 2D spatial locations $\mathbf{h}$. In areas where there are more LIDAR hits in the locality of a kernel, then its associated weight $w_{m}$ will be higher, and vice versa. This is because, as illustrated in Figure 3, here, $M$ squared-exponential (SE) kernels positioned at mean locations $(\mathbf{\bar{h}}_{1},\mathbf{\bar{h}}_{2},\dots,\mathbf{\bar{h}}_{M})$ are used to project 2D data into an $M$ dimensional vector such that each kernel has more effect from data in its locality. $\gamma$ are positive parameters that control the width of each kernel. Probability distributions $w_{m}\sim\mathcal{N}(\bar{w}_{m},\breve{w}_{m})$, $\mathbf{h}_{m}\sim\mathcal{N}(\mathbf{\bar{h}}_{m},\mathbf{\breve{h}}_{m})$, and $\gamma_{m}\sim\mathrm{Gamma}(\bar{\gamma}_{m},\breve{\gamma}_{m})$ are induced on the parameters to naturally encode uncertainty. Here, slightly abusing standard notations, $\bar{}$ and $\breve{}$ symbols are used to represent the mean and dispersion parameters, respectively (Table I).

The parameters of the model are learned using variational inference [13]. See Figure 3 for some of the estimated parameters. Since there are $8$ parameters ($\bar{w}_{m},\breve{w}_{m},\bar{\gamma}_{m},\breve{\gamma}_{m}\in\mathbb{R}$ and $\mathbf{\bar{h}}_{m},\mathbf{\breve{h}}_{m}\in\mathbb{R}^{2}$) associated with each kernel, it is required to learn $8M$ parameters. In order to achieve a practically satisfactory accuracy to cover a 100 m² area, it is necessary to have over 10000 kernels which would take around 10 minutes on a GPU. On the other hand, although ABHM provides high-quality maps, it is required to first collect the entire dataset as it does not support sequential training, making it practically unsuitable for mobile robotics applications.

The learned model parameters for a sample environment can be visualized in Figure 3.

Transferring knowledge obtained from one domain to the other has been widely discussed in the machine learning literature [1, 30]. The broader class of transferring from one type of domain to the other, e.g. images to text, is known as transfer learning. If the type of source and target domains are the same, as in occupancy mapping, the transfer process is called domain adaptation (DA). Applications in robotics include transferring control policies from simulation to real-world [2, 31], and making image processing tasks invariant to lighting and other changes [5, 32].

Variations of generative adversarial networks (GANs) such as DTN [33], CycleGAN [34], DiscoGAN [35], UNIT [36], DART [4] have been widely used for domain adaptation of RGB images. However, not only do these methods require a large amount of data but also it is not immediately clear how to use these techniques with sparse LIDAR data nor transferring probability distributions. In the next section, we consider an alternative domain adaptation method based on optimal transport (OT) [17] to transfer parameters of the Bayesian occupancy model using sparse LIDAR data.

In order to take advantage of domain adaptation we must have accurately pre-trained maps from which we can extract spatially relevant features. In the context of our problem, we must extract LIDAR scans (hits and free) with their corresponding model parameters including kernel weights, positions, and widths. To provide high-quality training data we extract learned model parameters from ABHM maps. Since ABHM can only be used on small areas due to the high computational cost, we learn separate ABHM maps for different areas and construct a dictionary of source atoms which we call a dictionary of atoms.

To construct the dictionary, as illustrated in Figure 4, we split each LIDAR scan into circular sectors with radii equal to the specified maximum LIDAR distance. Rather than using the entire LIDAR scan as the source dataset, this split not only results in a diverse set of geometric primitives but also provides simpler sources for the transfer procedure presented in the following section. The corresponding learned model parameters for each sector are considered as source parameters that we wish to transfer to the target domain. For each sector, we have $M^{(\mathcal{S})}$ parameters $\{\theta_{m}^{(\mathcal{S})}\}_{m=1}^{M^{(\mathcal{S})}}$ associated with ${N^{(\mathcal{S})}}$ LIDAR hits or free points $\{(\mathbf{x}_{n}^{(\mathcal{S})},y_{n}^{(\mathcal{S})})\}_{n=1}^{N^{(\mathcal{S})}}$. The collection of these different LIDAR sectors constitutes the dictionary of source atoms $\mathcal{X}^{(\mathcal{S})}$.

Until we present the general transfer procedure that we used in experiments in Section III-C, for the sake of simplicity of the following discussion, let us assume that the dictionary of atoms contains only one LIDAR sector and associated parameters.

Objective: Having determined source LIDAR data $\{(\mathbf{x}_{n}^{(\mathcal{S})},y_{n}^{(\mathcal{S})})\}_{n=1}^{N^{(\mathcal{S})}}$ and corresponding parameters $\{\theta_{m}^{(\mathcal{S})}\}_{m=1}^{M^{(\mathcal{S})}}$, our objective is to determine the new set of parameters $\{\theta^{(\mathcal{T})}\}_{m=1}^{M^{(\mathcal{T})}}$ for a new LIDAR dataset $\{(\mathbf{x}_{n}^{(\mathcal{T})},y_{n}^{(\mathcal{T})})\}_{n=1}^{N^{(\mathcal{T})}}$. This problem is illustrated in Figure 5 (a) and (b). In other words, we are looking for a nonlinear mapping technique to convert a source $(\mathcal{S})$ to a target $(\mathcal{T})$. We recognize this as an optimal transport (OT) problem given in Theorem 1.

Intuitively, as illustrated in Figures 5 (a) and 6, the OT problem attempts to determine the optimal way to move one probability distribution to another. If $\boldsymbol{\mu}^{(\mathcal{S})}$ and $\boldsymbol{\mu}^{(\mathcal{T})}$ constitute two datasets of size $N^{(\mathcal{S})}$ and $N^{(\mathcal{T})}$, respectively, there always exists an optimal probabilistic coupling $P_{*}\in\mathbb{R}^{N^{(\mathcal{S})}\times N^{(\mathcal{T})}}$ between the two datasets [37]. Here, as shown in Figures 6 where the source and target samples are assumed to separately follow bivariate distributions, $P_{*}$ is a doubly stochastic matrix—each row and column sums to one—that indicates the probability of a sample in the source match with all other points in the target. In occupancy mapping, $\boldsymbol{\mu}$ is computed as Dirac measures from LIDAR data (Appendix I-A).

With source data obtained in Section III-A, for a new target dataset, we attempt to obtain the optimal coupling,

for a given $D\in\mathbb{R}^{N^{(\mathcal{S})}\times N^{(\mathcal{T})}}$ distance matrix (e.g. squared Euclidean distance between source-target pairs) with the information entropy of $P$,

This entropic regularization, commonly known as the Sinkhorn distance [38, 39], enables solving the otherwise hard integer programming problem using an efficient iterative algorithm [40]. Here, $\lambda$ controls the amount of regularization²²2$\lambda$ can be set to a large number depending on the machine precision of the computer..

Having obtained the optimal coupling between source and target LIDAR, as illustrated in Figures 5 (b)-(c) and 7, now it is possible to transport source parameters $\theta^{(\mathcal{S})}$ to the target domain. This is done by associating the source parameter positions $\bar{\mathbf{h}}^{(\mathcal{S})}$ with source samples $\mathbf{x}^{(\mathcal{S})}$ as a linear map [41], and transporting them to the target domain $\bar{\mathbf{h}}^{(\mathcal{S})}\to\bar{\mathbf{h}}^{(\mathcal{T})}$ according to the coupling matrix $P_{*}$ learned from LIDAR matching. All other $\theta^{(\mathcal{S})}$ parameters associated with the kernels positioned at $\bar{\mathbf{h}}^{(\mathcal{S})}$ will also be transported to the target domain. This implicit transfer process is depicted in Figure 7.

Although we created a dictionary of atoms consisting of diverse geometric primitives in Section III-A, the transfer procedure introduced in Section III-B was limited to a single LIDAR sector. In order to effectively make use of the entire dictionary, it is required to find the optimal coupling matrix over all elements in the dictionary $\mathbf{x}^{(\mathcal{S})}\in\mathcal{X}^{(\mathcal{S})}$.

As another fact, although eq. 3 can be used to obtain a translation and scale invariant solution, it is not robust enough against large rotation variations. However, we can rotate data about the centroid of each atom using the rotation matrix,

for a discrete set of rotations $\alpha\in\mathcal{A}$.

Overall, we obtain a candidate optimal coupling set of size $|\mathcal{X}^{(\mathcal{S})}|\times|\mathcal{A}|$ by minimizing eq. 3 over all rotations and atoms,

Ultimately, we select the overall best coupling matrix from the candidate set $\mathcal{P}_{*}$ as the candidate that has the minimum 2-Wasserstein distance (refer Appendix I-B) to the target,

This $P_{*}$ can now be used to transfer parameters using the same method explained in Figure 7. As a result of the computation procedure introduced in this section, as depicted in Figure 8, atoms from various domains will be transferred to the target. Because atoms only consist of a few hundred LIDAR points, this transfer can be performed in real-time. Unlike in BHM or ABHM, we can now introduce thousands of kernels. The increasing number of pre-learned kernels as well as the nonstationarity help to improve the accuracy. The entire Parameter Optimal Transport (POT) algorithm is summarized in Algorithm 1.

Transporting parameters can be performed in two different ways. It is possible to transport parameters for each LIDAR scan separately, and immediately build the occupancy map. This results in an instantaneous map which is useful for understanding the occupancy of the surrounding at present. Such maps can be used for safe decision-making and control in the locality of the robot. On the other hand, it is also possible to build the overall map by sequentially aggregating the transported parameters as the robot moves. The overall map model completely discards training LIDAR data after transporting the parameters. This enables mapping large areas at a constant cost.

Once the parameters are transported with the intention of building an instantaneous or overall map, an occupancy map can be generated by plugging in the transported parameters to eq. (1) and querying occupancy probabilities. It will not only provide the mean occupancy map, but also the uncertainty as the variance estimate. Since only the parameters of the continuous mapping function eq.1 are stored, the occupancy map can later be queried at any time at any resolution.

Learning kernel parameters $\gamma$ and $\mathbf{h}$ in real-time is not feasible with ABHM. However, learning weights $w$, assuming other parameters are given, we have a fast approximation given by Bayesian Hilbert maps (BHMs) [27]. As an additional step to further improve the map quality, we propose to use transported parameters as prior distributions of the BHM and simply update the weights $w$ by using [27]. We call this improved map, the refined POT (RePOT) map.

In this experiment, we consider two paradigms: intra-domain and inter-domain transfer. In intra-domain transfer, the source atoms are generated from the first 10 frames of a particular dataset and parameters are transferred to the rest of the same dataset while they are transferred to a completely different domain in inter-domain transfer. Based on results reported in Table IV with 20% randomly sampled test LIDAR beams from each town, it is possible to accurately transfer parameters using POT. This enables mapping large scale towns in real-time. All parameters are aggregated over time to build occupancy maps of the entire environments as visualized in Figure 9 and Appendix II-C. Using the Town 1 3D dataset, we demonstrate the possibility of extending POT to 3D environments. In this case, source atoms described in Section III-A, were circular cylindrical sectors (i.e. pie slice shaped). The post-hoc refinement procedure, RePOT, introduced in Section III-D, further improved the map significantly. A visualization of RePOT is shown in Figure 10 and performance improvement, in direct comparison with results in Table IV, is reported in Table IV.

This experiment was designed to demonstrate how parameters can be instantaneously transported to build the instantaneous map of the surrounding. For this purpose, we used the two dynamic environments: Town 1 Dyna and KITTI Dyna. The source dictionary of atoms was prepared similar to the intra/inter-domain adaptation experiment. Such a map is shown in Figure 1. The performance of the model was evaluated on 20% of data that were not used for optimal transport. Table V shows the performance of transferring features extracted from each town to the dynamic datasets.

In this experiment, we compared various occupancy mapping algorithms in terms of accuracy and speed. Since these algorithms cannot be trained or queried in a similar fashion, we measured the per time unit performance. For instance, ABHM can only be trained in small environments although our datasets consist of large towns. Firstly, we measure the time for running POT per LIDAR scan. Then we decide the number of kernels to match the same runtime for BHM and ABHM. Results are reported in Table VI. Though OGM cannot be computed per time basis, we report the results for reference (See Appendix II-B). GPOM cannot be executed for datasets this large. As expected, ABHM outperforms BHM in all metrics because ABHM is a nonstationary model that takes into account local geometry. Theoretically, in the infinite memory and computation time limit, ABHM should outperform all methods. Nonetheless, practically, POT has a higher ACC and AUC compared to ABHM as POT can transfer kernels online to accommodate the complexity of the environment. However, the increase in NLL in POT compared to ABHM, indicates the inherent uncertainties of the transfer procedure. Once the weights were refined using RePOT, NLL has dropped as the weight distributions can be optimized to reduce the uncertainty giving better predictions.

Runtime: With a laptop with 4 cores and 8 GB RAM, on average, POT, programmed in Python, takes around 1 s update time. This is without parallelizing any part of the code. Note that eq. (6) is highly parallelizable making the algorithm $|\mathcal{X}^{(\mathcal{S})}|\times|\mathcal{A}|$ faster (approx. 25 times). This is a significant improvement to algorithms such as BHM and ABHM which would take several hours to build a large-scale map as they rely on complicated variational inference procedures. POT run-time increases with increasing $\lambda$ in the Sinkhorn algorithm we used in POT. As $\lambda\to\infty$ the convergence is guaranteed.