NDT-Map-Code: A 3D global descriptor for real-time loop closure detection in lidar SLAM

NDT is used to represent the point cloud due to its scalability in mapping large-scale scenes, as well as its advantages in noise and outlier removal and reducing the number of points to improve processing speed. As indicated in Table I, the storage space needed for a 2m resolution NDT map accounts for merely 0.36% of the storage space required by the raw point cloud. To obtain a point cloud in NDT representation, we divide the point cloud into uniformly distributed 3D grids and estimate a multivariate Gaussian for each cell. A one-pass algorithm is used to calculate the mean and covariance:

$$\small\begin{split}(\bar{x}_{i},\bar{y}_{i},\bar{z}_{i})=(\bar{x}_{i-1},\bar{y}_{i-1},\bar{z}_{i-1})+\\ \frac{1}{i}(x_{i}-\bar{x}_{i-1},y_{i}-\bar{y}_{i-1},z_{i}-\bar{z}_{i-1})\ \\ \end{split}$$

(1)

$$\small\begin{split}C_{i}=C_{i-1}+\frac{i-1}{i}(x_{i}-\bar{x}_{i-1},y_{i}-\bar{y}_{i-1},z_{i}-\bar{z}_{i-1})^{T}\\ \cdot(x_{i}-\bar{x}_{i-1},y_{i}-\bar{y}_{i-1},z_{i}-\bar{z}_{i-1})\ \\ \end{split}$$

(2)

$$\small\begin{split}{Cov}_{n}=\frac{C_{n}}{n}\end{split}$$

(3)

where $\left({\bar{x}}_{i},{\bar{y}}_{i},{\bar{z}}_{i}\right)$ refers to the mean value of the previous $i$ points in the same Cell, and $n$ is the total number of points, $C$ is a matrix of $3\times 3$, and ${Cov}_{n}$ represents the covariance matrix of $n$ points.

Similar to Scan-Context-like methods, polar-range coordinates ROI partition is used to divide 3D space into rings and sectors:

$$\small r_{k}=max\left\{r_{k}\in\mathbb{Z}\ |\ r_{k}\leq\frac{\sqrt{x_{k}^{2}+y_{k}^{2}}}{L_{r}},0\leq\sqrt{x_{k}^{2}+y_{k}^{2}}\leq R\right\}$$

(4)

$$\small\Theta\left(\frac{y_{k}}{x_{k}}\right)=\left\{\begin{aligned} arctan\frac{y_{k}}{x_{k}},{0<x}_{k},\ 0\leq y_{k}\\ arctan\frac{y_{k}}{x_{k}}+\pi,x_{k}<0\\ arctan\frac{y_{k}}{x_{k}}+2\pi,0<x_{k},y_{k}<0\end{aligned}\right.$$

(5)

$$\small\theta_{k}=max\left\{\theta_{k}\in\mathbb{Z}\ |\ \theta_{k}\leq\frac{\Theta\left(\frac{y_{k}}{x_{k}}\right)}{L_{\theta}},0\leq\sqrt{x_{k}^{2}+y_{k}^{2}}\leq R\right\}$$

(6)

where $L_{r}$ and $L_{\theta}$ represent the radial resolution and angular resolution of polar coordinates respectively and $R$ represents the maximum radial distance allowed. $\theta_{k}$ and $r_{k}$ represent the polar and range coordinates indices of $p_{k}$, whose maximum values are $N_{\theta}$ and $N_{r}$ respectively.

Existing methods focus on describing the buildings for outdoor localization. Specifically, each bin of [1] retains the maximal height value to represent the point cloud in the entire ROI bin; [3] and [4] include the most semantic labels and the maximum intensity value, respectively.

In underground parking lots, the presence of dynamic objects, such as parked cars, can significantly impact scan-context-like approaches. Moreover, due to the equivalent height of ceiling, maximal-height-based descriptors show limited effectiveness in discriminating between different locations.

To address these challenges, as illustrated in Fig. 1, we propose an alternative approach to dividing the scene $p_{k}(x_{k},y_{k},z_{k})$ into multiple layers based on height values with respect to the vehicle’s base link. Unlike the method proposed in [2], we do not use pitch angles for vertical partition. Instead, we segment the scene into layers according to the height values.

$$w_{k}=max\left\{{w_{k}}\in\mathbb{Z}|{w_{k}}\leq\frac{z_{k}}{L_{z}},0\leq{z_{k}}\leq Z\right\}$$

(7)

Here, $\mathbb{Z}$ means natural number, $L_{z}$ represents the height of each layer, $Z$ refers to the maximal $z$ value of truncated points $[0,Z]$, and $w_{k}$ represents the layer index to which $p_{k}$ belongs, whose maximum value is $N_{w}$.

Once the NDT point cloud is obtained, our approach analyzes each NDT cell by categorization of explicit geometric shape types, coupled with calculating the entropy of points within each NDT cell.

Our approach utilizes explicit categories to interpret the shape of each cell. For a given NDT cell, we calculate the sorted eigenvalues $e_{1}>e_{2}>e_{3}$ from the covariance matrix, together with their corresponding eigenvectors $v_{1},v_{2},v_{3}$. If $e_{1}\gg e_{2}\approx e_{3}$, the shape of the cell will be classified as a straight line. Conversely, if $e_{1}\approx e_{2}\gg e_{3}$, the cell will be considered a plane. However, this shape indicator requires two thresholds: 1) the ratio between $e_{1}$ and $e_{2}$, and 2) the ratio between $e_{2}$ and $e_{3}$. Instead of employing multiple thresholds, we propose an integrated one-dimensional NDT shape classification index:

$$g=\frac{e_{1}\cdot e_{3}}{\left(e_{2}\right)^{2}}$$

(8)

Therefore, by employing a straightforward thresholding strategy on $g$, we can effectively classify the geometric shapes. As illustrated in Fig. 3, $g$ enables the identification of four distinct shape categories: plane, ellipsoid, sphere, and line. To describe these explicit shape categories, we map the value of $g$ to different shapes using the following formula:

$$S=\left\{S\in\mathbb{Z}\ |\ S=\lceil\frac{g}{s}\rceil,0\leq g\leq g_{max}\right\}$$

(9)

Here, $g_{\text{max}}$, $s$, and $S$ denote the maximum $g$ value selected, the segmented value we choose, and the geometric value, respectively. Taking inspiration from NDD [15], we consider each NDT cell as a normal distribution characterized by a mean value $\mu$ and a covariance matrix $\Sigma$. Consequently, we utilize entropy, a statistical measure associated with the normal distribution, to describe the NDT cell:

$$E=\frac{N}{2}(log2\pi+1)+\frac{1}{2}log\left|\Sigma\right|$$

(10)

where $\left|\cdot\right|$ represents matrix determinant. $N$ represents the dimension of the vector space, and in this particular case, the value of N is 3 because the points involved are in three-dimensional space.

Then, the Polar-Range-Height ROI partition $r$, $\theta$, $w$, geometric value $S$, and entropy $E$ of each NDT cell can be obtained. Due to the variance of spatial partition between the voxel grid of NDT and ROI bins, one ROI bin likely contains multiple NDT cells. The majority value of geometric values is used for $S$, and sum-pooling is applied for the entropy $E$. The proposed descriptor NDT-MC integrates both geometric value and entropy features.

To adapt to both indoor and outdoor scenarios, NDT-MC is devised by combining geometric values $S_{r,\theta,w}$ and entropy $E_{r,\theta,w}$. Specifically, NDT-MC can be obtained by calculating $\mathcal{G}$ and $\mathcal{E}$ for each Region of Interest (ROI):

$$\mathcal{G}_{r,\theta}=\sum_{w=0}^{N_{z}-1}{\left(w+1\right)\cdot}S_{r,\theta,w}$$

(11)

$$\mathcal{E}_{r,\theta}=\sum_{w=0}^{N_{z}-1}{\left(w+1\right)\cdot}E_{r,\theta,w}$$

(12)

where $N_{z}$ represents the number of selected height layers.

Construction of geometric key (GK): Instead of using complete descriptors, we propose a histogram-based key called geometric key, to reduce the computation in similarity matching of descriptors. As shown in Fig. 4, we discrete the continuous $g$ values of NDT representations into columns with an increment of 1. This process results in a $1\times N_{s}$ vector, where $N_{s}$ represents the maximum value of the selected geometric value $S$.

Fast Retrieval based on kd-tree: By using the geometry key, the time complexity of kd-tree construction can be reduced from $O(nN_{r}N_{\theta}\log(n))$ to $O(nN_{s}\log(n))$, given the size of geometry $N_{s}$ is much smaller than both $N_{r}$ and $N_{\theta}$. Once the kd-tree is built, we undergo the k-nearest neighbor search for each query.

Construction of Sector Key: If a candidate keyframe satisfies the adaptive distance threshold, we compare its descriptor with that of the current frame using a similarity score. As mentioned earlier, rotation invariance is achieved by performing column shifts. However, iterating all columns will inevitably increase the computational complexity. Following $ScanContext$, we compute the column-wise average of the descriptor to obtain a $1\times N_{\theta}$ vector called the sector key. Sector keys simplify column shift approximation calculation, and descriptor matching is performed in the vicinity of the estimated column shift.

Our approach uses correlation coefficient as the similarity metric.

Given two descriptors $D_{q}=\left\{c_{q}^{0},\ c_{q}^{1},\ldots,c_{q}^{N_{\theta}}\right\}$, $D_{c}=\left\{c_{c}^{0},\ c_{c}^{1},\ldots,c_{c}^{N_{\theta}}\right\}$, $c_{q}^{i}$ and $c_{c}^{i}$ represent the $i^{th}$ column of $D_{q}$ and $D_{c}$ respectively.

Given a query descriptor $D_{q}$ to $\left\{c_{q}^{0},\ c_{q}^{1},\ldots,c_{q}^{N_{\theta}},c_{q}^{0},\ c_{q}^{1},\ldots,c_{q}^{N_{\theta}-1}\right\}$, The calculation method of correlation coefficient between the two descriptors is:

$$g_{k}\left(D_{q}^{k},D_{c}\right)=1-\frac{1}{N_{\theta}+1}\sum_{i=0}^{N_{\theta}}\left(\frac{(c_{q}^{i+k}-\overline{Q})\cdot(c_{c}^{i}-\overline{C})}{{\|c_{q}^{i+k}-\overline{Q}\|}\cdot{\|c_{c}^{i}-\overline{C}\|}}\right)$$

(13)

Where $\overline{C}$ and $\overline{Q}$ represent the mean of all elements of $D_{c}$ and $D_{q}^{k}$ respectively.

Then, column-shifting is employed to find the appropriate yaw angle to match:

	$$\displaystyle g_{s}\left(D_{q},D_{c}\right)=min\left(g_{k}\left(D_{q}^{k},D_{c}\right)\right)$$		(14)
	$$\displaystyle k\in\left\{0,1,\ldots,N_{\theta}-1\right\}$$		(15)

where $D_{q}^{k}$ means $D_{q}$ shifted by $k^{th}$. We determine the similarity between two frame descriptors by calculating the minimum similarity distance obtained through column displacement.

Our experiments have two distinct scenarios. We collect a dataset for underground parking lots localization (NIO underground parking-lot dataset), while the widely-used KITTI dataset[23] is also used for the evaluation of loop closure detection for on-road driving.

NIO underground parking-lot dataset. This dataset is collected in real-world underground parking lots using the ET7 model of NIO’s mass-produced cars, equipped with the Innovusion Falcon LiDAR(that of a FoV of $120^{\circ}\times 25^{\circ}$). As shown in Fig. 5, the dataset consists of four different parking lots, located in the basements of four commercial malls, i.e. Rongke, Lixiangguoji, Yinwang, and Yinzuo. A NovAtel IMU-ISA-100C system is used to generate ground truth trajectories for loop closures. A place is defined as a submap built by a trajectory segment of 4m. For each underground parking lot, the session with the largest map area (with the largest number of places), is selected as the database. Other sessions are used for testing. Because the pipeline is designed for crowd-sourced mapping, NDT representation is used instead of raw point cloud map. If the distance between the query pose and database pose is within the range of 4m on the x-y plane and 2m on the z-axis to a database pose, it will be considered as a true positive pair. The numbers of loop closures in test data 1 and test data 2 at Rongke are 439 and 415 respectively. Similarly, the numbers of loop closures at Lixiangguoji, Yinwang, and Yinzuo are 310 and 91, 94 and 142, 144 and 454. In Fig. 5, the database and testing trajectories of the four experiments are shown.

KITTI dataset. The sequence data in the KITTI dataset is used for this evaluation, where point cloud data is acquired using the Velodyne HDL-64E lidar sensor. Our experiment follows the same setting with NDD [15].

In the experiment, the database frames are extracted by cropping the NDT submap across the mapping trajectories. The testing observation is extracted from the local NDT map built on the fly by a Lidar mapper. There are three sessions of data were collected, one for the database and the other for testing. For each query observation, Eq. 13 and Eq. 14 are used to retrieve the most similar location from the database. Inter-session poses within 4 meters (with z differences not exceeding 2m) will be considered closed loops. In this experiment, the dense point cloud is not applicable. We use the mean values of NDT cells as the input of SOTA methods.

The proposed method is evaluated on eight testing datasets collected in four underground parking lots. For comparison, state-of-the-art place recognition methods are implemented, i.e., Scan Context[1], Lidar Iris[2], NDD[15], STD[24] and NDT-Histograms[25]. Since Scan Context-like methods are not designed for underground parking lots, we make the following adaptations: points below 2m w.r.t. lidar coordinate system are used for descriptor representation. For Scan Context and Lidar Iris, the mean values of NDT Submap as the input. For a fair comparison, the proposed method, Scan Context, and Lidar Iris are set with the same parameters, i.e., $N_{r}$=40, $N_{\theta}$=60, $Z$=3m, $L_{z}$=1m and $R$=80m. Since NDD and STD require the raw point cloud as input, both the observation and the database use the raw point cloud submap stitched by LIO. We use two widely-used evaluation metrics as NDD, namely F1 score, and Extended Precision (EP).

In the NIO parking lot dataset, performance evaluations of methods such as M2DP, OverlapTransformer, and Intensity Scan Context have not been conducted. M2DP exhibits a low Top-1 recall in scenes like Rongke1, Rongke2, and Liyangguoji1 due to its reliance on dense point clouds, while the sparse NDT data in our dataset adversely affects its performance. The OverlapTransformer method, relying on distance images and designed to run on CPU due to the sparsity of NDT point cloud data, is consequently expected to underperform on the NIO dataset. Lastly, the Intensity Scan Context method relies on specific intensity values, and variations in LiDAR sensor intensity values across different hardware platforms may impact its performance.

In this experiment, we define a true positive detection if the distance between the query and matched database frame node is less than 5 meters. Note, that consecutive frames within a certain range will not be considered as positive pair.

During the testing phase on the KITTI dataset, we evaluate the proposed NDT-MC as loop-closure detection component for a lidar SLAM system. As a comparison, other existing global descriptors are also evaluated, including Scan Context[1], Intensity Scan Context[4], Lidar Iris[2], M2DP[14], NDD[15] and STD[24]. Default parameters are used in SC¹¹1https://github.com/irapkaist/scancontext, ISC²²2https://github.com/wh200720041/iscloam, IRIS³³3https://github.com/JoestarK/lidar-Iris, M2DP⁴⁴4https://github.com/LiHeUA/M2DP, NDD⁵⁵5https://github.com/zhouruihao1001/NDD, and STD⁶⁶6https://github.com/hku-mars/STD. The same evaluation metrics, i.e. F1 score and Extended Precision (EP), are used in this experiment.

In this paper, the parameters of NDT-MC are set as follows: $N_{r}=20$, $N_{\theta}=60$, $N_{w}=6$, $g_{max}=2.4$, and the maximum point cloud range is 80m. The proposed descriptor is a $(2\times 20)\times 60$ matrix.

Different from most of the state-of-the-art descriptors, our approach can deploy in real-time using a normal CPU. We integrate NDT-MC with LIO-SAM[26] and set $K=10$.

Afterward, we compare our method with open-source algorithms SC-LIO-SAM in terms of real-time loop closure performance. we compare the real-time loop closure performance between both our integrated systems, i.e., NDT-MC with LIO-SAM, and SC-LIO-SAM. For a fair comparison, parameters, i.e. a maximum distance of 80 meters, a similarity distance threshold of 0.6, and a submap update frequency of 1Hz are used in both our approach and the baseline. A video demo of the integrated full SLAM system can be accessed through the hyperlink provided below https://youtu.be/xCtWRlEKCfk?si=J-_TAYcQmW1g_Juv.

We also conduct a comparison of runtime performance for descriptor extraction and retrieval on the KITTI. Our method is benchmarked with Scan Context, Lidar Iris, and STD. All comparisons were performed on a desktop equipped with an i9-10900K CPU @ 3.70GHz and 32GB of memory. As shown in TableIV, our approach shows a supreme performance in terms of both the descriptor extraction and query time.

TABLE II presents the F1 Score and EP of the proposed method and the compared methods and NDT point clouds of 2m resolution are investigated. The use of ”—” in the table signifies the inability to compute the Extended Precision at various thresholds. This is because these methods did not achieve 100 % precision at different thresholds, hence preventing the derivation of the $R_{P100}$ (recall at a precision of 100 % ) value. The Extended Precision (EP) is calculated as $0.5\times(R_{P100}+P_{R0})$, $P_{R0}$ is the precision at minimum recall. On the eight testing trajectories of four testing scenes, our approach achieves the highest F1 Score and EP. Specifically, our approach with an NDT point cloud resolution achieves the highest performance across all eight test datasets, demonstrating significant advancements compared to other baseline methods.

NDT-MC presents superior performance in underground parking scenarios due to its effective utilization of height information, its ability to capture complex geometric structures, and its capacity to handle dynamic environments. Encoding and weighting height information in a limited vertical range enhances loop closure accuracy. NDT-MC categorizes NDT cells to capture geometric intricacies, and it emphasizes permanent structure shapes over non-static elements to ensure robust performance in dynamic environments. This approach provides a unique and stable feature identification for prolonged and accurate localization, surpassing methods that rely on less informative high z-values, especially in indoor and dynamic scenes.

TABLE III presents the comparison results of F1 Score and EP on the KITTI dataset. In the six test scenarios, our method NDT-MC achieved the highest F1 Score in sequences 00, 02, 05, and 06, and the highest EP in sequences 05, 06, 07, and 08. Additionally, our method ranked second in EP in sequences 00 and 02, and second in F1 Score in sequences 07 and 08. Specifically, our method has shown significant advances in the experiment on the KITTI dataset. Our method achieves superior performance on sequences 00, 02, 05, and 06 over the six sequences. In this table, NDD without superscript represents the test results from the NDD paper[15], while NDD with superscript indicates the results obtained by the provided code.

We provide standalone comparisons of global descriptors and matching performance on both the NIO dataset and KITTI datasets, as detailed in Sections V-B and V-C. Note that in these comparisons, no prior poses were utilized. Our technique demonstrated superior results on the KITTI dataset, evident from the relatively high F1 Scores and Extended Precision. On the NIO dataset, our method’s performance was particularly noteworthy. Additionally, in Section V-D, we provided the assessment results of our proposed technique within an integrated SLAM framework. For this experiment, our proposed global descriptor and matching were combined with the selection of a prior pose for loop-closure detection. We named this system NDTMC-LIO-SAM. Its performance was benchmarked against a leading open-source Lidar SLAM system, SC-LIO-SAM, by comparing overall trajectory accuracy. This is a system-level comparison that takes into consideration integration and run-time factors. From Fig. 6, the proposed NDT-MC-LIO-SAM and the SC-LIO-SAM achieve similar performance on sequences 02 and 07. Especially on sequence 08, the trajectory of our method significantly outperforms SC-LIO-SAM.