Reading classic literature--SLAMesh (grid laser SLAM)

0. Introduction

There are really more and more new works on laser SLAM recently, and most current laser SLAM systems build maps in point clouds. Even though they appear dense to the human eye, the point clouds are sparse when zoomed in. of. Dense maps are crucial for robotics applications such as map-based navigation. Due to low memory costs, grids have become an attractive model for dense mapping in recent years. " Real-time LiDAR Simultaneous Localization and Meshing " proposes the first CPU-only real-time LiDAR SLAM system that can simultaneously build a grid map and perform localization against the grid map. A direct gridding strategy using Gaussian process reconstruction enables rapid construction, registration, and updating of grid maps. The specific open source code is also on Github .

1. Main contributions

The contributions of this article are as follows:

1. We propose a meshing strategy based on GP reconstruction and vertex connections that allows rapid construction, querying, and updating of mesh maps

2. We design a point-to-grid registration method. Combining constraint composition and multi-threaded implementation, we ensure the efficiency and accuracy of grid map positioning and mapping.

3. We developed a dense, real-time, scalable open source LiDAR SLAM system on grid maps and demonstrated its advantages through experiments.

2. Overview of methods

The motivation of this paper is to build, utilize and maintain a grid map in a LiDAR SLAM system. Figure 2 shows a general overview. The system mainly consists of three components: gridding, registration and grid management. First, each new LiDAR scan is transformed into a world coordinate system using an initial guess from the constant velocity model { WW $W$}下的$S^{raw}$ . The following operations are also { WW Executed in$W\; \}.$Then, the points are assigned into voxel units. GP reconstructs the local surface within each cell and obtains the vertices$v_i$, these vertices are connected to each other to form a mesh. In the registration component, a point-to-grid registration method is designed to combine the reconstructed current scan with the constructed grid map $M$ align. Finally, the grid map is iteratively updated.

Figure 2. A general diagram of our SLAMesh. The system is divided into three parts: gridding, registration and grid management. (a) The original point cloud is transformed into the world coordinate system { WW $W$ }, is downsampled and distributed into voxel cells; (b) Gaussian process model (light purple) models the local surface of each cell to obtain uniformly distributed vertices. The color of the vertices represents their uncertainty (warmer means higher), which increases with its distance from the original point; © We build the mesh by directly connecting adjacent vertices (light blue); (d ) Point-to-grid registration aligns the reconstructed current scan (red dots) to the grid map. Fast matching based on vertex positions is possible. Normals are smoothed from the surrounding mesh. A GPed scan represents a scan reconstructed by a Gaussian process. Finally, we only need to adjust the 1-D predictions of the vertices and their uncertainties to maintain the grid map.

3. Grid strategy

As mentioned before, building and updating grids is time-consuming. To solve this problem, we adopt a reconstruction and connection strategy so that the subsequent process can run in real time. As shown in Figure 2, GP recovers local surfaces from noisy and sparse point clouds inside voxels. The vertices are then the interpolated results of the surface. Two coordinates of a three-dimensional vertex are uniformly distributed (called positions), while the other coordinate (called predictions) has a continuous range of values. The position is used as an index to achieve fast query, and the predicted value range is continuous to avoid accuracy loss caused by discretization.

The GP process is described here (a more detailed GP description can be found in $found\; in\; [16\left]\right[17]$ ). Bold lowercase letters represent vectors, uppercase letters represent matrices. The input of GP is the original point cloudS kraw = { pi = ( xi , yi , zi , σ in 2 ) , i = 1 , . . . . , ni } S^{raw}_k= \{ p_i = (x_i , y_i , z_i , σ^2_{in}), i = 1, ...., n_i\}Skraw​={ pi​=(xi​,yi​,zi​,pin2​),i=1,....,ni​} , including$n_i$points, where $k$ represents the kthin the current scan$k$ cells$C_k$, $p_{in}^2$is the isotropic variance of the input noise. The output is an array containing $n_j$with uncertainty $p_j^2$The vertex $L_k^z$，其中 { v j = （ x j ， y j ， z j ， σ j 2 ）， j = 1 ， … ， n j } \{v_j =（x_j，y_j，z_j，σ^2_j），j = 1，…，n_j\} { vj​=（xj​，yj​，zj​, pj2​），j=1，…，nj​} . Top page$z$ represents the GP predicted coordinate$z$为 f ~ = { z j ， j = 1 ， … ， n j } \tilde{f} = \{z_j，j = 1，…，n_j\} f~​={ zj​，j=1，…，nj​} , when the coordinate can be$x$，$yorzz$ _$When\; any\; of\; z$ is used, the superscript is omitted. We denote the positions of the input and output point sets as$i$ and$j$ . Given input observation$f$ , the prediction $\tilde{f}$Also\; follows\; Gaussian\; distribution.$The expected value of \tilde{f} $ is:

The uncertainty of a forecast refers to its variance.

$k_{jj}$、$k_{ij}$Japanese $k_{ii}$Represents different combinations of position kernel functions

Among them $k(i,j)$ is a proportional value,$\kappa$ is a constant ( κ = 1 κ=1in our algorithm$K=1$ ). This exponential kernel function can represent locally smooth surfaces in 2D manifolds. Therefore, when modeling a complex structure, a cell can contain more grid layers with different Gaussian process functions (up to 3 layers if all coordinates are interpolated) [17].

At some threshold $p_{match}^2$, with uncertainty $p_j^2$The vertices are sufficiently accurate or valid. Triangular mesh faces are built in the 2D space of locations by connecting adjacent or diagonal vertices. A mesh face is valid if all vertices are valid. Similar to Delaunay triangulation, which establishes edges in 2D space, our method prevents thin mesh faces.

4. Point-to-grid registration (key point)

As mentioned before, we perform positioning and meshing simultaneously so that meshing can benefit from positioning. To do this, an intuitive idea is to treat vertices as points or extract points from faces and then use traditional point cloud registration for pose estimation. However, this idea ignores the normal information of the mesh faces. Puma [3] showed that point-to-grid errors can improve accuracy. Unlike the ray casting data association in Puma, our SLAMesh establishes correspondence based on position.

image 3. Data associations can be established between cells next to predicted coordinates (green cells). As the registration tends to converge, the query length $b$ can be reduced. from (a) to ©,$b$ decreases from 2 to 0

For the reconstructed current scan $S^{}$Vertex vp v_pin${}^g$${}^p$$v_p$, we first query the subgrid layers located in the same or adjacent cells (see Figure 3), and then find the vertices $v_q$(See Figure 2(d)). Find nq n_q along edges in the grid$n_q$adjacent vertices are fast. Contains $v_q$The normals of those faces are averaged to obtain a smooth normal $\bar{n}$ , in case the surface is not smooth. Create$v_p$and contains $v_q$Data associations between valid faces of :

where $||\cdot ||$ is the 2-norm. We denote the corresponding quantity in a cell as$n_q$, the number of overlapping cells is $K.$ _ Point$v_p^{\prime }$and apply the transformation $T\in SE3$ (includes rotating$R$ and translation$t$ ) residuals from the points to the grid.

The superscript $T$ stands for matrix transpose. Optimal relative transformation$T$ is calculated in an optimization problem where all$K$ overlapping layers$L_k$The residuals from the points in to the grid are merged.

We use the Levenberg-Marquardt algorithm in Ceres solver1 to solve this nonlinear least squares problem. Analytical Jacobian matrices are derived to speed up the solution process.

where the symbol $(\cdot {)}_{\times }$Represents the corresponding antisymmetric matrix of the vector. A mesh may contain many faces, so the optimization problem may be very large. We merge the residuals within each layer into one by averaging before optimization to speed up the optimization process.

Storing data in voxel units introduces discontinuities in data association at boundaries. If we only perform registration on overlapping cells, the convergence region of the registration will be too narrow when the vehicle is moving very fast. Therefore, we allow cross-unit overlap by querying the units next to the prediction axis of each layer, as shown in Figure 3. As registration tends to converge, the length of the query decreases.

5. Grid management and multi-threading

…For details, please refer to Gu Yueju