A laser radar positioning and three-dimensional mapping method based on implicit neural field

Abstract

The application discloses a laser radar positioning and three-dimensional mapping method based on an implicit neural field, and belongs to the technical field of positioning and map construction. The method comprises the following steps: for first frame data in radar point cloud data, a unit matrix or a constant matrix is used to convert the input sampling points from a sensor coordinate system to a world coordinate system to initialize a map; a self-supervised method is used to optimize layered latent features, and an octree data structure is used to store the optimized latent features to generate a latent feature set; features in the latent feature set are decoded into signed distance values by a shallow multilayer perceptron (MLP); starting from second frame data of the radar point cloud data, a predicted pose is obtained by matching each input frame to an implicit map; the obtained predicted pose is used to convert newly input points to the world coordinate system, and whether new latent features need to be added is determined according to the Morton value of the newly input points; and the NSDF construction is completed by repeating the operations of predicting the pose and expanding the NSDF.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of positioning and mapping, and in particular to a laser radar positioning and three-dimensional mapping method based on an implicit neural field. BACKGROUND

[0002] SLAM (Simultaneous Localization and Mapping) is a technology that integrates positioning and mapping, mainly applied in the fields of mobile robots, autonomous vehicles, etc. The goal of SLAM is to estimate the pose (position and attitude) of a mobile robot in six degrees of freedom (6-DoF) and construct a map of the environment around the mobile robot. LiDAR Odometry is a technology that estimates the pose changes (position and direction) of a robot or vehicle by using LiDAR data.

[0003] Most existing three-dimensional reconstruction works in map reconstruction are developed for RGB-D cameras and indoor environments, which are not suitable for large-scale outdoor environments. For large-scale mapping, LiDAR plays a key role due to its ability to provide accurate distance measurements. In the use of LiDAR positioning methods, existing technologies are mostly explicit methods, i.e., directly relying on point cloud data itself, or extracting point features, face features, edge features, etc. from point cloud data to minimize the Euclidean distance by finding corresponding points or features in adjacent frames to obtain the pose. Existing deep learning methods also mostly use supervised training to obtain the pose, i.e., additional real poses are needed in the training process, which requires a large amount of real data from the environment. Due to the possibility of noise in LiDAR data, especially in harsh weather conditions (such as rain, snow, heavy fog, etc.), these noises will affect the feature extraction and matching process, leading to an increase in pose estimation error and a large computational overhead. In real-time applications, processing speed may become a bottleneck, especially when the point cloud density is high, and the initial pose estimation is highly dependent. If the initial guess is too large, it may lead to convergence to a local optimal solution, resulting in a large error and being unable to cope with dynamic environments, easily affected by dynamic objects, and affecting position estimation. SUMMARY

[0004] The present application provides a laser radar positioning and three-dimensional mapping method based on an implicit neural field, which can solve the problems of existing SLAM technology, such as sensitivity to noise, large computational overhead, strong dependence on initial pose estimation, and inability to cope with dynamic environments. The present application aims to provide a new solution that is more intelligent, efficient, and adaptable to outdoor environments, to effectively improve the accuracy and reliability of laser radar in positioning and mapping.

[0005] The present application solves the above technical problems and proposes a laser radar positioning and three-dimensional mapping method based on implicit neural field, comprising the following steps:

[0006] S1 initializes the map and the neural symbolic distance field NSDF:

[0007] S1.1 initializes the map: for the first frame of data in the radar point cloud data, the input sampling points are converted from the sensor coordinate system to the world coordinate system using a unit matrix or a constant matrix, and the points converted to the world coordinate system are used to initialize the map, and in the process of initializing the map, the hierarchical latent features are randomly generated by Gaussian distribution;

[0008] S1.2 initializes the neural symbolic distance field NSDF: the hierarchical latent features are optimized in a self-supervised manner, and the optimized latent features are stored in an octree data structure to generate a latent feature set F, and the features in the latent feature set F are decoded into symbolic distance values by a shallow multilayer perceptron MLP to complete the initialization of the NSDF;

[0009] S2 predicts the pose and extends the NSDF:

[0010] S2.1 predicts the pose: starting from the second frame of data of the radar point cloud data, a predicted pose is obtained by frame-to-implicit map matching for each input frame;

[0011] S2.2 extends the NSDF: using the obtained predicted pose, the new input points are converted to the world coordinate system, and whether the new input points are in the current map is determined according to the Merton Morton value of the new input points; if the new input points have corresponding nodes, new latent features do not need to be added; otherwise, the new latent features are also stored in the latent feature set F based on the octree data structure in a hierarchical manner;

[0012] By repeatedly predicting the pose and extending the NSDF, a complete map and the poses of all data frames are obtained after all the radar point cloud data are inputted, and the NSDF construction is completed.

[0013] Beneficial effects: the present application is based on a self-supervised training method, i.e. no real pose training is needed, and it is an implicit method, i.e. we do not need to find corresponding points or corresponding features in adjacent frames, which is a kind of alignment without corresponding relationship, and only the hierarchical latent features based on the octree structure are used to minimize the SDF value of the current frame to obtain the pose of the current frame. Since it is not necessary to find corresponding points or corresponding features in adjacent frames, the computational consumption is also reduced. Moreover, the hierarchical feature fusion also reduces the influence of dynamic objects on the positioning accuracy.

[0014] In terms of map construction, unlike traditional explicit reconstruction methods, it adopts an implicit approach to represent the map, resulting in more realistic details. The octree-based data structure storage and hierarchical index features not only reduce memory consumption when building large-scale maps, but also improve the efficiency of data access. The multi-layered latent features make the map details richer, making large-scale 3D mapping more efficient and accurate. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the Hi-LOAM system architecture in this invention;

[0016] Figure 2 This is a flowchart illustrating the lidar localization and 3D mapping method based on implicit neural fields in this invention. Detailed Implementation

[0017] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments, but the scope of protection of the present invention is not limited to the content described.

[0018] This invention proposes a lidar localization and 3D mapping method based on implicit neural fields, integrating localization and map construction into a unified implicit framework. Existing lidar localization methods typically employ explicit approaches, relying directly on point cloud data or extracting point, surface, and edge features from the point cloud data to minimize the Euclidean distance by finding corresponding points or features in adjacent frames to obtain the pose. Existing deep learning methods also mostly rely on supervised training to obtain the pose, requiring additional real-world pose data during training.

[0019] The invention designs a novel system architecture, named Hi-LOAM, which implicitly constructs large-scale environments from input data solely from LiDAR (Light Detection and Ranging) systems. Its system architecture is as follows: Figure 1 As shown, firstly, points sampled along the light rays are used to construct an octree-based neural symbolic distance field (NSDF). Secondly, the endpoints of the light rays are input into a neural implicit odometry system to obtain the pose. Specifically, the pose of the previous frame in the video is used as the initial pose of the current frame. This pose is quickly indexed using a hash table through the NSDF to query hierarchical latent features. Then, the latent features from different levels are fused, and the Levenberg-Marquardt method is used to minimize the loss function to obtain the predicted pose of the current frame. Furthermore, the predicted pose is used to transform the input points (i.e., LiDAR data) into global coordinates, and the NSDF is further expanded. This transformation and expansion process is repeated until the map is constructed. At this point, the Marching Cubes algorithm is used to extract isosurfaces to visualize the map. The multi-level latent features ensure both the accuracy of the localization and the completeness of the map details.

[0020] likeFigure 2 As shown, the laser radar positioning and three-dimensional mapping method based on implicit neural field in the present application comprises:

[0021] S1 initializes the map and the neural signed distance field NSDF:

[0022] S1.1 initializes the map: for the first frame of data in the radar point cloud data, the input sampling points are converted from the sensor coordinate system to the world coordinate system using a unit matrix or a constant matrix, and the points converted to the world coordinate system are used to initialize the map, and in the process of initializing the map, the hierarchical latent features are randomly generated by Gaussian distribution.

[0023] S1.2 initializes the neural signed distance field NSDF: the hierarchical latent features are optimized in a self-supervised manner, and the optimized latent features are stored in an octree data structure to generate a latent feature set F, and the features in the latent feature set F are decoded into signed distance values by a shallow multilayer perceptron MLP to complete the initialization of the NSDF.

[0024] Specifically, the latent feature is denoted as F, L is the number of features. F is a one-dimensional feature vector set, and the length of each feature vector in the set is H1, which is stored in the octree.

[0025] Octree is a tree data structure commonly used to represent discrete structures in three-dimensional space. It recursively divides three-dimensional space into eight equal-sized cubes, each of which is called a node. This process can continue recursively, subdividing each child node into eight child nodes, until the desired depth or condition is reached. An octree node consists of 8 corners. Each corner has a corresponding one-dimensional latent feature. Since the octree contains multiple levels, we store the corresponding latent features at multiple levels, which constitute our hierarchical latent features. The latent features at different levels express the details of the map from coarse to fine. Our method only stores the last three levels of octree latent features. The level of the octree is defined as Level K, where K = 0,1,2,…,k-1. It should be noted that the first level is defined as Level 0. In different levels, nodes without points are pruned. The purpose of this pruning operation is to optimize the memory usage for large-scale scene construction.

[0026] Further, when generating the latent feature set F in the octree data structure, the table N and the table G are also generated, where the table N is represented as , which is used to establish the relationship between the hierarchical nodes in the octree data structure and the hierarchical latent features, and the table G is represented as This is used to establish the relationship between the corner points of hierarchical nodes and the hierarchical latent features; Morton coding is used to map the three-dimensional coordinates to one-dimensional coding, and the hierarchical latent features are quickly indexed by combining tables N and G. The Morton coding of the corner points of the nodes in table G corresponds to the index of a single latent feature in the latent feature set F.

[0027] Morton coding is a method for mapping points in a multidimensional space to a one-dimensional space. It is often used to preserve locality in multidimensional data, meaning that adjacent points remain adjacent in the one-dimensional encoding. The basic idea of ​​Morton coding is to interleave multidimensional coordinates bit by bit to form a single integer code. The most common form is zigzag coding, where adjacent points are also adjacent in the encoding. This helps maintain locality in one-dimensional sequences, thus improving access efficiency. It is worth noting that NSDF is an continuously expanding process with the input.

[0028] S2 Predicted Pose and Extended NSDF:

[0029] S2.1 Predicted pose: Starting from the second frame of radar point cloud data, for each input frame, the predicted pose is obtained through a frame-to-implicit map matching method.

[0030] The process of obtaining the predicted pose by matching frames to an implicit map is called implicit odometry. This process is not performed in the first frame of data. Starting from the second frame, the implicit odometry is used to predict the pose of each input frame.

[0031] Specifically, in this invention, the method of obtaining the predicted pose through frame-to-implicit map matching uses a constant-speed motion model to initialize the pose. Specifically, the pose is obtained by using formula (1) based on the poses of the first two frames. and Predict the initial pose of the current frame. Formula (1) is: (1); secondly, based on the initial pose, the current frame is transformed to the world coordinate system, and the SDF value of the input point in the current frame is queried through NSDF; finally, the Levenberg-Marquardt (LM) algorithm or Adam optimizer is used to minimize the SDF value to iteratively obtain the predicted pose of the current frame. R is a 3x3 rotation matrix, and t is a 3x1 translation vector.

[0032] Levenberg-Marquardt (LM) algorithm is a nonlinear least squares optimization algorithm. It combines the advantages of gradient descent and Gauss-Newton method, and performs well in handling nonlinear least squares problems. It is mainly used for curve fitting, surface fitting and other data fitting problems, as well as parameter optimization in neural network and other machine learning models.

[0033] The specific solving process of LM algorithm for minimizing SDF value is as follows:

[0034] (2) where p ct represents the point cloud in the coordinate system under the sensor, p is any point in the coordinate system under the sensor, R and t are the initial pose rotation matrix and translation vector in the initial pose , and arg min R

[0035] , t * is the current frame of the predicted pose R * , t .

[0036] In order to solve the above equation using LM algorithm, the Jacobian matrix J is calculated, and in the process of solving the Jacobian matrix, we actually assume that the rotation angle is small. From the derivation of the Rodrigues formula, we can get the following formula (3). Since the derivation process is more, we do not give the specific derivation process here. represents the point cloud in the world coordinate system, the gradient of the point, which can usually be obtained through the automatic differentiation mechanism of pytorch framework, so we do not need to calculate the gradient additionally in our method. From the above formula (2), it can be seen that formula (2) does not show the corresponding relationship between the point and the point or the point and the plane, which is different from the traditional ICP method. Our method is an implicit method without correspondence. Then we can approximate the Hessian matrix H= and the gradient of the objective function g= From this we can get the increment of each iteration process, as shown in the following formula (4):

[0037] ; (4)

[0038] diag(H) is to extract the diagonal elements of H and create a diagonal matrix. After several iterations, when the increment is small enough, we can get the final predicted pose, as shown in the following formula (5):

[0039] (5) where exp( ) represents the incremental anti-symmetric matrix, R* is the predicted rotation matrix, and t* is the predicted translation vector.

[0040] S2.2 Extend NSDF: Convert the new input point to the world coordinate system using the obtained predicted pose, and determine whether it is in the current map according to the Morton value of the new input point. If the new input point has a corresponding node, there is no need to add a new potential feature; otherwise, the new potential feature is also stored in the potential feature set F based on the octree data structure. Correspondingly, the table G and the table N are also extended.

[0041] In order to determine the size of the current map, the maximum node coordinates maxk and the minimum node mink of each level k of the octree are traversed, and the calculation formula is M=(maxk - mink) / Scube(6), wherein Scube represents the size of the MarchingCubes cube, and M represents the number of cubes of the entire map on the x, y and z axes.

[0042] By repeating the operations of predicting the pose and extending the NSDF, all frames of the radar point cloud data are inputted completely, and a complete map and the poses of all data frames are obtained, so that the NSDF construction is completed.

[0043] After the NSDF construction is completed, the Marching Cubes algorithm is used to convert the implicitly represented map into a visual form.

[0044] Optionally, in the above method, the parameters are: for the MLP, the number of hidden layers p of all MLPs is 2. scube is 0.1 meters, a is 0.05, the number of sampling points N of the LiDAR ray is 6, the length of the feature vector H1 is 12, the octree level K=15, and the scaling factor .

[0045] In summary, the present application is based on a self-supervised training method, that is, no real pose training is needed, and it is an implicit method, that is, we do not need to find the corresponding points or corresponding features of adjacent frames, which is a non-correspondence alignment method, only the hierarchical latent features based on the octree structure are used to obtain the pose of the current frame by minimizing the SDF value of the current frame, since it is not necessary to find the corresponding points or corresponding features of adjacent frames, the calculation consumption is also reduced. Moreover, the hierarchical feature fusion also reduces the influence of dynamic objects on the positioning accuracy. In terms of map construction, unlike traditional explicit reconstruction methods, an implicit method is used to represent the map, so the details are more realistic, the octree-based data structure can reduce memory consumption when constructing a large-scale map, and the multi-layer latent features make the map details more abundant. Therefore, the octree data structure is used to store and index the hierarchical latent features, which not only reduces the memory consumption, but also improves the data access efficiency, making large-scale 3D mapping more efficient and accurate.

[0046] Finally, it should be pointed out that the above preferred embodiments are only used to illustrate the technical solutions of the present application and are not limiting. Although the present application has been described in detail through the above preferred embodiments, those skilled in the art should understand that various changes can be made in form and details without departing from the scope defined by the claims of the present application.

Claims

1. A lidar localization and 3D mapping method based on implicit neural fields, characterized in that, Includes the following steps: S1 Initialization Map and Neural Symbol Distance Field (NSDF): S1.1 Initialize the map: For the first frame of radar point cloud data, use an identity matrix or constant matrix to transform the input sampling points from the sensor coordinate system to the world coordinate system, and use the transformed points in the world coordinate system to initialize the map. During the map initialization process, hierarchical latent features are randomly generated through Gaussian distribution. S1.2 Initialize the Neural Symbolic Distance Field (NSDF): Optimize the hierarchical latent features using a self-supervised approach, and use an octree data structure to store the optimized latent features to generate a latent feature set F. Then, decode the features in the latent feature set F into symbolic distance values ​​using a shallow multilayer perceptron (MLP) to complete the initialization of the NSDF. S2 Predicted Pose and Extended NSDF: S2.1 Predicted pose: Starting from the second frame of radar point cloud data, the predicted pose is obtained for each input frame by matching the frame to the implicit map. S2.2 Extended NSDF: Using the obtained predicted pose, the new input point is transformed into the world coordinate system. The Morton value of the new input point is used to determine whether it is in the current map. If the new input point has a corresponding node, there is no need to add new latent features; otherwise, the new latent features are also stored hierarchically in the latent feature set F based on the octree data structure. By repeatedly predicting pose and expanding NSDF, the complete map and pose of all data frames are obtained after all radar point cloud data has been input, thus completing the NSDF construction.

2. The lidar localization and 3D mapping method based on implicit neural fields according to claim 1, characterized in that, When generating the latent feature set F in an octree data structure, tables N and G are also generated simultaneously; where table N is represented as... This is used to establish the relationship between hierarchical nodes and hierarchical potential features in an octree data structure. Table G is represented as This is used to establish the relationship between the corner points of hierarchical nodes and the potential features of the hierarchical structure. Morton coding is used to map three-dimensional coordinates to one-dimensional codes. Combined with tables N and G, hierarchical latent features are quickly indexed. The Morton code of the corner point of a node in table G corresponds to the index of a single latent feature in the latent feature set F.

3. The lidar localization and 3D mapping method based on implicit neural fields according to claim 1, characterized in that, The method of obtaining the predicted pose through frame-to-implicit map matching includes: Formula (1) is used to obtain the pose from the first two frames. and Predict the initial pose of the current frame. Formula (1) is: (1); Based on the initial pose, the current frame is transformed to the world coordinate system, and the SDF value of the input point in the current frame is queried through NSDF. The predicted pose of the current frame is obtained by iteratively minimizing the SDF value using the LM algorithm or the Adam optimizer. in, R is a 3x3 rotation matrix, and t is a 3x1 translation vector.

4. The lidar localization and 3D mapping method based on implicit neural fields according to claim 1, characterized in that, Determining the size of the map includes: Traverse the octree to find the maximum node coordinate maxk and the minimum node coordinate mink at each level of the octree; The map size of the current level is calculated using the formula M = (maxk - mink) / Scube, where Scube is the size of the MarchingCubes cube.

5. The lidar localization and 3D mapping method based on implicit neural fields according to claim 1, characterized in that, The visualization of the map includes: The Marching Cubes algorithm is used to convert implicitly represented maps into a visual form.

6. The lidar localization and 3D mapping method based on implicit neural fields according to claim 1, characterized in that, The MLP has 2 hidden layers, a latent feature vector length H1 of 12, and an octree level K of 15.

Images