GP-SLAM+: real-time 3D lidar SLAM based on improved regionalized Gaussian process map reconstruction

Initially, to establish different function relationships locally, we divide the whole domain into several evenly distributed cubic cells in the word coordinate system $\{W\}$. This decomposition also accelerates the reconstruction process [19]. The side length of each cell is $a$. The subset of the raw points $S_{t}^{W}$ located in the $k$th cell is denoted by $S_{t,k}^{W}=\left\{p_{k,i},i=1,\dots,n_{k}\right\}$.

Then we need to determine the function relationship between the coordinates as $x=f(y,z)$, or $y=f(x,z)$, or $z=f(x,y)$. Considering one function can only express a 2.5D surface, in case of complex 3D structure, generally we assume three functions exist in a cell. Each function provides corresponding constrains along its direction, which will be detailed in the Section IV. Accordingly, if the surface in a cell is perpendicular to one coordinate plane, as it only provides constrains along its normal, we can omit the corresponding function in this cell. This situation is judged based on the principled component analysis (PCA). Fig. 2 illustrates a example that when a set of raw data are drawn from a vertical wall, the function whose direction is $z$ will be neglected as the wall cannot provide vertical constrains.

After the regionalization, we conduct GP map reconstruction in each nonempty cell. Lidar measurements are noisy samples of the environment. The noise model of it can be derived from manufacture data as in [11]. Here, we simply assume that each lidar point follows an independent normal distribution with a isotropic variance $\sigma^{2}$. In this case, GP regression, which can produce the best linear unbiased prediction [19], is used.

The GP regression problem is detailed as follows based on [21]. Given $n_{k}$ training points as $D=\left\{\left(f_{i},{l}_{i}\right),i=1,\dots n_{k}\right\}$, the relationship between the observations $f_{i}\in\mathbb{R}$ in the training locations $l_{i}\in\mathbb{R}^{2}$ is expressed as $f_{i}=f\left({l}_{i}\right)+\varepsilon_{i},i=1,\dots,{n}_{k}$, where $\varepsilon_{i}$ is the noise term following the distribution of $\varepsilon_{i}\sim\mathcal{N}\left(0,\sigma^{2}\right)$. The goal is to achieve the distribution of $n_{{test}}$ predictions $\boldsymbol{f}_{*}$ in the preset test locations $\boldsymbol{l}_{*}=\left[l_{*1},l_{*2},\ldots,l_{*n_{{test}}}\right]^{T}$ denoted by $f_{*j}=f\left(l_{*j}\right)+\varepsilon_{*j},j=1,\ldots,{n}_{{test}}$. Defining $\boldsymbol{f}=\left[f_{1},f_{2},\ldots,f_{{n}_{k}}\right]^{T}$, $\boldsymbol{l}=\left[l_{1},l_{2},\ldots,l_{{n}_{k}}\right]^{T}$ the predictive distribution $\boldsymbol{f}_{*}$ of given $\boldsymbol{f}$ will be

$$\begin{array}[]{r}P\left(\boldsymbol{f}_{*}|\boldsymbol{f}\right)=\mathcal{N}\left(k_{\boldsymbol{l*}}^{T}\left(\sigma^{2}I+K_{\boldsymbol{ll}}\right)^{-1}\boldsymbol{f},k_{\boldsymbol{**}}-\right.\\ \left.k_{\boldsymbol{l*}}^{T}\left(\sigma^{2}I+K_{\boldsymbol{ll}}\right)^{-1}k_{\boldsymbol{l*}}\right)\end{array}$$

(1)

in which the mean value $k_{\boldsymbol{l*}}^{T}\left(\sigma^{2}I+{K}_{\boldsymbol{ll}}\right)^{-1}\boldsymbol{f}$ is taken as the point prediction of $\boldsymbol{f}_{*}$ at test locations $\boldsymbol{l}_{*}$. Its variance is estimated by $k_{\boldsymbol{**}}-k_{\boldsymbol{l*}}^{T}\left(\sigma^{2}I+{K}_{\boldsymbol{l}\boldsymbol{l}}\right)^{-1}k_{\boldsymbol{l*}}$. Here, $k_{\boldsymbol{**}}=k\left(\boldsymbol{l}_{*},\boldsymbol{l}_{*}\right)$, $k_{\boldsymbol{l*}}={k\left(\boldsymbol{l},\boldsymbol{l}_{*}\right)}^{T}$ and $\boldsymbol{K}_{\boldsymbol{l}\boldsymbol{l}}$ is an $n_{k}\times n_{k}$ matrix, $\boldsymbol{K}_{\boldsymbol{l}\boldsymbol{l}}(i,j)=k(i,j)$, $k(.,.)$ represent the kernel function. In this work, we choose the commonly used exponential covariance function, $k\left(l_{i},l_{j}\right)=\exp\left(-\kappa\left|l_{i}-l_{j}\right|\right)$, with a preset length-scale parameter $\kappa$.

In this context, the training points are the raw points $S_{t,k}^{W}$ in a cell. The coordinate used as observation is named direction, and the other two serve as a training location. The $n_{{test}}$ test locations are evenly set, and the interval between them is $r$. Recalling the side length of the cell is $a$, we set $a$ as integral times of $r$, which means $n_{{test}}=({a}/{r})^{2}$. Those predictions with variance are samples drawn from the implicit surfaces, and each set of samples are named as layer. As shown in Fig. 2, those predictions which are remote from raw data are more unreliable. We use these samples as the reconstruction result.

After the reconstruction, there are 0$\sim$3 layers in one cell. The cells is stored in a hashing table data structure. A sample is represented by $p_{i}=\left(f_{i},l_{i}\right)$, where $f_{i}\sim\mathcal{N}\left({u}_{i},\sigma_{i}^{2}\right)$ and the test location serve as index. By this way, a sample can be queried directly.

Besides the domain decomposition, we further accelerate the training process of GP through the concept of local regression. The central idea is that a prediction is mostly influenced by those observations whose training locations are closer to the test location of that prediction. Thus, the training process can be accelerated by principled down-sample of the training points without much precision loss [20]. Accordingly, we retain the raw data but only use all the closest points of each test location in GP map reconstruction each time. As a result, the amount of the filtered training points is reduced significantly.

One approach to complete this filtering process is utilizing the Kd-tree. However, the initializing cost of this data structure is $O({n{log}n})$ with $n$ inputs, and the average searching cost is $O({log}n)$ [18]. Although Kd-tree is faster than the brute-force searching, it is still time-consuming especially when the amount of points is large. As the searching targets in our application are evenly distributed, we use a modified 2D voxel filter to approximate this process. As shown in Fig. 3, in a cell, the training locations spread over a 2D domain, which is divided into smaller grids whose center are the test locations. The original voxel filter calculates the mean of all raw data in each smaller grid. The modification is that we keep a point if it is the closest one to the test location among all points in the same smaller grid. By this way. The filtering process can be finished with linear complexity cost.

The correspondences will be established between two samples coming from the current frame $P_{t}^{W}$ and the reference frame $Q_{t-1}^{W}$ respectively when they satisfy the following conditions: 1) two samples are located in the same or adjacent cell; 2) two samples share the same prediction direction and test location; 3) both variances of the samples are below threshold $\sigma_{thr}^{2}$. We illustrate this process in Fig. 4 in 2D space for simplicity. Pair-1 is a qualified correspondence while Pair-2 is invalid as the variance of one sample is too large. Pair-3 is established between two samples as they share another direction and are located in adjacent cells. In Pair-1, there are several samples satisfy aforementioned conditions. In this case the closer one is chosen. Paired samples are expressed by $\{p_{i},q_{i}\}$, where $p_{i}=\left(f_{pi},l_{pi}\right)$, $f_{pi}\sim\mathcal{N}\left({u}_{pi},\sigma_{pi}^{2}\right)$ and $q_{i}=\left(f_{qi},l_{qi}\right)$, $f_{qi}\sim\mathcal{N}\left({u}_{qi},\sigma_{qi}^{2}\right)$.

Based on the idea that layers only offer observability in their directions, we design the error metric as the distance between prediction of samples. Firstly, Given $e_{x}=[1,0,0]$, the coordinate $x_{i}$ of a 3D point $p_{i}={[x_{i},y_{i},z_{i}]}^{T}$ can be computed by $x_{i}=e_{x}\cdot p_{i}$, and the other two directions are the same. For the sake of brevity, we define an operator $\left(\cdot\right)_{\cdot}{\circ}$ to express such operation of obtaining the coordinate that corresponds to the direction $\circ$ in the following context. Similar to the MLE approach expressed in [8], with $n_{cur}$ paired samples $\{p_{i},q_{i}\},i=1,\dots,n_{cur}$ in all layers, for a transformation $\boldmath{T}$, we define the 1-dimensional distribution of an observation in certain test location as ${d}_{i}\sim\mathcal{N}\left({\left(T{p}_{i}\right)}_{\cdot}\circ-q_{i\cdot}\circ,\sigma_{pi}^{2}+\sigma_{qi}^{2}\right)$. Then the relative transformation is compute by

$${T}=\underset{T}{{argmax}}\prod_{i}\left({P}\left({d}_{i}\right)\right)\\ =\underset{T}{{argmax}}\sum_{i}\log\left({P}\left({d}_{i}\right)\right)$$

(2)

The above objective function can be simplified to

	$\displaystyle{T}$	$\displaystyle=\underset{T}{{argmin}}\sum_{i}\left({{d}_{i}}^{T}\left(\sigma_{pi}^{2}+\sigma_{qi}^{2}\right)^{-1}{d}_{i}\right)$		(3)
		$\displaystyle=\underset{T}{{argmin}}\sum_{i}\frac{\left\|\left(T{p}_{i}\right)_{\cdot}{\circ}-q_{i\cdot}\circ\right\|^{2}}{\sigma_{pi}^{2}+\sigma_{qi}^{2}}$

where the variance can be seen as weight. This optimization problem is solved by the non-linear solvers Ceres [22].

Compared with our previous work, the registration has been redesigned in several aspects. In the previous work, the correspondences are established using only one layer within each cell. It treats all the reconstructed samples as 3D points, and use the 3D Euclidean distance as the error metric. The problem is solved by singular value decomposition (SVD). In contrast, we use several layers to model complex structure and extended the correspondences searching area to avoid information loss in borders (In the previous work, the pair-3 in Fig. 4 is omitted). The error metric is also changed so that it will drags the surfaces, rater than points as in previous work, closer. This leads to faster convergence as it avoids inducing the test locations into the objective function.

We select two sets of typical range data to demonstrate the advantages of our registration upon ICP and previous method. In the first test, we use two frames of range data measured with a spinning 2D lidar from experiment A in Section VII. As shown in Fig. 5(a), the point cloud provided by this kind of sensor is rather sparse and uneven. Here, the Generalized ICP (GICP) is selected as the benchmark. The results in Fig. 5(b)&(c) shows that GICP falls into wrong local minima, while our method align structure well.

Secondly we check the impact of our modification on registration strategy. We choose a frame of range data collected by Velodyne VLP-16 from experiment C as the target frame and set an initial transformation error (1m in translation and 5 degree in rotation around the $z$-axis) on it to form the source frame. Then these two frames are aligned using the original and current registration methods. The root mean square error (RMSE) of the distances between the closest points from two frames are used as the convergence criteria. As shown in Fig. 6, the result indicates that the registration part of our method is significantly improved.

We use a data set collected by a spinning 2D lidar mounted on a micro aerial vehicle (MAV) [2] in a parking garage to demonstrate the robustness of our method when faced with sparse point cloud. The data set contains 200 frames of 3D data assembled from a 2D laser scan with the aid of visual odometry. The low-resolution point clouds are particularly sparse. Thus, the registration becomes challenging for ICP method as shown in Section IV-C. The overall trajectory length is 73 m.

The mapping result by our method is shown in Fig. 9. The map recovers the structure of the garage and the walls show few distortion. The dense and uniformly distributed point-cloud-like map depicts rich details inside the building. By contrary, as shown in the Fig. 18(c) in work [2], the GICP produces distort map even with graph optimization and registration with local dense map.

In the second part of experiments, we test the performance of the core workflow in our system without IMU. Here, we use one open-access method, A-LOAM²²2https://github.com/HKUST-Aerial-Robotics/A-LOAM, as the benchmark, which is an advanced implementation of LOAM [9].

Finally, we test the full system in a large-scale task. The VLP-16 lidar together with an Xsens MTi-610 IMU was mounted upon a passenger vehicle. The ground-truth was provided by an RTK-GPS. The vehicle traveled 2.1 km in a campus at an average speed of 2.7 m/s. Since A-LOAM do not utilize IMU information, here we choose LeGO-LOAM [3] as the baseline to conduct a fair comparison. It performs higher efficiency compared with the original LOAM and is optimized for ground application, which also means it is not suitable for the aerial experiment B directly.

This scenario includes urban and unstructured environment. We overlay the mapping result produced by our refinement thread with satellite image in Fig. 13. Our method produces coherent map. To visualize the drift, we align the front 30% part of the trajectories produced by two methods with the ground-truth respectively, and draw them in Fig. 14. Due to occlusion from buildings or trees, the RTK-GPS signal is unavailable in the southwest part. In those areas, the accuracy can be demonstrated by the map and satellite image in Fig. 13. For quantitatively evaluation, we align the entire trajectories with the ground truth respectively. Then, in Table III, we calculate the average translation error in x-y plane and the elevation difference when the vehicle returned to the start point. As it shows, the core workflow in our method accumulates less pose drift compared with LeGO-LOAM, and the refinement thread further enhances the performance especially in terms of the elevation error.