A method, system, medium and product for optimizing construction of large-scale point cloud maps
Through sliding windows and BA optimization model based on plane features, the problems of cumulative drift and low BA algorithm efficiency in large-scale scenarios in laser SLAM are solved, and high-precision and efficient large-scale point cloud map construction are realized, for autonomous driving and The robotics industry provides technical support.
Patent Information
- Application Number
- CN202410954092.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-17
- Publication Date
- 2024-11-01
- Estimated Expiration
- 2044-07-17
AI Technical Summary
In the field of laser SLAM, existing algorithms are difficult to eliminate cumulative drift in large-scale scenarios, resulting in deviations in graph construction. The loopback detection method based on distance or Scan Context cannot meet the construction of large-scale scenarios. The BA algorithm is computed in laser SLAM. Low, it is difficult to optimize point-to-point feature matching.
Sliding windows are used to sample and iterate the initial global point cloud map, build a BA optimization model based on plane features, optimize the position pose of each point cloud map in the pyramid model, and build a factor graph optimization model, with all inter-frame relative poses The set is a constraint to solve the final large-scale global point cloud map.
It effectively improves the accuracy of map construction, optimization efficiency and global consistency of large-scale point cloud maps, reduces the cost of map optimization, and provides technical support for the autonomous driving and robotics industries.
Smart Images

Figure CN118505920B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of laser SLAM, and in particular to an optimization construction method, system, medium and product for a large-scale point cloud map. Background Art
[0002] In the field of laser SLAM technology, many excellent algorithm frameworks have been born, such as LOAM, LeGO-LOAM, LIO-SAM and FAST-LIO series. These algorithms reduce drift by tightly coupling with IMU (Inertial Measurement Unit) or loop detection. But without exception, in large-scale scenes, cumulative drift will occur and cannot be completely eliminated, resulting in deviations in mapping. The reasons include the influence of dynamic objects, degraded scenes, obstruction of the laser radar view, unstable GPS signals, etc. Since the environmental information perceived by the laser radar is relatively simple, it is difficult to use the bag-of-words model for loop detection like visual SLAM. Laser SLAM generally uses distance-based or Scan Context-based laser descriptors to detect loops, and neither of these two methods can meet the construction of large-scale scenes.
[0003] BA (Bundle Adjustment) is an optimization algorithm for simultaneously estimating 3D structure and sensor motion. It has been widely used to optimize camera pose and map accuracy in the field of visual SLAM and 3D reconstruction. However, in the field of laser SLAM, the feature points are sparse and non-repetitive, which makes it impossible to perform point-to-point feature matching. In addition, how to optimize the computational efficiency of BA is also a current problem. Summary of the invention
[0004] The purpose of the present invention is to provide a method, system, medium and product for optimizing the construction of large-scale point cloud maps, which can effectively improve the mapping accuracy and optimization efficiency and global consistency of large-scale point cloud maps.
[0005] To achieve the above object, the present invention provides the following solution.
[0006] In a first aspect, the present invention provides a method for optimizing and constructing a large-scale point cloud map, comprising the following steps.
[0007] Get the laser radar pose set and plane feature point set collected by the laser SLAM module.
[0008] An initial global point cloud map is constructed based on the laser radar pose set and the plane feature point set.
[0009] The initial global point cloud map is sampled iteratively using a sliding window, and each sampling is performed on the local point cloud map obtained by the previous sampling. When a preset stop condition is met, the sampling is stopped to obtain a pyramid model consisting of the initial global point cloud map and multiple local point cloud maps.
[0010] A BA optimization model based on plane features is constructed, and then the BA optimization model is used to optimize the pose of each point cloud map in the pyramid model to obtain a relative pose set between frames corresponding to each point cloud map.
[0011] Based on the pyramid model, a factor graph optimization model is constructed with all the relative pose sets between frames as constraints.
[0012] The factor graph optimization model is solved to obtain a final large-scale global point cloud map.
[0013] In a second aspect, the present invention provides a computer system comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method for optimizing the construction of the large-scale point cloud map.
[0014] In a third aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method for optimizing the construction of the large-scale point cloud map.
[0015] In a fourth aspect, the present invention provides a computer program product, comprising a computer program, which, when executed by a processor, implements the steps of the method for optimizing the construction of a large-scale point cloud map.
[0016] According to the specific embodiment provided by the present invention, the present invention discloses the following technical effects: according to the laser radar pose set and plane feature point set collected by the laser SLAM module, an initial global point cloud map is constructed, and then a sliding window is used to sample and iterate it to obtain a pyramid model; a BA optimization model based on plane features is constructed, and it is used to optimize the pose of each point cloud map in the pyramid model to obtain the relative pose set between frames corresponding to each point cloud map; with all the relative pose sets between frames as constraints, a factor graph optimization model is constructed based on the pyramid model, and the final large-scale global point cloud map is obtained after solving. It can be seen from the above that the present invention proposes a more effective feature matching method, and the BA algorithm is efficiently applied to the optimization of point cloud maps. Constructing a global consistency map is the key to solving the cumulative drift of laser SLAM. The present invention uses a multi-layer pyramid model based on a sliding window to divide the global map for parallel processing, and finally constructs a global factor graph optimization model, which can effectively improve the mapping accuracy and the optimization efficiency and global consistency of large-scale point cloud maps, reduce the map optimization cost, and provide technical support for the development of autonomous driving and robotics industries. BRIEF DESCRIPTION OF THE DRAWINGS
[0017] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0018] Figure 1 It is a schematic diagram of the process of the optimization construction method of the large-scale point cloud map of the present invention.
[0019] Figure 2 It is a structural schematic diagram of the pyramid model of the present invention.
[0020] Figure 3 It is the connection graph of the factor graph optimization model of the present invention. DETAILED DESCRIPTION
[0021] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0022] The purpose of the present invention is to provide a method, system, medium and product for optimizing the construction of large-scale point cloud maps, which uses plane features in the BA algorithm to optimize the lidar posture, and based on a multi-layer pyramid model of a sliding window, the global map can be segmented and processed in parallel, and finally a global factor graph optimization is constructed, which can effectively improve the mapping accuracy and optimization efficiency of large-scale point cloud maps, reduce the map optimization cost, and provide technical support for the development of the autonomous driving and robotics industries.
[0023] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.
[0024] like Figure 1 As shown, the present invention provides an optimization construction method for a large-scale point cloud map, comprising the following steps.
[0025] Step 100, obtaining a laser radar pose set and a plane feature point set collected by the laser SLAM module. The laser radar pose set includes multiple frames of laser radar poses obtained by laser radar scanning, and the plane feature point set includes multiple plane feature point subsets obtained by laser radar scanning, and each plane feature point subset corresponds to one frame of laser radar pose.
[0026] Step 200, constructing an initial global point cloud map based on the laser radar pose set and the plane feature point set. The present invention is used for map optimization after SLAM mapping, and the pose of each frame of laser radar and the subset of plane feature points output by the laser SLAM module are used to construct a large-scale initial global point cloud map in a world coordinate system; the initial global point cloud map still includes multiple laser radar frames; each of the laser radar frames includes the corresponding laser radar pose and the subset of plane feature points.
[0027] Step 200 specifically includes: using the laser radar pose set, converting multiple plane feature points corresponding to each frame in the plane feature point set into the same world coordinate system to obtain an initial global point cloud map; wherein the conversion formula is as follows.
[0028] .
[0029] Among them, P ij represents the position of the i-th plane feature point of the j-th frame in the world coordinate system, j and t j represents the j-th frame lidar pose in the lidar pose set, represents the position of the i-th plane feature point in the j-th frame in the laser radar coordinate system, represents the total number of plane feature points in the plane feature point subset, and M represents the number of laser radar frames. It should be noted that the number of plane feature points corresponding to each frame, that is, the total number of plane feature points in each plane feature point subset, may be different.
[0030] Step 300, using a sliding window to iteratively sample the initial global point cloud map, and each sampling is performed on the local point cloud map obtained by the previous sampling, until the preset stop condition is met, the sampling is stopped, and a pyramid model consisting of the initial global point cloud map and multiple local point cloud maps is obtained.
[0031] Specifically, Figure 2 As shown, a first sliding window is used to sample the initial global point cloud map to obtain a first local point cloud map; a second sliding window is used to sample the first local point cloud map to obtain a second local point cloud map; a third sliding window is used to sample the second local point cloud map to obtain a third local point cloud map, and the above sampling is repeated until a preset stop condition is met, thereby dividing the global point cloud map into a multi-layer set of local point cloud maps.
[0032] In one example, the size and step length of the sliding window are first set, such as setting the size of the sliding window to 8 and the step length to 4. Relevant technicians can adjust the parameters according to the number of point clouds per frame and the computing power of the computer to achieve the best optimization effect. By setting the size and step length of the sliding window, the number of sliding windows required based on the existing initial global point cloud map can be determined. It should be noted that by setting the size and step length of the sliding window above, overlapping areas can also exist between adjacent sliding windows. That is, when sliding sampling is performed, there are multiple overlapping lidar frames between two adjacent sliding windows, and the overlapping area can ensure the smoothness of the subsequent posture optimization.
[0033] Then, the initial global point cloud map is sampled using a sliding window. This step specifically includes: the initial global point cloud map is sampled using a sliding window, and the laser radar frame corresponding to the plane feature point subset with the largest number of points in the sliding window is marked as a key frame; multiple key frames constitute a local point cloud map. That is, a frame containing the most plane feature points in the sliding window is selected as a key frame, the lower sliding window provides a key frame for the upper layer, and multiple key frames constitute a local point cloud map of the next layer, which is optimized again.
[0034] The number of pyramid layers is not added until no plane points can be extracted in the overlapping area of adjacent key frames, and the final pyramid model is obtained. That is, the preset stop condition is: in any local point cloud map, there are no plane feature points in the overlapping area of adjacent key frames.
[0035] Step 400, constructing a BA optimization model based on plane features, and then using the BA optimization model to optimize the pose of each point cloud map in the pyramid model to obtain a relative pose set between frames corresponding to each point cloud map.
[0036] In one example, the BA optimization model is used to perform multi-threaded parallel processing of pose optimization on multiple point cloud maps in the pyramid model. Specifically, a C++ thread pool is used to perform multi-threaded parallel processing on multiple local maps to optimize the global pose, and all relative poses between frames are recorded as constraint relationships for subsequent factor graph global optimization.
[0037] The constructed BA optimization model based on plane features acts on all plane feature points in the local point cloud map. The derivation process of the BA optimization model is as follows.
[0038] Select a subset of plane feature points in any point cloud map (i=1, ..., N), N represents the total number of plane feature points in the plane feature point subset; the i-th plane feature point comes from the s-th plane feature point. i Frames of LiDAR scans, ; The M-times scanned pose is converted into the LiDAR pose T = (T1, ..., T M ) represents, T1 represents the laser radar pose obtained by the first laser radar scan, T M Indicates M The lidar pose obtained from the lidar scan.
[0039] , the coordinates of all these feature points in world coordinates are shown below.
[0040] .
[0041] The covariance matrix and mean value are calculated based on the coordinates. The calculation formula is as follows.
[0042] ; .
[0043] Among them, p i Represents the position of the i-th plane feature point in the world coordinate system; Represents N p i The average value of Indicates the s i A pose expression of the lidar pose obtained from a lidar scan.
[0044] According to the above description, the BA problem in laser SLAM can be simplified to determine the optimal pose T of the plane feature, that is, to minimize the feature point p iThe sum of the squares of the distances to the plane, where n is the plane unit normal vector.
[0045] .
[0046] in, represents the minimum eigenvalue of the covariance matrix A, is the eigenvector corresponding to the minimum eigenvalue. So far, the problem is modeled as minimizing the minimum eigenvalue of the covariance matrix.
[0047] To solve this problem, we need to derive the derivative of the minimum eigenvalue expression with respect to the posture, and first derive the derivative with respect to the coordinate point according to the chain rule.
[0048] .
[0049] In the above formula, represents the eigenvalue of the covariance matrix A, express The corresponding feature vector.
[0050] Approximate the formula according to the first-order approximation , and the following conclusions are obtained.
[0051] .
[0052] In the above formula, J(p) is the derivative about the coordinate point The first-order Jacobian matrix in the formula of . According to the chain rule, it is necessary to derive the derivative of the coordinate point p with respect to the posture T as follows.
[0053] .
[0054] .
[0055] .
[0056] .
[0057] in, Indicates the s i The lidar pose obtained by the lidar scan; I represents the identity matrix.
[0058] The formula and formula Substitute into the formula , and the formula is as follows.
[0059] .
[0060] In summary, the BA optimization model can be expressed as follows.
[0061] .
[0062] .
[0063] .
[0064] The BA optimization model can use Gauss-Newton to iteratively solve the minimization cost function, and the incremental equation is shown below.
[0065] .
[0066] Step 500: Based on the pyramid model, a factor graph optimization model is constructed with all the relative pose sets between frames as constraints. Specifically, based on the pyramid model, the following is constructed: Figure 3 As shown in the structural diagram, it is precisely because of this pyramid structure and the overlapping area of the sliding window that the pose optimized by the factor graph can maintain global consistency and smoothness.
[0067] The objective function in the factor graph optimization model is as follows.
[0068] .
[0069] when When , there is a first-layer minimization objective function formula as shown below.
[0070] .
[0071] in, (T) represents the value of the objective function, L represents the number of point cloud maps in the pyramid model, Indicates f1(T) represents the target value of the initial global point cloud map; represents the number of sliding windows in the initial global point cloud map, j=(i-1)ω+1 represents the index of the first frame of each sliding window, i represents the index of the plane feature point, S represents the size of the sliding window, ω represents the step size of the sliding window, and m represents the total number of lidar frames. represents the LiDAR pose, 1 indicates the preset weight; j represents the lidar pose corresponding to the jth lidar frame, T j+1 Represents the lidar pose corresponding to the j+1th lidar frame.
[0072] By analogy, we get the objective functions of all layers corresponding to L=2, 3, 4, etc., and then add up the L layers to minimize the objective function, that is, to get the final f(T).
[0073] Step 600, solve the factor graph optimization model to obtain the final large-scale global point cloud map. Specifically, use GTSAM to solve the factor graph optimization model, complete global consistency optimization, and obtain corresponding multiple optimized poses; then use the optimized poses to splice each frame of the point cloud map to obtain the final optimized large-scale point cloud map.
[0074] In summary, the present invention uses the posture and plane feature points of the laser radar to construct a global map; uses a multi-layer pyramid model based on a sliding window to divide the global map into a multi-layer local map set; constructs a BA optimization model based on plane features, optimizes the posture of each local map, and uses multi-threading to process all local maps in parallel; based on the factor graph optimization model, all relative posture constraints between frames after BA optimization are added to the factor graph model, and GTSAM is used to solve and complete global consistency optimization. The present invention applies plane features to BA optimization, applies the BA optimization method to the field of laser SLAM, and uses a multi-layer pyramid model to divide the global map for parallel processing, and finally constructs a global factor graph optimization, which can effectively improve the mapping accuracy and the optimization efficiency of large-scale point cloud maps, reduce the cost of map optimization, and provide technical support for the development of autonomous driving and robotics industries.
[0075] In another embodiment, the present invention provides a computer device, which may be a database. The computer device includes a processor, a memory, an input / output interface (Input / Output, referred to as I / O) and a communication interface. The processor, the memory and the input / output interface are connected via a system bus, and the communication interface is connected to the system bus via the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store pending transactions. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, an optimization construction method for a large-scale point cloud map is implemented.
[0076] In another embodiment, a computer system is provided, including a memory and a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps in the above-mentioned method for optimizing the construction of a large-scale point cloud map when executing the computer program.
[0077] In another embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps in the above-mentioned method for optimizing the construction of a large-scale point cloud map are implemented.
[0078] In another embodiment, a computer program product is provided, including a computer program, which, when executed by a processor, implements the steps in the above-mentioned method for optimizing the construction of a large-scale point cloud map.
[0079] It should be noted that the object information (including but not limited to object device information, object personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the object or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant laws, regulations and standards of relevant countries and regions.
[0080] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.
[0081] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0082] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of the present invention. At the same time, for those skilled in the art, according to the idea of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.
Claims
1. A method for optimizing and constructing a large-scale point cloud map, characterized in that: include: Obtain the laser radar pose set and plane feature point set collected by the laser SLAM module; Based on the laser radar pose set and the plane feature point set, an initial global point cloud map is constructed; specifically comprising: using the laser radar pose set, converting multiple plane feature points corresponding to each frame in the plane feature point set to the same world coordinate system to obtain an initial global point cloud map; wherein the conversion formula is as follows: ; in, represents the position of the i-th plane feature point in the j-th frame in the world coordinate system, R j and t j represents the j-th frame lidar pose in the lidar pose set, represents the position of the i-th plane feature point of the j-th frame in the laser radar coordinate system, N represents the total number of plane feature points in the plane feature point subset, and M represents the number of laser radar frames; The initial global point cloud map is sampled iteratively using a sliding window, and each sampling is performed on the local point cloud map obtained by the previous sampling, until a preset stop condition is met, the sampling is stopped, and a pyramid model consisting of the initial global point cloud map and multiple local point cloud maps is obtained; Constructing a BA optimization model based on plane features, and then using the BA optimization model to optimize the pose of each point cloud map in the pyramid model to obtain a relative pose set between frames corresponding to each point cloud map; Based on the pyramid model, a factor graph optimization model is constructed with all the relative pose sets between frames as constraints; The factor graph optimization model is solved to obtain a final large-scale global point cloud map.
2. The optimization construction method of a large-scale point cloud map according to claim 1 is characterized in that: The initial global point cloud map includes a plurality of laser radar frames; each of the laser radar frames includes a corresponding laser radar pose and a subset of plane feature points; The step of sampling the initial global point cloud map using a sliding window specifically includes: sliding sampling the initial global point cloud map using a sliding window, and marking the laser radar frame corresponding to the plane feature point subset with the largest number of points in the sliding window as a key frame; a plurality of the key frames constitute a local point cloud map; The preset stop condition is: in any local point cloud map, there are no plane feature points in the overlapping area of adjacent key frames.
3. The optimization construction method of a large-scale point cloud map according to claim 2 is characterized in that: The step of sampling the initial global point cloud map using a sliding window also includes: when performing sliding sampling, there are multiple overlapping laser radar frames between two adjacent sliding windows.
4. The optimization construction method of a large-scale point cloud map according to claim 1 is characterized in that: The BA optimization model is: ; ; ; ; in, represents a set of plane feature point subsets in any point cloud map, i=1,…,N, N represents the total number of plane feature points in the plane feature point subset; the i-th plane feature point comes from the s-th plane feature point i Frames of LiDAR scans, ; T represents the laser radar posture, Indicates the s i The lidar pose obtained from the lidar scan; Indicates the s i A position expression of the LiDAR position obtained by the LiDAR scan; j = 1, ..., M; j represents the index of the LiDAR frame; M represents the number of LiDAR frames; p represents the position of the plane feature point in the world coordinate system, p i Represents the position of the i-th plane feature point in the world coordinate system; Represents N p i The average value of , I represents the identity matrix; represents the eigenvalue of the covariance matrix A, express The corresponding eigenvector; the function formula of the covariance matrix A is: 。 5. The optimization construction method of a large-scale point cloud map according to claim 1, characterized in that: The objective function in the factor graph optimization model is: ; when When , there is the following function formula: ; Among them, f(T) represents the value of the objective function, L represents the number of point cloud maps in the pyramid model, Indicates f1(T) represents the target value of the initial global point cloud map; represents the number of sliding windows in the initial global point cloud map, represents the index of the first frame of each sliding window, i represents the index of the plane feature point, S represents the size of the sliding window, represents the step size of the sliding window, m represents the total number of lidar frames, T represents the lidar pose, and c1 represents the preset weight; T j represents the lidar pose corresponding to the jth lidar frame, T j+1 Represents the lidar pose corresponding to the j+1th lidar frame.
6. The optimization construction method of a large-scale point cloud map according to claim 1, characterized in that: The method also includes: using the BA optimization model to perform multi-threaded parallel processing posture optimization on multiple point cloud maps in the pyramid model.
7. A computer system comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method for optimizing the construction of a large-scale point cloud map as described in any one of claims 1 to 6.
8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method for optimizing the construction of a large-scale point cloud map described in any one of claims 1 to 6 are implemented.
9. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method for optimizing the construction of a large-scale point cloud map described in any one of claims 1 to 6 are implemented.
