Map construction method and map construction device

Abstract

The present application provides a map construction method and a map construction device, which relate to the field of real-time positioning and map construction technology. Among them, the method includes: constructing multiple point cloud levels that are adjacent in sequence according to multi-frame point cloud data, performing voxel segmentation on each point cloud group in the point cloud level, and obtaining multiple voxels corresponding to each point cloud group, and performing posture optimization in the point cloud level according to the geometric characteristics of the feature points of the point cloud data in each voxel, and obtaining the posture optimization result corresponding to the point cloud level, and performing global optimization on the posture of the multi-frame point cloud data in combination with the preset constraint factor to obtain the optimized point cloud data, and performing coordinate transformation according to the optimized point cloud data to obtain a global map. The present application optimizes the posture by extracting the geometric characteristics of the feature points in the voxel, and introduces constraint factors for global optimization on the basis of hierarchical optimization, so as to ensure that the posture of all point cloud data in the global coordinate system is consistent, thereby improving the consistency of the global map.

Description

Technical Field

[0001] The present application relates to the technical field of real-time positioning and map construction, and in particular to a scene map construction method and a map construction device. Background Art

[0002] Laser SLAM (Simultaneous Localization and Mapping) refers to the technology of using laser radar sensors for real-time positioning and mapping. When a portable 3D laser scanning mapping system based on laser SLAM technology is used to scan and map a scene, the global accuracy and scale consistency of the mapping results are key indicators for measuring the quality of mobile mapping. However, as the scanning distance and time increase, cumulative errors will inevitably occur, and the cumulative errors will cause the position estimate to gradually deviate from the actual position, thereby affecting the global accuracy and scale consistency of the mapping results.

[0003] Existing mobile mapping methods mainly include closed-loop optimization methods and local bundle adjustment (BA) optimization methods, or a combination of the above two methods. Among them, closed-loop optimization refers to correcting the accumulated error through the detected closed loop when revisiting the previously scanned area, while local BA refers to reducing the accumulated error by jointly optimizing the observation data over a period of time. Although the above closed-loop optimization method can improve the global consistency of mapping, it still cannot solve the problem of position deviation in areas that are not revisited. Although the above local BA optimization method can alleviate the accumulated error within a certain time span, it cannot provide global constraints and the optimization efficiency is low.

[0004] Therefore, existing mobile mapping methods have certain limitations. The global accuracy and scale consistency of mobile mapping cannot be guaranteed, and the execution efficiency is low, which leads to low quality of mobile mapping. Summary of the invention

[0005] The purpose of this application is to provide a map construction method and a map construction device to address the deficiencies in the above-mentioned prior art, so as to solve the problem that mobile mapping in the prior art has certain limitations, the global accuracy and scale consistency of mobile mapping cannot be guaranteed, and the execution efficiency is low, which leads to low quality of mobile mapping.

[0006] To achieve the above purpose, the technical solution adopted in the embodiment of the present application is as follows:

[0007] In a first aspect, an embodiment of the present application provides a map construction method, the method comprising:

[0008] Obtain multi-frame point cloud data scanned and collected by a 3D laser radar sensor;

[0009] According to the multi-frame point cloud data, a plurality of point cloud levels that are sequentially adjacent are constructed, wherein each point cloud level includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in adjacent first point cloud level and second point cloud level, the number of point cloud groups in the second point cloud level is less than the number of point cloud groups in the first point cloud level, and each point cloud group in the second point cloud level is formed by merging a plurality of point cloud groups in the first point cloud level;

[0010] For each point cloud level, each point cloud group in the point cloud level is divided into voxels to obtain a plurality of voxels corresponding to each point cloud group, and according to the geometric characteristics of the feature points of the point cloud data in each voxel, the pose optimization is performed in the point cloud level to obtain the pose optimization result corresponding to the point cloud level;

[0011] Based on the pose optimization results corresponding to each point cloud level and the preset constraint factors, the pose of the multi-frame point cloud data is globally optimized to obtain optimized point cloud data, and coordinate transformation is performed according to the optimized point cloud data to obtain a global map.

[0012] In a second aspect, an embodiment of the present application provides a map construction device, the device comprising:

[0013] An acquisition module is used to acquire multi-frame point cloud data scanned and collected by a three-dimensional laser radar sensor;

[0014] A hierarchical construction module, configured to construct a plurality of point cloud hierarchies that are sequentially adjacent to each other according to the plurality of frames of point cloud data, wherein each point cloud hierarchy includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in adjacent first and second point cloud hierarchies, the number of point cloud groups in the second point cloud hierarchy is less than the number of point cloud groups in the first point cloud hierarchy, and each point cloud group in the second point cloud hierarchy is formed by merging a plurality of point cloud groups in the first point cloud hierarchy;

[0015] A posture optimization module is used to perform voxel segmentation on each point cloud group in each point cloud level to obtain a plurality of voxels corresponding to each point cloud group, and perform posture optimization in the point cloud level according to the geometric characteristics of the feature points of the point cloud data in each voxel to obtain a posture optimization result corresponding to the point cloud level;

[0016] The global optimization module is used to globally optimize the posture of the multi-frame point cloud data based on the posture optimization results corresponding to each point cloud level and the preset constraint factors to obtain the optimized point cloud data, and perform coordinate transformation according to the optimized point cloud data to obtain a global map.

[0017] In a third aspect, an embodiment of the present application provides an electronic device, comprising: a processor, a storage medium and a bus, wherein the storage medium stores machine-readable instructions executable by the processor, and when the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the machine-readable instructions to perform the steps of the map construction method as described in any one of the first aspects above.

[0018] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the map construction method as described in any one of the above-mentioned first aspects are executed.

[0019] According to the map construction method and map construction device of the embodiment of the present application, multiple adjacent point cloud levels are constructed based on multi-frame point cloud data scanned and collected by a three-dimensional laser radar sensor, and for each point cloud level, voxel segmentation is performed on each point cloud group in the point cloud level to obtain multiple voxels corresponding to each point cloud group, and posture optimization is performed in the point cloud level based on the geometric characteristics of the feature points of the point cloud data in each voxel to obtain a posture optimization result corresponding to the point cloud level, and the posture of the multi-frame point cloud data is globally optimized based on the posture optimization results corresponding to each point cloud level and preset constraint factors to obtain optimized point cloud data, and coordinate transformation is performed based on the optimized point cloud data to obtain a global map. According to the embodiment of the present application, each point cloud level includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in the adjacent first point cloud level and second point cloud level, the number of point cloud groups in the second point cloud level is less than the number of point cloud groups in the first point cloud level, and, each point cloud group in the second point cloud level is formed by merging multiple point cloud groups in the first point cloud level, thereby constructing a multi-level point cloud data structure, and in the high-level level, the point cloud data is fused into larger voxels or point cloud groups, and the amount of data processing is further reduced by voxel segmentation to improve the computational efficiency. Further, pose optimization is performed by extracting the geometric characteristics of feature points in the voxels, and pose optimization is performed at different levels, so as to achieve matching at a finer granularity and gradually refine the pose estimation from coarse to fine, thereby improving the accuracy of pose estimation. And on the basis of hierarchical optimization, a constraint factor is introduced for global optimization, and the pose of all point cloud data in the global coordinate system is ensured to be consistent in the global range, thereby improving the consistency of the global map. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0021] Figure 1 A schematic diagram of a process of a map construction method provided in an embodiment of the present application is shown;

[0022] Figure 2 A schematic diagram of a voxel segmentation method provided in an embodiment of the present application is shown;

[0023] Figure 3 A schematic diagram of a voxel splitting primitive provided by an embodiment of the present application is shown;

[0024] Figure 4 A schematic diagram of an octree data structure provided by an embodiment of the present application is shown;

[0025] Figure 5 A schematic diagram of a process for determining a posture optimization result provided in an embodiment of the present application is shown;

[0026] Figure 6 A schematic diagram of a flow chart of another method for determining a posture optimization result provided in an embodiment of the present application is shown;

[0027] Figure 7 A schematic diagram of a point-to-surface error measurement provided by an embodiment of the present application is shown;

[0028] Figure 8 A schematic diagram of a point-to-line error measurement provided by an embodiment of the present application is shown;

[0029] Fig. 9 A schematic diagram of a global optimization method provided in an embodiment of the present application is shown;

[0030] Fig.10 A schematic diagram of hierarchical local optimization taking into account spatial neighbors provided in an embodiment of the present application is shown;

[0031] Fig.11 A factor graph for optimizing a pose graph taking into account inter-layer constraints provided in an embodiment of the present application is shown;

[0032] Fig.12 A schematic diagram of a posture optimization process provided by an embodiment of the present application is shown;

[0033] Fig.13 A schematic diagram of the structure of a map construction device provided in an embodiment of the present application is shown;

[0034] Fig.14 A schematic structural diagram of an electronic device provided in an embodiment of the present application is shown. DETAILED DESCRIPTION

[0035] To make the purpose, technical scheme and advantages of the embodiments of the present application clearer, the technical scheme in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. It should be understood that the drawings in the present application only serve the purpose of explanation and description and are not used to limit the scope of protection of the present application. In addition, it should be understood that the schematic drawings are not drawn in real proportion. The flowchart used in this application shows the operations implemented according to some embodiments of the present application. It should be understood that the operations of the flowchart can be implemented out of sequence, and the steps without logical context can be reversed in order or implemented simultaneously. In addition, those skilled in the art can add one or more other operations to the flowchart under the guidance of the content of the present application, or remove one or more operations from the flowchart.

[0036] In addition, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application described and shown in the drawings here can be arranged and designed in various configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application claimed for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present application.

[0037] In order to enable those skilled in the art to use the content of this application, the following implementation is given in combination with the specific application scenario "A method for improving the accuracy and global consistency of laser SLAM mobile mapping". For those skilled in the art, the general principles defined here can be applied to other embodiments and application scenarios without departing from the spirit and scope of this application. Although this application is mainly described around the map construction method, it should be understood that this is only an exemplary embodiment.

[0038] It should be noted that the term "comprising" will be used in the embodiments of the present application to indicate the existence of the features declared thereafter, but does not exclude the addition of other features.

[0039] The embodiment of the present application provides a method for improving the accuracy and global consistency of laser SLAM mobile mapping, which eliminates the pose error by adopting bottom-up hierarchical local BA and top-down global optimization, thereby improving the accuracy, efficiency and global consistency of optimization. This method can effectively improve the accuracy and global consistency of mobile mapping of integrated long-range three-dimensional laser radar sensors (Light Detection And Ranging, LiDAR), such as HESAI-XT16, or short-range LiDAR (such as Livox MID-360).

[0040] Figure 1 A flowchart of a map construction method provided in an embodiment of the present application is shown. Figure 1 As shown, the method specifically comprises the following steps:

[0041] S101, obtaining multi-frame point cloud data scanned and collected by a three-dimensional laser radar sensor.

[0042] Optionally, the three-dimensional laser radar sensor LiDAR is a technology that uses laser pulses to measure distance. By emitting laser pulses and receiving reflected signals, the LiDAR sensor can measure the distance, angle and other information of an object. Specifically, point cloud data is a collection of a series of three-dimensional points collected by the LiDAR sensor, each of which contains three-dimensional coordinates and other possible information.

[0043] Optionally, multi-frame point cloud data refers to multiple point cloud data sets collected by the LiDAR sensor at different time points or different positions, and each point cloud data set represents the state of the environment observed by the LiDAR sensor at a point in time. The collection of multi-frame point cloud data usually involves continuous scanning of the LiDAR sensor. As the LiDAR sensor moves or rotates, the LiDAR sensor captures different parts of the environment at different time points, thereby generating a series of point cloud data.

[0044] S102: construct a plurality of point cloud levels that are adjacent to each other in sequence according to the multi-frame point cloud data.

[0045] Optionally, each point cloud level includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in adjacent first point cloud levels and second point cloud levels, the number of point cloud groups in the second point cloud level is less than the number of point cloud groups in the first point cloud level, and each point cloud group in the second point cloud level is formed by merging multiple point cloud groups in the first point cloud level.

[0046] Optionally, the point cloud level refers to dividing the multi-frame point cloud data into multiple levels according to certain rules, and in each point cloud level, the multi-frame point cloud data contained in the point cloud level can be divided into several point cloud groups, and each point cloud group includes at least one frame of point cloud data. It should be noted that the multiple point cloud levels constructed are adjacent to each other in sequence, which means that each level is processed based on the point cloud data of the previous level, and in the adjacent first point cloud level and second point cloud level, the change in the number of point cloud groups is that the number of point cloud groups in the second point cloud level is less than the number of point cloud groups in the first point cloud level, and the point cloud groups in the second point cloud level are obtained by fusion of the point cloud groups in the first point cloud level, that is, the point cloud groups in the second point cloud level are fused by multiple point cloud groups in the first point cloud level.

[0047] For example, 1000 frames of point cloud data are collected by a three-dimensional laser radar sensor LiDAR. In the first point cloud level, one frame of point cloud data is taken as one point cloud group, and the collected 1000 frames of point cloud data are divided into 1000 point cloud groups, that is, each point cloud group includes one frame of point cloud data to construct the first point cloud level. In the second point cloud level, multiple point cloud groups in the first point cloud level are fused into a smaller number of point cloud groups. The fusion can adopt a simple averaging or weighted averaging fusion method. For example, the first point cloud level includes 1000 point cloud groups, and 1000 point cloud groups can be fused into 50 point cloud groups based on 20 frames of point cloud data as a fusion unit, that is, each point cloud group includes 20 frames of point cloud data to construct the second point cloud level. In this way, more point cloud levels can be constructed. With each additional point cloud level, the number of point cloud groups continues to decrease, while the number of point clouds in the point cloud group increases.

[0048] It should be noted that multiple point cloud levels that are adjacent to each other are constructed based on multi-frame point cloud data. The multi-level structure enables data analysis at different data granularities and scales, effectively improving the processing efficiency of point cloud data while reducing the amount of point cloud data. In addition, fusion processing is performed between different point cloud levels. Through fusion processing, data downsampling can be achieved to reduce redundant information. Since processing at different scales can reduce the impact of noise, the robustness of data processing can be improved based on the multi-level structure.

[0049] S103. For each point cloud level, each point cloud group in the point cloud level is divided into voxels to obtain multiple voxels corresponding to each point cloud group, and according to the geometric characteristics of the feature points of the point cloud data in each voxel, pose optimization is performed in the point cloud level to obtain a pose optimization result corresponding to the point cloud level.

[0050] Optionally, voxels are also called volume pixels. Voxel segmentation is to segment the point cloud data into multiple small three-dimensional voxels, each of which contains point cloud data within a certain range. The purpose of voxel segmentation is to segment the point cloud data into smaller areas and ensure that the feature points in each voxel belong to the plane with the same normal vector. Specifically, taking plane feature points as an example, a voxel contains feature points of multiple planes. For example, a voxel includes 10 feature points, and these 10 feature points come from 5 different data frames. In this case, voxel segmentation is required, and the choice of voxel size can be determined according to the specific application scenario.

[0051] Optionally, in the point cloud data, the feature point is a point with significant geometric features, such as an edge point, a plane point, etc. Correspondingly, the feature point geometric property refers to the feature point being a plane geometric property or an edge geometric property, etc. According to the geometric properties of the feature points of the point cloud data in the voxel, pose optimization is performed within the point cloud level. The goal of pose optimization is to adjust the position and pose of the point cloud so that the matching between adjacent point clouds is more accurate, and the optimal pose of each point cloud group is obtained through pose optimization, and the optimized pose is applied to the point cloud data to obtain the optimized point cloud data.

[0052] Optionally, based on the constructed multi-level structure, hierarchical local optimization is performed for each point cloud level, and each point cloud group in the point cloud level is divided into voxels. Through voxel division and feature point extraction, pose optimization is performed at a finer granularity and at different scales, thereby improving the accuracy of point cloud matching. Moreover, voxel division not only reduces the amount of point cloud data to be processed, but also reduces the computational complexity, thereby achieving more comprehensive optimization results.

[0053] S104, globally optimizing the pose of the multi-frame point cloud data based on the pose optimization results corresponding to each point cloud level and the preset constraint factors to obtain optimized point cloud data, and performing coordinate transformation according to the optimized point cloud data to obtain a global map.

[0054] Optionally, the pose optimization result refers to the optimal pose of each point cloud group obtained after pose optimization by voxel segmentation and feature point geometric characteristics in each point cloud level. The preset constraint factors include the constructed relative pose factors and the directly accessible closed-loop factors and position constraint factors, where the closed-loop factors are used to represent the closed-loop relationship found in the global map, help correct the accumulated error, and ensure global consistency, and the position constraint factors are used to represent the known absolute pose information, forcing certain poses to remain unchanged.

[0055] Optionally, global optimization refers to optimizing the pose of multi-frame point cloud data on a global scale to eliminate the accumulated errors in local optimization and ensure global consistency. Optimized point cloud data refers to point cloud data that has been globally optimized and has obtained the best pose estimation. On this basis, the optimized point cloud data is transformed into a global coordinate system so that all point cloud data are integrated in a unified coordinate system to form a global map, which contains the three-dimensional information of the entire scanning environment.

[0056] Based on this, according to the map construction method provided by the embodiment of the present application, each point cloud level includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in the adjacent first point cloud level and second point cloud level, the number of point cloud groups in the second point cloud level is less than the number of point cloud groups in the first point cloud level, and, each point cloud group in the second point cloud level is formed by merging multiple point cloud groups in the first point cloud level, thereby constructing a multi-level point cloud data structure, in a high-level level, the point cloud data is fused into larger voxels or point cloud groups, and the amount of data processing is further reduced by voxel segmentation to improve computational efficiency. Further, pose optimization is performed by extracting the geometric characteristics of feature points in the voxels, and pose optimization is performed at different levels, so as to achieve matching at a finer granularity and gradually refine pose estimation from coarse to fine, thereby improving the accuracy of pose estimation. And on the basis of hierarchical optimization, a constraint factor is introduced for global optimization to ensure that the pose of all point cloud data in the global coordinate system is consistent within the global scope, thereby improving the consistency of the global map.

[0057] Figure 2 FIG. 1 is a flow chart of a voxel segmentation method provided in an embodiment of the present application. As a possible implementation method, refer to Figure 2 As shown, the above step S103 performs voxel segmentation on each point cloud group in the point cloud level to obtain a plurality of voxels corresponding to each point cloud group, which specifically includes the following steps:

[0058] S201 . For each point cloud group, perform voxel segmentation on the point cloud group according to a preset voxel size to obtain a plurality of initial voxels corresponding to the point cloud group.

[0059] Optionally, each point cloud group includes at least one frame of point cloud data, and the at least one frame of point cloud data corresponds to a local scene in the complete scan scene, that is, each point cloud group corresponds to a local scene, so the point cloud group is voxel-segmented, that is, the local scene corresponding to the point cloud group is separated by using multiple voxels.

[0060] For example, the preset voxel size can be determined according to the application scenario and requirements, taking into account factors such as data volume, computing efficiency and accuracy. Figure 3As shown, the preset voxel size can be set to 1 meter * 1 meter * 1 meter. The point cloud data in each point cloud group are divided according to the preset voxel size to obtain multiple initial voxels corresponding to each point cloud group.

[0061] S202 . Determine a covariance matrix of the point cloud data in the first voxel according to the point cloud center, the number of point clouds, and the vector coordinates of the point cloud data in the first voxel.

[0062] Optionally, the first voxel is any one of a plurality of initial voxels. For each first voxel, whether the first voxel is further split needs to be determined by judging whether the eigenvalue of the covariance matrix of the point cloud data in the first voxel meets the requirement of the feature type.

[0063] For example, the covariance matrix of the point cloud data within a voxel can be obtained by the following formula (1):

[0064] (1)

[0065] Where A represents the covariance matrix of the point cloud data within the voxel, Represents the vector coordinates of the point cloud data within the voxel, N represents the number of point clouds within the voxel, represents the center of the point cloud within the voxel, and , Represents the transpose operator.

[0066] Exemplarily, the above formula (1) is used to determine the covariance matrix of the point cloud data in each first voxel, and then according to the eigenvalue of the covariance matrix of the point cloud data in each first voxel, it is determined whether to continue to divide the first voxel.

[0067] S203, determining whether to segment the first voxel according to the eigenvalue of the covariance matrix of the point cloud data in the first voxel, and determining whether the first voxel satisfies a preset stop segmentation condition; if the first voxel does not satisfy the preset stop segmentation condition, segmenting the first voxel again until the second voxel after segmentation satisfies the preset stop segmentation condition, so as to segment the point cloud group into multiple voxels.

[0068] Optionally, the covariance matrix is ​​a statistic used to describe the distribution of point cloud data in three-dimensional space, and the eigenvalues ​​of the covariance matrix reflect the degree of dispersion of the data in different directions. The larger the eigenvalue, the higher the degree of dispersion in the corresponding direction. For each first voxel, on the basis of obtaining the covariance matrix of the point cloud data in the first voxel, the eigenvalue of the covariance matrix of the point cloud data in the first voxel is further calculated to dynamically determine whether to continue to segment the first voxel based on the distribution of the point cloud data reflected by the eigenvalue.

[0069] Optionally, the above step S203 determines whether to segment the first voxel according to the eigenvalue of the covariance matrix of the point cloud data in the first voxel, including:

[0070] According to the eigenvalue of the covariance matrix of the point cloud data in the first voxel, the characteristic ratio corresponding to the first voxel is determined; according to the characteristic ratio corresponding to the first voxel, it is determined whether the first voxel meets the preset segmentation condition, if the first voxel meets the preset segmentation condition, the first voxel is segmented, otherwise, the first voxel is not segmented.

[0071] Exemplarily, the feature ratio is used to evaluate the distribution characteristics of point cloud data. The preset segmentation condition is whether the feature ratio corresponding to the voxel is less than the empirical threshold. If the feature ratio corresponding to the voxel is less than the empirical threshold, the segmentation will not continue. If the feature ratio corresponding to the voxel is greater than or equal to the empirical threshold, the voxel will continue to be segmented.

[0072] For example, for the first voxel containing the face feature point, the empirical threshold Can be set to 9, if the feature ratio , then the first voxel is not split, otherwise, the first voxel can be split further. For the first voxel containing edge feature points, the empirical threshold Can be set to 4 if the feature ratio , then the first voxel is not split, otherwise, the first voxel can continue to be split. represents the eigenvalue of the covariance matrix of the point cloud data within the voxel, where ( ) are the minimum, second minimum, and maximum eigenvalues ​​of the covariance matrix, respectively.

[0073] Optionally, determining whether the first voxel satisfies a preset stop segmentation condition includes: if the first voxel satisfies the preset segmentation condition, segmenting the first voxel into a preset number of second voxels, determining a characteristic ratio corresponding to each second voxel based on the eigenvalue of the covariance matrix of the point cloud data in each second voxel, and determining whether the first voxel satisfies the preset stop segmentation condition based on the characteristic ratio corresponding to each second voxel and the voxel size of each second voxel.

[0074] Exemplarily, the preset stop segmentation condition is whether the voxel meets the requirements of the characteristic ratio and the voxel size. If the first voxel needs to be further segmented, the first voxel is segmented into a preset number of second voxels, which may be eight. For each second voxel obtained by segmentation, the covariance matrix of the point cloud data in each second voxel and the eigenvalue of the covariance matrix of the point cloud data in each second voxel are calculated, and then the characteristic ratio is calculated according to the eigenvalue of the covariance matrix of the point cloud data in the second voxel. The characteristic ratio and the voxel size of the second voxel are used to determine whether the second voxel meets the preset stop segmentation condition.

[0075] Exemplarily, the segmentation of the second voxel refers to the segmentation of the first voxel, and the second voxel containing the surface feature point and the voxel containing the edge feature point also need to be classified and processed. Specifically, if the feature ratio of the second voxel is less than the empirical threshold, and the voxel size meets the voxel size requirement, it is determined that the second voxel meets the preset stop segmentation condition, and the second voxel is stopped from being segmented again. On the contrary, if the feature ratio of the second voxel is greater than or equal to the empirical threshold, or the voxel size does not meet the voxel size requirement, it is determined that the second voxel does not meet the preset stop segmentation condition, and the second voxel needs to continue to be voxel segmented until all voxels meet the preset stop segmentation condition to end the voxel segmentation.

[0076] Based on this, by calculating the feature ratio to dynamically decide whether to continue to split the voxels, point cloud data with different distribution characteristics can be processed adaptively. Further, by guiding the splitting through the feature ratio and voxel size requirements, the details of the point cloud data can be better preserved and the accuracy of subsequent processing can be improved. And through multiple voxel splitting until each voxel meets the preset stop splitting condition, this process ensures that the distribution characteristics of the point cloud data within the voxel meet certain standards and can also reduce the impact of noise and outliers.

[0077] As a possible implementation manner, after obtaining a plurality of voxels corresponding to each point cloud group, the method further includes:

[0078] Based on the preset hash mapping strategy, a key value corresponding to each voxel is generated, the point cloud data in each voxel is stored in a hash table according to the preset data structure, and the key value corresponding to each voxel is used as the index of each voxel, so as to query the point cloud data in each voxel from the hash table through the index of each voxel.

[0079] Optionally, the preset hash mapping strategy refers to a predefined hash function and related parameters, which are used to generate a voxel key value, and the key value is used to uniquely identify a voxel. Figure 4 As shown, the preset data structure is an octree data structure. In order to accelerate the processing of adaptive voxelization, the point cloud data in each voxel can be stored using the octree data structure, and a hash map is used to generate a displacement key value for the index of each voxel, and the key value is stored in the hash table. Then, indexing in the hash table according to the key value can significantly improve the query efficiency of the corresponding voxel.

[0080] Exemplarily, hash mapping is a method of mapping data elements to a storage space of limited size (such as a hash table). According to the location information of the voxel, the corresponding key value is calculated by a hash function. Usually, the key value can be calculated based on the coordinates of the center point of the voxel. The point cloud data in each voxel is stored in the hash table according to the octree data structure, and the key value corresponding to each voxel is used as the index of each voxel, and then the point cloud data in the corresponding voxel can be quickly queried from the hash table through the index (key value) of the voxel.

[0081] Based on this, since the correlation of corresponding features between LiDAR frames is the basis for calculating the local BA optimization cost, compared with the matching method of points of the same name in visual SLAM, the extraction of corresponding points that relies on the similarity of LiDAR points is not feasible due to the lack of reliable measurement criteria to filter corresponding points between LiDAR frames. However, the embodiment of the present application voxelizes the three-dimensional space scanned by LiDAR and determines whether the point cloud in the voxel satisfies the geometric properties of the edge or plane. If so, the LiDAR point that falls in the voxel corresponds to the geometric feature, and these LiDAR points from different LiDAR frames have a common view constraint.

[0082] In addition, the embodiments of the present application realize the association of inter-frame corresponding features based on adaptive voxelization through voxel segmentation and key value indexing, which has the following advantages: (1) voxels that are adaptive to changes in scene structure (such as changes in the size of local planes) can be associated with more inter-frame corresponding features, thereby forming sufficient constraints; (2) Compared with the nearest neighbor search method (such as kd-tree), adaptive voxelization based on octree storage and hash table retrieval is more efficient; (3) When adaptive voxelization associates inter-frame features, the covariance and its eigenvalues ​​are calculated, which is also a step in constructing the local BA optimization cost function. This ingenious design compresses the processing flow and can reduce the time complexity of the algorithm.

[0083] Figure 5 A schematic diagram of a method for determining a posture optimization result provided in an embodiment of the present application is shown. As a possible implementation method, refer to Figure 5 As shown, the above step S103 performs pose optimization in the point cloud level according to the geometric characteristics of the feature points of the point cloud data in each voxel to obtain the pose optimization result corresponding to the point cloud level, which specifically includes the following steps:

[0084] S501 , extracting feature points in each voxel at the point cloud level to obtain plane feature points and edge feature points corresponding to each voxel.

[0085] Optionally, feature points are points in point cloud data with significant geometric characteristics, such as plane feature points and edge feature points. Plane feature points refer to feature points located in a plane area and are usually used to describe the characteristics of a plane. Edge feature points refer to feature points located in an edge area and are usually used to describe the characteristics of an edge.

[0086] Exemplarily, feature points are extracted for each voxel at each point cloud level, the point cloud data in each voxel is filtered to remove noise points, and plane feature points and edge feature points are detected in each voxel. Specifically, for plane feature point detection, a plane fitting method can be used to detect a plane area and extract feature points therefrom. For edge feature point detection, an edge detection method can be used to detect an edge area and extract feature points therefrom.

[0087] S502, performing pose optimization corresponding to the point cloud level according to the pose of the plane feature points and the pose of the edge feature points corresponding to each voxel, and obtaining a pose optimization result corresponding to the point cloud level.

[0088] Optionally, the pose of each voxel is optimized according to the pose of the plane feature points and the pose of the edge feature points corresponding to each voxel to obtain the pose optimization result of each voxel; the pose optimization result corresponding to the point cloud level is determined according to the pose optimization result of each voxel and the pose optimization result of the previous level of the point cloud level.

[0089] Exemplarily, the pose of each voxel is optimized according to the pose of the plane feature points and the pose of the edge feature points corresponding to each voxel. The purpose of the pose optimization is to adjust the position and pose of the point cloud data so that the point cloud data in the voxel can be better matched. The position and pose of each voxel are adjusted based on the observed values ​​of the feature points in each voxel so that all observed feature points can better match the actual scene. Furthermore, after the pose of each voxel is optimized separately, each voxel will have an optimized pose result, and the pose optimization result of each point cloud level is determined based on the pose optimization results of each voxel in the point cloud level and the optimization results of the previous level, so that the pose optimization is refined layer by layer, which significantly improves the pose optimization accuracy.

[0090] Figure 6 FIG. 1 is a flow chart of another method for determining a posture optimization result provided by an embodiment of the present application. As a possible implementation method, refer to Figure 6 As shown, the above steps optimize the posture of each voxel according to the posture of the plane feature point corresponding to each voxel and the posture of the edge feature point to obtain the posture optimization result of each voxel, which specifically includes the following steps:

[0091] S601. Optimize the position and posture of the plane feature point according to the distance from the plane feature point to the plane corresponding to the plane feature point, and obtain the position and posture optimization result of the plane feature point.

[0092] Exemplarily, taking plane features as an example, when the pose of the LiDAR frame reaches the optimal state, the plane formed by the plane feature points of multiple LiDAR frames has the best state, that is, the local plane feature point cloud is more in line with the plane geometric characteristics, so for voxels containing plane feature points, the pose of the plane feature points can be optimized by minimizing the distance from the plane feature points in the voxel to the plane corresponding to the plane feature points, specifically, the sum of the squares of the distances, that is, the pose of the data frame associated with the voxel is optimized.

[0093] For example, refer to Figure 7 The schematic diagram of the point-to-surface error measurement is shown in Figure 1. Figure 7 The point-to-surface error metric shown in Based on the following formula (2), by minimizing the distance from point to surface The sum of squares to optimize the pose:

[0094] (2)

[0095] in, T * represents the optimized pose, n * represents the best normal vector after face element optimization, q * represents the best surface feature point in the voxel, argmin represents the minimum value operator, and N represents the number of point clouds in the voxel. T represents the pose to be optimized, The normal vector representing the plane feature can be obtained by finding the eigenvector of the covariance matrix during voxelization. Represents the vector coordinates of the point cloud data within the voxel, represents the best candidate feature point in a voxel, A represents the covariance matrix of the point cloud data in the voxel, Represents the covariance matrix No. The largest eigenvalue, k represents the largest eigenvalue (for example, the value can be 3 in the above formula (2)). represents the corresponding eigenvector, represents the center of the point cloud within the voxel, and , Represents the transpose operator.

[0096] S602 , optimizing the position and posture of the edge feature point according to the distance from the edge feature point to the edge line corresponding to the edge feature point, and obtaining a position and posture optimization result of the edge feature point.

[0097] Similarly, for voxels containing edge feature points, the position and posture of the edge feature points can be optimized by minimizing the sum of the squares of the distances from the points in the voxels to the lines, that is, the position and posture of the data frame associated with the voxels can be optimized.

[0098] For example, refer to Figure 8 The point-to-line error metric diagram is shown in Figure 1. Figure 8 The point-to-line error metric shown in The pose can be optimized by minimizing the sum of the squares of the distances from the point to the line based on the following formula (3):

[0099] (3)

[0100] in, T * represents the optimized pose, represents the best direction vector after line feature optimization, represents the best edge feature point within the voxel, T represents the pose to be optimized, argmin represents the minimum value operator, and N represents the number of point clouds in the voxel. represents the direction vector of the edge feature within the voxel, Represents the covariance matrix , and , Represents the vector coordinates of the point cloud data within the voxel, represents the best candidate feature point in a voxel, A represents the covariance matrix of the point cloud data in the voxel, represents the minimization of the covariance matrix, Represents the covariance matrix No. The largest eigenvalue, Indicates the largest number (for example, the possible values ​​in formula (3) above are 1, 2, and 3), represents the corresponding eigenvector, represents the center of the point cloud within the voxel, and , Represents the transpose operator.

[0101] It should be noted that the best of the above formulas (2) and (3) is is not unique, because the point can move arbitrarily in the plane (or along the edge). However, this non-uniqueness does not affect the cost function's effect on the pose. Therefore, the cost of local BA optimization of LiDAR frame pose can be simplified to minimizing the eigenvalue of the point cloud covariance matrix within the voxel, as shown in the following formula (4):

[0102] (4)

[0103] in, represents the optimized posture, min represents the minimum value operator, represents the lth smallest eigenvalue of the covariance matrix, p represents the vector of all feature points in the voxel corresponding to the same feature, and In the specific processing, for plane and edge features, the optimization objectives are to minimize the covariance matrix The smallest and second smallest eigenvalues ​​of .

[0104] S603, obtaining the pose optimization result of the voxel according to the pose optimization result of each plane feature point corresponding to the voxel and the pose optimization result of each edge feature point.

[0105] For example, the above formula (2) and formula (3) essentially define an error function for measuring the consistency of the optimized feature point poses. By minimizing the above formula (2) and formula (3), the optimized poses of all feature points can better match the actual observation data. On this basis, according to the pose information of the optimized plane feature points and edge feature points, the position and pose of a single voxel are calculated or updated by combining this information, which means that the final pose of each voxel is based on the best estimate of all the feature points inside it.

[0106] Based on this, the local BA optimization in the embodiment of the present application is achieved by minimizing the distance from the feature point to the corresponding plane or edge and optimizing the pose of the multi-frame point cloud data. By using the information of different types of feature points (planes and edges) inside the voxel to improve the estimation of the voxel pose, this method can improve the accuracy of three-dimensional reconstruction, especially when dealing with complex scenes.

[0107] As a possible implementation method, before the above-mentioned step S104 globally optimizes the posture of the multi-frame point cloud data based on the posture optimization results corresponding to each point cloud level and the preset constraint factors to obtain the optimized point cloud data, the method also includes: performing displacement constraint processing on the degraded data frames contained in the multi-frame point cloud data based on preset displacement consistency constraint parameters.

[0108] Optionally, the degraded data frame is a data frame in which the point cloud data is insufficiently constrained in a preset direction, and the preset displacement consistency constraint parameter is used to indicate the change in relative displacement between consecutive frame point cloud data before and after optimization. In actual mobile mapping tasks, degraded scanning frames may appear during LiDAR scanning. When degraded LiDAR frames are used for local BA optimization, pathological optimization results may be output due to insufficient constraints in the degraded direction, which is reflected in the trajectory as burrs and reduced smoothness. More importantly, it will also have an adverse effect on the mapping accuracy.

[0109] For example, in order to suppress the adverse effects of degraded LiDAR frames on BA optimization, the present application adds the displacement consistency constraint shown in the following formula (5) on the basis of the above formula (4): (5)

[0110] Among them, SE (3) represents the Euclidean transformation, min represents the minimum value operator, represents the displacement consistency constraint, specifically refers to the change in relative displacement between consecutive frames before and after optimization, and , Represents the weighted matrix for adjusting the strength of the constraints in the three displacement directions. Considering that it is mainly used to constrain the horizontal direction, the weighted matrix can be set to an empirical value , Indicates the number of features (planes or edges) involved in this optimization. Indicates the number of poses that need to be optimized in the sliding window, The value of is determined by the feature type, that is, the goal of plane feature optimization is , the goal of edge feature optimization is (The eigenvalues ​​of the covariance matrix are sorted in ascending order by default), Indicates the current optimization i A set of feature points, Represents the transpose operator.

[0111] For example, the point set in the above formula (5) is It can be calculated by the following formula (6):

[0112] (6)

[0113] in, Indicates the current optimization i A set of feature points, represents the number of LiDAR points that constitute the feature, k represents the index of the LiDAR point, Indicates belonging to id for j The first LiDAR frame k LiDAR feature points, Represents the transpose operator.

[0114] Exemplarily, when processing multiple frames of point cloud data, displacement consistency constraint parameters are applied to degenerate data frames so that the degenerate data frames meet certain displacement consistency requirements in a preset direction. Specifically, degenerate data frames with insufficient constraints in a preset direction can be eliminated or marked by data screening, and the position and posture of the degenerate data frames can be corrected in combination with the posture information of multiple other data frames. Preset displacement consistency constraint parameters are added during the optimization process to ensure that the relative displacement consistency between consecutive frames can be maintained even when processing degenerate data frames.

[0115] Based on this, applying preset displacement consistency constraint parameters to multi-frame point cloud data containing degraded data frames can effectively alleviate the adverse effects of degradation on local BA optimization. Through this displacement constraint processing, it can be ensured that even in the case of poor quality of some data frames, the overall 3D mapping results can still maintain high accuracy and reliability.

[0116] Fig. 9 A schematic diagram of a global optimization method provided in an embodiment of the present application is shown. As a possible implementation method, refer to Fig. 9 As shown, the above step S104 performs global optimization on the pose of the multi-frame point cloud data based on the pose optimization results corresponding to each point cloud level and the preset constraint factors to obtain the optimized point cloud data, which specifically includes the following steps:

[0117] S901. Generate a relative pose factor corresponding to each point cloud level according to the pose optimization result corresponding to each point cloud level.

[0118] Optionally, point cloud data is usually divided into different levels, each of which may have different resolutions or levels of detail, and the pose optimization result of each point cloud level refers to the best estimate of the point cloud position and pose obtained at that point cloud level. The relative pose factor refers to the relative position and pose relationship between two adjacent point cloud levels or between different point clouds in the same level. The relative pose factor reflects how the point clouds should be moved and rotated relative to each other to better match each other.

[0119] For example, refer to Fig.10 As shown in Figure 2, the execution details of pose graph optimization taking into account inter-layer constraints are: from the top layer (usually the third layer) to the first layer, the relative pose factor (i.e., odometer factor) between consecutive frames is constructed, and the first layer is used as the Layer Pose Node and For example, the error of the relative pose factor and the cost function It can be shown as the following formula (7):

[0120] (7)

[0121] in, represents the error of the relative pose factor, represents the cost function, and represents the pose node of the i-th layer, and represents the pose to be optimized, log represents the logarithmic operator, represents the covariance matrix, which can be obtained by inverting the Hessian matrix of the local BA output.

[0122] S902: construct an overall cost function based on the relative pose factor, closed-loop factor and position constraint factor corresponding to each point cloud level.

[0123] Optionally, the closed-loop factor refers to the correction of accumulated errors by identifying the same areas or features that have been processed previously when processing multi-frame point cloud data. Closed-loop detection can help reduce drift and improve global consistency. The position constraint factor refers to the restriction on the position of the point cloud during the optimization process, which is used to help limit the position range or direction of the point cloud. The overall cost function is used to indicate the error of the pose estimation of multi-frame point cloud data. The overall cost function is a mathematical expression that combines the above three factors to measure the quality of the current point cloud data configuration. It should be noted that the design goal of the cost function is to satisfy all factors at the same time as much as possible, so as to obtain an optimal point cloud configuration. Usually, the cost function may be in the form of a weighted sum of various error terms.

[0124] Exemplarily, based on the above formula (7), the above construction process is repeated to the first layer, and the overall cost function at the first layer can be obtained. The overall cost function can be shown in the following formula (8):

[0125] (8)

[0126] Where F represents the set of LiDAR frames, X represents the set of pose nodes, L represents the set of layers in hierarchical BA, i represents the level, and j represents the index number of the LiDAR in the current layer. represents the cost function of the first layer, Indicates i The collection of LiDAR frames in the layer, Indicates the stride between local windows.

[0127] For example, the correspondence of LiDAR frame projections between layers can give the cost function falling on the first layer: The specific form of is shown in the following formula (9):

[0128] (9)

[0129] in, represents the cost function of the first layer, log represents the logarithmic operator, represents the covariance matrix, Represents the error of the relative pose factor.

[0130] For example, based on the above processing and combined with Fig.10 As shown in , the principle of inter-layer constraints can be clearly seen, that is, the relative posture constraints of the high-level layers are projected to the first layer to provide cross-window prior constraints for the optimization of the first layer, but this constraint is a local constraint. At this time, the composition of the factor graph is as follows Fig.11 As shown, set the number of layers , stride , local window size .

[0131] S903, iteratively optimize the overall cost function until the iterative optimization is stopped when the iterative stop condition is met, and the optimized point cloud data is obtained.

[0132] Optionally, the iteration stop condition is that the sum of the errors of the relative pose factor, the closed-loop factor, and the position constraint factor is less than a preset threshold. Iterative optimization refers to gradually reducing the value of the overall cost function by adjusting the position and pose of the point cloud multiple times. In each iteration, an attempt is made to find a new point cloud configuration that reduces the value of the above overall cost function, and the process is repeated until a specific iteration stop condition is met.

[0133] Exemplarily, the iteration stop condition defines when to end the optimization process. In the present application, the iteration stop condition is that the sum of the errors of the relative pose factor, the closed-loop factor, and the position constraint factor is less than a preset threshold. That is, when the optimization process has reduced the sum of the errors of the above factors to a preset acceptable level, the iteration can be stopped. When the iteration stop condition is reached, what is obtained is the optimized point cloud data. After multiple iterations, the optimized point cloud data has reached a relatively ideal state, that is, the errors of all factors are controlled within a reasonable range, the relative position and posture between the point clouds are more accurate, and the consistency and accuracy of the overall data are improved.

[0134] Based on this, by constructing and optimizing an overall cost function that integrates multiple constraint factors to achieve accurate registration and optimization of point cloud data, the accuracy and robustness of 3D reconstruction or point cloud registration can be significantly improved.

[0135] Fig.12 A schematic diagram of a posture optimization process provided by an embodiment of the present application is shown. Fig.12As shown, the method for improving the global accuracy and consistency of laser SLAM mobile mapping in the present application is to eliminate the pose error. The specific processing includes the correlation of corresponding features between frames based on adaptive voxelization, hierarchical local BA taking into account the displacement consistency constraint, and global optimization taking into account the inter-layer and closed-loop constraint factors. And by adding the displacement consistency constraint to the cost function of the existing local BA suitable for LiDAR observation, the robustness of scanning for degraded situations is enhanced. Then, by implementing the local BA in a hierarchical and intra-layer concurrent execution manner, the optimization efficiency and global consistency are significantly improved, and the global adjustment optimization taking into account the inter-layer constraint factors is adopted to improve the scale consistency of the mobile mapping results. Compared with the existing methods, the method proposed in the present application has a significant effect on improving the accuracy and scale consistency of large-scale mobile mapping and has higher execution efficiency.

[0136] Based on the same inventive concept, a map construction device corresponding to the map construction method is also provided in the embodiment of the present application. Since the principle of solving the problem by the map construction device in the embodiment of the present application is similar to that of the above-mentioned map construction method in the embodiment of the present application, the implementation of the map construction device can refer to the implementation of the map construction method, and the repeated parts will not be repeated.

[0137] Reference Fig.13 , which is a schematic diagram of the structure of a map construction device provided in an embodiment of the present application, the map construction device 1300 includes: an acquisition module 1301, a hierarchical construction module 1302, a posture optimization module 1303 and a global optimization module 1304, wherein:

[0138] An acquisition module 1301 is used to acquire multi-frame point cloud data scanned and collected by a three-dimensional laser radar sensor;

[0139] The hierarchical construction module 1302 is used to construct a plurality of point cloud hierarchies that are sequentially adjacent to each other according to the plurality of frames of point cloud data, wherein each point cloud hierarchy includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in the adjacent first point cloud hierarchy and second point cloud hierarchy, the number of point cloud groups in the second point cloud hierarchy is less than the number of point cloud groups in the first point cloud hierarchy, and each point cloud group in the second point cloud hierarchy is formed by merging a plurality of point cloud groups in the first point cloud hierarchy;

[0140] The posture optimization module 1303 is used to perform voxel segmentation on each point cloud group in each point cloud level to obtain a plurality of voxels corresponding to each point cloud group, and perform posture optimization in the point cloud level according to the geometric characteristics of the feature points of the point cloud data in each voxel to obtain a posture optimization result corresponding to the point cloud level;

[0141] The global optimization module 1304 is used to globally optimize the posture of multi-frame point cloud data based on the posture optimization results corresponding to each point cloud level and the preset constraint factors to obtain optimized point cloud data, and perform coordinate transformation based on the optimized point cloud data to obtain a global map.

[0142] Based on this, according to the map construction device of the embodiment of the present application, each point cloud level includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in the adjacent first point cloud level and second point cloud level, the number of point cloud groups in the second point cloud level is less than the number of point cloud groups in the first point cloud level, and, each point cloud group in the second point cloud level is formed by merging multiple point cloud groups in the first point cloud level, so as to construct a multi-level point cloud data structure, in the high-level level, the point cloud data is fused into larger voxels or point cloud groups, and the data processing amount is further reduced by voxel segmentation to improve the calculation efficiency. Further, the pose optimization is performed by extracting the geometric characteristics of the feature points in the voxels, and the pose optimization is performed at different levels, so as to achieve matching at a finer granularity and gradually refine the pose estimation from coarse to fine, thereby improving the accuracy of the pose estimation. And on the basis of hierarchical optimization, a constraint factor is introduced for global optimization, and the pose consistency of all point cloud data in the global coordinate system is ensured in the global range, thereby improving the consistency of the global map.

[0143] In a possible implementation, the posture optimization module 1303 is specifically used to:

[0144] Feature points are extracted in each voxel at the point cloud level to obtain the plane feature points and edge feature points corresponding to each voxel;

[0145] According to the pose of the plane feature points corresponding to each voxel and the pose of the edge feature points, the pose optimization corresponding to the point cloud level is performed to obtain the pose optimization result corresponding to the point cloud level.

[0146] In a possible implementation, the posture optimization module 1303 is specifically used to:

[0147] According to the pose of the plane feature point corresponding to each voxel and the pose of the edge feature point, the pose of each voxel is optimized to obtain the pose optimization result of each voxel;

[0148] According to the pose optimization result of each voxel and the pose optimization result of the previous level of the point cloud level, the pose optimization result corresponding to the point cloud level is determined.

[0149] In a possible implementation, the posture optimization module 1303 is specifically used to:

[0150] According to the distance between the plane feature point and the plane corresponding to the plane feature point, the pose of the plane feature point is optimized to obtain the pose optimization result of the plane feature point;

[0151] According to the distance between the edge feature point and the edge line corresponding to the edge feature point, the position and posture of the edge feature point are optimized to obtain the position and posture optimization result of the edge feature point;

[0152] According to the pose optimization results of each plane feature point corresponding to the voxel and the pose optimization results of each edge feature point, the pose optimization result of the voxel is obtained.

[0153] In a possible implementation manner, the posture optimization module 1303 is further used to:

[0154] Based on preset displacement consistency constraint parameters, displacement constraint processing is performed on the degraded data frames contained in the multi-frame point cloud data. The degraded data frames are data frames whose point cloud data are insufficiently constrained in a preset direction. The preset displacement consistency constraint parameters are used to indicate the change in the relative displacement between consecutive frames of point cloud data before and after optimization.

[0155] In a possible implementation, the global optimization module 1304 is specifically configured to:

[0156] According to the pose optimization results corresponding to each point cloud level, the relative pose factors corresponding to each point cloud level are generated;

[0157] Based on the relative pose factor, closed-loop factor and position constraint factor corresponding to each point cloud level, an overall cost function is constructed, and the overall cost function is used to indicate the error of pose estimation of multi-frame point cloud data;

[0158] The overall cost function is iteratively optimized until the iterative stopping condition is met, and the optimized point cloud data is obtained. The iterative stopping condition is that the sum of the errors of the relative pose factor, closed-loop factor and position constraint factor is less than the preset threshold.

[0159] In a possible implementation, the hierarchical construction module 1302 is specifically configured to:

[0160] For each point cloud group, voxel segmentation is performed on the point cloud group according to a preset voxel size to obtain a plurality of initial voxels corresponding to the point cloud group;

[0161] Determine a covariance matrix of the point cloud data in the first voxel according to the point cloud center, the number of point clouds and the vector coordinates of the point cloud data in the first voxel, where the first voxel is any one of the multiple initial voxels;

[0162] According to the eigenvalue of the covariance matrix of the point cloud data in the first voxel, it is determined whether to divide the first voxel and whether the first voxel satisfies a preset stop division condition. If the first voxel does not meet the preset stop division condition, the first voxel is divided again until the second voxel after division meets the preset stop division condition, so as to divide the point cloud group into multiple voxels.

[0163] In a possible implementation, the hierarchical construction module 1302 is specifically configured to:

[0164] Determine a characteristic ratio corresponding to the first voxel according to an eigenvalue of a covariance matrix of the point cloud data within the first voxel;

[0165] According to the feature ratio corresponding to the first voxel, it is determined whether the first voxel meets the preset segmentation condition. If the first voxel meets the preset segmentation condition, the first voxel is segmented; otherwise, the first voxel is not segmented.

[0166] In a possible implementation manner, the posture optimization module 1303 is further used to:

[0167] Based on the preset hash mapping strategy, a key value corresponding to each voxel is generated, and the key value is used to uniquely identify a voxel;

[0168] The point cloud data in each voxel is stored in a hash table according to a preset data structure, and the key value corresponding to each voxel is used as the index of each voxel, so that the point cloud data in each voxel can be queried from the hash table through the index of each voxel.

[0169] For descriptions of the processing flow of each module in the device and the interaction flow between each module, reference may be made to the relevant descriptions in the above method embodiment, which will not be described in detail here.

[0170] The present application embodiment also provides an electronic device 1400, such as Fig.14 As shown, it is a schematic diagram of the structure of an electronic device 1400 provided in an embodiment of the present application, including: a processor 1401, a memory 1402, and optionally, a bus 1403. The memory 1402 stores machine-readable instructions executable by the processor 1401. When the electronic device 1400 is running, the processor 1401 communicates with the memory 1402 through the bus 1403. When the machine-readable instructions are executed by the processor 1401, the steps of the map construction method described in any one of the above items are performed.

[0171] An embodiment of the present application further provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the map construction method as described in any one of the above items are executed.

[0172] Those skilled in the art can clearly understand that, for the convenience and simplicity of description, the specific working process of the system and device described above can refer to the corresponding process in the method embodiment, and will not be repeated in this application. In the several embodiments provided in this application, it should be understood that the disclosed system, device and method can be implemented in other ways. The device embodiments described above are merely schematic. For example, the division of the modules is only a logical function division. There may be other division methods in actual implementation. For example, multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some communication interfaces, indirect coupling or communication connection of devices or modules, which can be electrical, mechanical or other forms.

[0173] In addition, each functional unit in each embodiment of the present application can be integrated into a processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. If the function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or part of the technical solution that contributes to the prior art or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, RandomAccess Memory), disk or optical disk and other media that can store program code.

[0174] The above are only specific implementation methods of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be covered by the protection scope of the present application.

Claims

1. A map construction method, characterized in that: include: Obtain multi-frame point cloud data scanned and collected by a 3D laser radar sensor; According to the multi-frame point cloud data, a plurality of point cloud levels that are sequentially adjacent are constructed, wherein each point cloud level includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in adjacent first point cloud level and second point cloud level, the number of point cloud groups in the second point cloud level is less than the number of point cloud groups in the first point cloud level, and each point cloud group in the second point cloud level is formed by merging a plurality of point cloud groups in the first point cloud level; For each point cloud level, voxel segmentation is performed on each point cloud group in the point cloud level to obtain a plurality of voxels corresponding to each point cloud group, and pose optimization is performed in the point cloud level according to geometric characteristics of feature points of point cloud data in each voxel to obtain pose optimization results corresponding to the point cloud level, wherein the geometric characteristics of the feature points refer to whether the feature points are plane geometric characteristics or edge geometric characteristics, and the feature points include edge points and plane points; Based on the pose optimization results corresponding to each point cloud level and the preset constraint factors, the pose of the multi-frame point cloud data is globally optimized to obtain optimized point cloud data, and coordinate transformation is performed according to the optimized point cloud data to obtain a global map, wherein the preset constraint factors include a relative pose factor, a closed-loop factor, and a position constraint factor; Before globally optimizing the pose of the multi-frame point cloud data based on the pose optimization results corresponding to each point cloud level and the preset constraint factors to obtain the optimized point cloud data, the method further includes: Based on preset displacement consistency constraint parameters, displacement constraint processing is performed on the degraded data frames contained in the multiple frames of point cloud data, the degraded data frames are data frames in which the point cloud data are insufficiently constrained in a preset direction, and the preset displacement consistency constraint parameters are used to indicate the change in relative displacement between consecutive frames of point cloud data before and after optimization.

2. The method according to claim 1, characterized in that: The performing of posture optimization in the point cloud level according to the geometric characteristics of the feature points of the point cloud data in each voxel to obtain the posture optimization result corresponding to the point cloud level includes: Extracting feature points in each voxel at the point cloud level to obtain plane feature points and edge feature points corresponding to each voxel; According to the poses of the plane feature points and the edge feature points corresponding to each voxel, the pose optimization corresponding to the point cloud level is performed to obtain the pose optimization result corresponding to the point cloud level.

3. The method according to claim 2, characterized in that The optimizing the pose corresponding to the point cloud level according to the pose of the plane feature point and the pose of the edge feature point corresponding to each voxel includes: According to the pose of the plane feature point corresponding to each voxel and the pose of the edge feature point, the pose of each voxel is optimized to obtain the pose optimization result of each voxel; According to the pose optimization result of each voxel and the pose optimization result of the previous level of the point cloud level, the pose optimization result corresponding to the point cloud level is determined.

4. The method according to claim 3, characterized in that The pose optimization of each voxel is performed according to the pose of the plane feature point and the pose of the edge feature point corresponding to each voxel to obtain the pose optimization result of each voxel, including: Optimizing the pose of the plane feature point according to the distance from the plane feature point to the plane corresponding to the plane feature point to obtain a pose optimization result of the plane feature point; Optimizing the position and posture of the edge feature point according to the distance from the edge feature point to the edge line corresponding to the edge feature point to obtain an optimized position and posture result of the edge feature point; The pose optimization result of the voxel is obtained according to the pose optimization result of each plane feature point corresponding to the voxel and the pose optimization result of each edge feature point.

5. The method according to claim 1, characterized in that The globally optimizing the pose of the multi-frame point cloud data based on the pose optimization results corresponding to each point cloud level and the preset constraint factors to obtain the optimized point cloud data includes: According to the pose optimization results corresponding to each point cloud level, the relative pose factors corresponding to each point cloud level are generated; Based on the relative pose factors, closed-loop factors and position constraint factors corresponding to each point cloud level, an overall cost function is constructed, where the overall cost function is used to indicate the error of the pose estimation of the multi-frame point cloud data; The overall cost function is iteratively optimized until an iterative stopping condition is met, and the iterative optimization is stopped to obtain optimized point cloud data, wherein the iterative stopping condition is that the sum of the errors of the relative pose factor, the closed-loop factor and the position constraint factor is less than a preset threshold.

6. The method according to claim 1, characterized in that The voxel segmentation of each point cloud group in the point cloud level to obtain a plurality of voxels corresponding to each point cloud group includes: For each point cloud group, voxel segmentation is performed on the point cloud group according to a preset voxel size to obtain a plurality of initial voxels corresponding to the point cloud group; Determining a covariance matrix of the point cloud data in a first voxel according to a point cloud center, a point cloud quantity, and a vector coordinate of the point cloud data in the first voxel, wherein the first voxel is any one of the plurality of initial voxels; According to the eigenvalue of the covariance matrix of the point cloud data in the first voxel, determine whether to split the first voxel and determine whether the first voxel meets the preset stop splitting condition. If the first voxel does not meet the preset stop splitting condition, split the first voxel again until the second voxel after splitting meets the preset stop splitting condition, so as to split the point cloud group into multiple voxels.

7. The method according to claim 6, characterized in that The determining whether to segment the first voxel according to the eigenvalue of the covariance matrix of the point cloud data in the first voxel includes: determining a characteristic ratio corresponding to the first voxel according to an eigenvalue of a covariance matrix of point cloud data within the first voxel; According to the feature ratio corresponding to the first voxel, it is determined whether the first voxel meets the preset segmentation condition. If the first voxel meets the preset segmentation condition, the first voxel is segmented; otherwise, the first voxel is not segmented.

8. The method according to claim 1, characterized in that After obtaining the plurality of voxels corresponding to each point cloud group, the method further includes: Based on a preset hash mapping strategy, a key value corresponding to each voxel is generated, where the key value is used to uniquely identify a voxel; The point cloud data in each voxel is stored in a hash table according to a preset data structure, and the key value corresponding to each voxel is used as the index of each voxel, so as to query the point cloud data in each voxel from the hash table through the index of each voxel.

9. A map construction device, characterized in that: include: An acquisition module is used to acquire multi-frame point cloud data scanned and collected by a three-dimensional laser radar sensor; A hierarchical construction module, configured to construct a plurality of point cloud hierarchies that are sequentially adjacent to each other according to the plurality of frames of point cloud data, wherein each point cloud hierarchy includes at least one point cloud group, each point cloud group includes at least one frame of point cloud data, and, in adjacent first and second point cloud hierarchies, the number of point cloud groups in the second point cloud hierarchy is less than the number of point cloud groups in the first point cloud hierarchy, and each point cloud group in the second point cloud hierarchy is formed by merging a plurality of point cloud groups in the first point cloud hierarchy; A posture optimization module is used for performing voxel segmentation on each point cloud group in each point cloud level to obtain a plurality of voxels corresponding to each point cloud group, and performing posture optimization in the point cloud level according to geometric characteristics of feature points of point cloud data in each voxel to obtain a posture optimization result corresponding to the point cloud level, wherein the geometric characteristics of the feature points refer to whether the feature points are plane geometric characteristics or edge geometric characteristics, and the feature points include edge points and plane points; A global optimization module, used to globally optimize the pose of the multi-frame point cloud data based on the pose optimization results corresponding to each point cloud level and preset constraint factors to obtain optimized point cloud data, and perform coordinate transformation according to the optimized point cloud data to obtain a global map, wherein the preset constraint factors include relative pose factors, closed-loop factors and position constraint factors; The posture optimization module is also used to perform displacement constraint processing on the degraded data frames contained in the multiple frames of point cloud data based on preset displacement consistency constraint parameters, wherein the degraded data frames are data frames in which the point cloud data are insufficiently constrained in a preset direction, and the preset displacement consistency constraint parameters are used to indicate the change in the relative displacement between consecutive frames of point cloud data before and after optimization.

Images