An Hybrid Registration Method for SLAM with the M8 Quanergy LiDAR

Abstract

Simultaneous localization and mapping (SLAM) is process highly relevant for autonomous systems. Accurate sensing provided by range sensors such as the M8 Quanergy LiDAR improves the speed and accuracy of SLAM, which can become an integral part of the control of innovative autonomous cars. In this paper we propose a hybrid point cloud registration method that profits from the high accuracy of classic iterated closest points (ICP) algorithm, and the robustness of the Normal Distributions Transform (NDT) registration method. We report positive results in an in-house experiment encouraging further research and experimentation.

1 Introduction

The simultaneous localization and mapping (SLAM) [1,2,3] aims to estimate a reconstruction of the environment along with the path traversed by the sensor has become an integral part of the robotic operating system (ROS) [4, 5]. One of the most widely used kinds of sensors used for SLAM are laser based depth measurement sensors, or light detection and ranging (LiDAR) sensors, which have been used for scanning and reconstruction of indoor and outdoor environments [6], even in underground mining vehicles [7]. Fusion of LiDAR with GPS allows for large scale navigation [8] of autonomous systems.

Our aim in this paper is to propose a hybridization of two well known point cloud registration methods. Hybridization is akin to the composition of subsystems in circuit like systems. It can be done in series, parallel or interleaving the systems in time. In this paper we propose a serial hybridization where one algorithm serves to provide a robust initial condition to the other. We have carried out SLAM experiments over different datasets using the Iterative Closest Point (ICP) [9], and the Normal Distribution Transform (NDT) [10]. We have found that the ICP method provides less noisy registrations under specific conditions, though it is much more fragile than the NDT. Figure 1 shows a reconstruction of the initial point clouds with ICP and NDT methods, respectively, for the inhouse data recording that is the demonstrator for the works in this paper. As we can see, ICP reconstruction generates a better surface with higher point density than NDT. Nevertheless, the ICP method is not able to properly register point sets at turning sections of the path traversed by the sensors, as shown in Figure 2, where the registration becomes unwieldy after this turning point. In other words, the ICP methods deals poorly with big rotations.

Fig. 1.

Results of registration with ICP method, (a) 2D plot of one the range detection at an specific position, (b) reconstructed ray sampling of the 3D surface. Results of registration with NDT method, (c) 2D plot of one the range detection at an specific position, (d) reconstructed ray sampling of the 3D surface.

Fig. 2.

Estimated trajectory (white points) and registered cloud of points using ICP.

This paper is structured as follow: A brief presentation of the environment where experiment was carried out and the LiDAR sensor used in it, Quanergy M8. Next, we provide a formal description of the 3D registration methods used in the paper: the Iterative Closest Point (ICP) and the Normal Distribution Transform (NDT). Then, the proposed hybrid NDT algorithm with ICP initialization is described. Finally, experimental results are presented for standard NDT registration and for the hybrid system described in this article, comparing root mean square error of the Euclidean distance, the path obtained and resulting reconstructed surfaces.

2 Materials

New affordable LiDAR sensors, such as the M8 from Quanergy that we are testing in this paper, allow for further popularization of LiDAR based SLAM applications. Due to its specific innovative characteristics, the M8 sensor still needs extensive testing by the community in order to assume its integration in the newly developed systems [11]. The work reported in this paper is intended partly to provide such empirical confirmation of the M8 sensor quality continuing experimentation over this sensor data reported elsewhere [12]. Both the time sequence of M8 captured point clouds and the Matlab code used to carry out the computational experiments has been published as open data and open source code¹ in the Zenodo repository for reproducibility.

Fig. 3.

Nominal path followed during the LiDAR recording.

Location and Experiment Setting. The experiment was carried out in the third floor of the Computer Science School of the UPV/EHU in San Sebastian. Figure 3 shows the nominal path followed by the M8 LiDAR on a manually driven mobile platform. The actual path shows small perturbations around the nominal path. We do not have a precise actual path measurement allowing to quantify the error in the trajectory.

LiDAR M8 Quanergy. The Quanergy M8 LiDAR sensor is a multi-laser system with 8 2D line scanners located on a spinning head. This system is based on Time-of-Flight (TOF) technology whose spin rate is 5 Hz and 20 Hz and its maximum range is 100 m. The Table 1 shows the M8 LiDAR main parameters. Besides, M8 LiDAR comes with 2 desktop applications to manage and visualize point clouds, a SDK to record and show data in real time, and a SDK in framework ROS.

Table 1. Quanergy M8 sensor specifications

3 Point Cloud Registration Methods

Point cloud registration methods are composed of two steps: (a) finding the correspondence between points in one cloud (the moving) to the points in the other cloud (the reference), and (b) the estimation of the motion parameters that achieve optimal match of the moving points to the reference points after correcting for the motion. If the motion is modeled by a rigid body or an affine transformation, then a matrix transformation common to all points is estimated. If the motion is some non linear deformation, then we have to estimate a flow field. In this paper we are restricted to rigid body transformations, which are compositions of a translation and a rotation. The transformation estimation process takes the form of a minimization problem where the energy function is related to the quality of the correspondence achieved. Next we recall the basics of the two point cloud registration methods. Figure 4 shows the structure of the algorithms as flow diagram of their basic iterations.

Fig. 4.

ICP (a) and NDT (b) processes.

3.1 Iterated Closest Point

The most popular and earliest point cloud registration method is the Iterative Closest Point (ICP) proposed by Besl in 1992 [9]. This technique has been exploited in many domains, giving rise to a host of variations whose relative merits are not so easy to assess [13]. Given a point cloud $P=\left\{ \mathbf {p}_{i}\right\} _{i=1}^{Np}$ and a shape described by another point cloud $X=\left\{ \mathbf {x}_{i}\right\} _{i=1}^{Nx}$ (The original paper includes the possibility to specify other primitives such as lines or triangles with well defined distances to a point, but we will not consider them in this paper.) the least squares registration of P is given by $\left( \mathbf {q},d\right) =\mathcal {Q}\left( P,Y\right)$, where $Y=\left\{ \mathbf {y}_{i}\right\} _{i=1}^{Np}$ is the set of nearest points from X to the points in P, i.e. $\mathbf {p}_{i}\in P;\mathbf {y}_{i}=\arg \underset{\mathbf {x}\in X}{\min }\left\| \mathbf {x}-\mathbf {p}_{i}\right\| ^{2}$, denoted $Y=\mathcal {C}\left( P,X\right)$, and operator $\mathcal {Q}$ is the least squares estimation of the rotation and translation mapping P to Y using quaternion notation, thus $\mathbf {q}=\left[ \mathbf {q}_{R}\mid \mathbf {q}_{T}\right] ^{t}$ is the optimal transformation specified by a rotation quaternion $\mathbf {q}_{R}$ and a translation $\mathbf {q}_{T}$, and d is the registration error. The energy function minimized to obtain the optimal registration is $f\left( \mathbf {q}\right) =\frac{1}{N_{p}}\sum _{i=1}^{Np}\left\| \mathbf {y}_{i}-\mathbf {R}\left( \mathbf {q}_{R}\right) \mathbf {p}_{i}-\mathbf {q}_{T}\right\| ^{2}$, where $\mathbf {R}\left( \mathbf {q}_{R}\right)$ is the rotation matrix constructed from quaternion $\mathbf {q}_{R}$. The iteration is initialized by setting $P_{0}=P$, $\mathbf {q}_{0}=\left[ 1,0,0,0,0,0,0\right] ^{t}$, and $k=0$. The algorithm iteration is as follows: (1) compute the closest points $Y_{k}=\mathcal {C}\left( P_{k},X\right)$, (2) compute the registration $\left( \mathbf {q}_{k},d_{k}\right) =\mathcal {Q}\left( P_{0},Y_{k}\right)$, (3) apply the registration $P_{k+1}=\mathbf {q}_{k}\left( P_{0}\right)$, and (4) terminate the iteration if the results are within a tolerance: $d_{k}-d_{k+1}<\tau$.

3.2 Normal Distribution Transform

The key difference of the NDT [10] method is the data representation. The space around the sensor is discretized into regular overlapped cells. The content of each cell having more than 3 points is modeled by a Gaussian probability distribution of mean $\mathbf {q=\frac{1}{n}}\sum _{i}\mathbf {x}_{i}$ and covariance matrix $\Sigma =\frac{1}{n-1}\sum _{i}\left( \mathbf {x}_{i}-\mathbf {q}\right) \left( \mathbf {x}_{i}-\mathbf {q}\right) ^{t}$, so that the probability of a LiDAR sample falling in the cell is of the form: $p\left( \mathbf {x}\right) \sim \exp \left( -\frac{1}{2}\right) \left( \mathbf {x}-\mathbf {q}\right) \Sigma ^{-1}\left( \mathbf {x}-\mathbf {q}\right)$. Given an initial rigid body transformation $T\left( \mathbf {x};\mathbf {p}_{0}\right)$, where $\mathbf {p}$ is the vector of translation and rotation parameters, a reference point cloud $\left\{ \mathbf {x}_{i}\right\}$ modeled by the mixture of the cells Gaussian distributions, and the moving point cloud $\left\{ \mathbf {y}_{i}\right\}$, the iterative registration process is as follows: the new laser sample points $\mathbf {y}_{i}$ are transformed into the reference frame of the first cloud $\mathbf {y}'_{i}=T\left( \mathbf {y}_{i};\mathbf {p}_{t-1}\right)$, where we find the cell where it falls and use its parameters $\left( \mathbf {q},\Sigma \right)$ to estimate its likelihood $p\left( \mathbf {y}'_{i}\right)$. The score of the transformation is given by $score\left( \mathbf {p}\right) =\sum _{i}p\left( \mathbf {y}'_{i}\right)$. The maximization of the score is carried out by gradient ascent using Newton’s method, i.e. $\mathbf {p}_{t}=\mathbf {p}_{t-1}+\mathbf {\triangle p}$. The parameter update is computed solving the equation $\mathbf {H}\mathbf {\triangle p}=\mathbf {-g}$, where $\mathbf {H}$ and $\mathbf {g}$ are the Hessian and the gradient of the $-score\left( \mathbf {p}_{t-1}\right)$ function, respectively. Closed forms of $\mathbf {H}$ and $\mathbf {g}$ are derived in [10] for the 2D case. An extension to 3D is described in [14].

4 Hybrid Point Cloud Registration Algorithm

In this section, hybrid registration algorithm which uses ICP and NDT methods is presented. As ICP reconstructed surface generates a better surface, i.e. with greater point density than NDT, but ICP method is not able to properly register point sets at turning sections. Our hybrid algorithm that initially uses ICP method changing to the NDT method when the registration error becomes higher than a threshold.

Algorithm 1 presents an algorithmic description of the proposed hybrid registration method. The input to the algorithm is the sequence of point clouds recorded by the LiDAR $N\left( t\right)$; $t=\left\{ 1,\ldots ,T\right\}$. The point sets are obtained while the LiDAR sensor is being displaced manually in the environment according to the approximate path in Figure 3. The final result of the process is a global point cloud $M\left( T\right)$ that contains all the recorded 3D points registered relative to the first acquired point cloud $N\left( 0\right)$, and the estimation of the LiDAR recording positions relative to the initial position given by the composition of the point cloud registration transformations estimated up to this time instant $\left\{ \mathcal {T}_{t}\right\} _{t=1}^{T}$. The process of each point cloud is as follows: For each point cloud $N\left( t\right)$ acquired at time t, firstly we remove the ground plane applying a segmentation denoted $N^{\left( 1\right) }\left( t\right)$. Secondly we remove the ego-vehicle points, denoted $N^{\left( 2\right) }\left( t\right)$. Thirdly, we down-sample the point cloud to decrease the computation time and improve accuracy registration, denoted $N^{\left( 3\right) }\left( t\right)$. For the initial point cloud $N^{\left( 3\right) }\left( 0\right)$ becomes the global merged reference cloud $M\left( 0\right)$. For subsequent time instants $t>0$, the fourth step is to estimate the transformation $\mathcal {T}_{t}$ of the acquired data $N^{\left( 3\right) }\left( t\right)$ optimally registered to the previous global point cloud $M\left( t-1\right)$. For this estimation, we may use ICP or NDT methods. We then apply this transformation to the acquired point cloud previous to downsampling $N^{\left( 4\right) }\left( t\right) =\mathcal {T}_{t}\left( N^{\left( 2\right) }\left( t\right) \right)$, which is used to obtain the new global registered point cloud by merging $M\left( t\right) \leftarrow merge\left( M\left( t-1\right) , N^{\left( 4\right) }\left( t\right) \right)$. Our hybrid strategy consists in using the ICP method in the initial steps of the algorithm, up a time instant when the registration error meets a given threshold, after this time point the system shifts to use the NDT method to continue registration of all remaining point clouds. The rationale is that the ICP acts as a good initial estimation for the ensuing NDT steps, as will be demonstrated in the results section below.

5 Results

Figure 5 shows the evolution of the registration error for ICP, NDT and the hybrid algorithm presented in the previous section, setting $\theta _{e}=0.25$. The plot scale is logarithmic in order to be able to represent the three error course in the same plot. The ICP algorithm gives the highest error. At the beginning the error is low, but it is increasing when we add more point clouds, until it explodes in iteration $t=1308$. Both NDT and the proposed hybrid method registration errors are bounded over time. The error of the hybrid method error is the same as the error of the ICP until the point where the algorithm shifts from ICP to NDT at the 138-th iteration. Afterwards the hybrid method has a slightly lower error that the NDT due to the quality of the initialization provided by the ICP. Table 2 gives numerical registration errors. However, the effect of using the ICP initialization is much more evident when assessing the overall results. Figure 6 shows the trajectory estimation (up) and the projection of the reconstructed surfaces on the floor plan (bottom) when using only NDT registration method. Though the error of the method is bounded, it can be appreciated that after the second turning point a noticeable drift appears, causing misalignment of the reconstructed surfaces with the actual floor plan walls. (Direction of advance is from right to left). On the other hand, the hybrid method trajectory and reconstruction results are provided in Figure 7 showing a much better fit to the floor plan walls. We conclude that the ICP provides a much more robust initialization for NDT.

Fig. 5.

Evolution of the registration error (log plot) for NDT (blue dots), ICP (red dots), and hybrid ICP-NDT method (green dots). (Color figure online)

Table 2. Registration error for ICP method, NDT method and hybrid ICP-NDT method

Fig. 6.

Estimated trajectory (white points) and registered cloud of points using NDT (Above). Projection of the NDT registered point cloud on the plan of stage 3 of the building.

Fig. 7.

Estimated trajectory (white points) and registered cloud of points using hybrid method proposed (Above). Projection of the hybrid registered point cloud on the plan of stage 3 of the building.

6 Conclusion

In this paper we report a hybridization between two registration methods for 3D point clouds, namely the Iterative Closest Point (ICP) and the Normal Distributions Transform (NDT). The experimental point clouds have been recorded with the M8 Quanergy LiDAR sensor traversing an indoor path the third floor of the Computer Science School of the UPV/EHU in San Sebastian. The general SLAM algorithm followed in this paper includes preprocessing (detect and remove ego-vehicle and floor, and down-sample), registration, transformation and merger of point cloud. We report the registration error, the estimation of the path traversed by the sensor, and the reconstructed point cloud. The hybrid method produces a slightly lower error than the NDT. On a qualitative assessment the hybrid method produces better surface reconstruction and path estimation. Future works will consider the analysis and exploitation of out-door recording methods.