A 3D pose estimation method, device and application that fuse normal vectors

Abstract

This solution provides a three-dimensional pose estimation method, device, and application that incorporate normal vectors, including the steps of: S1: obtaining laser point data; S2: managing an incremental voxel map; S3: solving for the three-dimensional pose based on the incremental voxel map. By expanding the voxel data structure of the voxel map, this solution not only stores point cloud data but also combines normal vector information, enabling the system to evaluate the consistency of normal vectors during nearest neighbor search and map update processes, effectively distinguishing the front and back sides of the surface, avoiding incorrect point-to-plane constraints, and also proposing an adaptive radius KD-tree search method that dynamically adjusts the search radius according to the local density of the laser data. In addition, the present invention also introduces a recently used cache strategy to improve real-time performance and storage efficiency and support incremental updates of the voxel map. The present invention can significantly improve the accuracy of pose estimation and the system efficiency in indoor and outdoor environments.

Description

Technical Field

[0001] This application relates to the technical field of mobile device positioning, and in particular to a three-dimensional pose estimation method, device and application that fuse normal vectors. Background Art

[0002] In the field of indoor map drawing and positioning, SLAM (Simultaneous Localization and Mapping) technology has important application value. SLAM technology is widely used in multiple fields such as autonomous driving, robot navigation, and augmented reality. However, in complex indoor environments, SLAM systems often face a challenge, namely the so-called "two-sided mapping problem", which usually occurs between walls, doors and windows, and other closely arranged surfaces. In these cases, the SLAM system may erroneously identify multiple adjacent surfaces as a single plane, thus significantly affecting the accuracy and consistency of the map.

[0003] Traditional SLAM methods usually rely on point cloud data for environmental modeling and positioning, but these methods often fail to effectively solve the data association problem between adjacent surfaces. For example, the front and back sides of a wall may be mistaken for the same surface, resulting in incorrect point-plane constraints during map updates, thereby affecting the accuracy and robustness of the entire SLAM system. In addition, in the case of uneven point cloud data density, traditional nearest neighbor search methods may not be able to accurately calculate normal vectors, further affecting the map update and optimization effects.

[0004] Chinese Patent CN117974919A provides a high-precision three-dimensional map reconstruction method and system. Although it mentions constructing a three-dimensional reconstruction map using real voxel values after optimizing voxels, in this patent solution, a TSDF integrator is used to integrate feature point clouds to obtain voxels, and the voxels obtained by integrating feature point clouds using the TSDF model are three-dimensional space blocks. Each voxel stores the distance between the block and the nearest object surface, and does not store point cloud data; moreover, this patent uses the remaining feature point clouds in the voxel to construct a local plane, thereby calculating normal vector information for each feature point. The normal vectors mentioned in its solution are used to optimize the voxel gradient, aiming to reduce the system calculation amount while ensuring the accuracy of the reconstructed map, but it still fails to solve the problem of the accuracy of adjacent surface recognition. Summary of the Invention

[0005] The embodiments of this application provide a three-dimensional pose estimation method, device and application that fuse normal vectors, overcoming the problem of incorrect point-plane association existing in three-dimensional pose estimation in existing indoor and outdoor environments, and aiming to improve the accuracy and consistency of map construction in indoor environments.

[0006] In a first aspect, an embodiment of the present application provides a three-dimensional pose estimation method integrating normal vectors, including the following steps:

[0007] S1: Obtain laser point data:

[0008] Obtain laser data composed of laser data points and calculate the normal vector of each laser data point;

[0009] S2: Manage the incremental voxel map:

[0010] Expand the voxel data structure of the voxels in the voxel map with the normal vector and re-partition the pixel blocks of the voxel map based on the normal vector;

[0011] For the laser data points that need to insert voxel blocks, calculate the hash index of the current laser data point. If there is no voxel block corresponding to the hash index in the incremental voxel map, initialize the voxel block and store the laser data point in the voxel block; if there is a voxel block corresponding to the hash index in the incremental voxel map, judge the normal vector field consistency between the normal vector of the laser data point and the voxel data of the corresponding voxel block. If the normal vector fields are consistent, update the laser data point to the front of the current voxel block. If the normal vector fields are inconsistent, update the laser data point to the back of the current voxel block;

[0012] S3: Solve the three-dimensional pose based on the incremental voxel map.

[0013] In a second aspect, an embodiment of the present application provides a three-dimensional pose estimation device integrating normal vectors, including:

[0014] An acquisition unit for acquiring laser data composed of laser data points

[0015] An analysis and processing unit for calculating the normal vector of the laser data, judging whether the field of view of the current laser data point is consistent with the existing voxel data of the current voxel for the laser data points that need to insert voxels. If they are consistent, insert the current laser data point into the front of the current voxel. If they are inconsistent, insert the current laser data point into the back of the current voxel, expand the voxel data structure of the voxels in the voxel map with the normal vector, and re-partition the pixel blocks of the voxel map based on the normal vector; for the laser data points that need to insert voxel blocks, calculate the hash index of the current laser data point. If there is no voxel block corresponding to the hash index in the incremental voxel map, initialize the voxel block and store the laser data point in the voxel block; if there is a voxel block corresponding to the hash index in the incremental voxel map, judge the normal vector field consistency between the normal vector of the laser data point and the voxel data of the corresponding voxel block. If the normal vector fields are consistent, update the laser data point to the front of the current voxel block. If the normal vector fields are inconsistent, update the laser data point to the back of the current voxel block, and solve the three-dimensional pose based on the incremental voxel map;

[0016] An output unit for outputting the three-dimensional pose calculated by the analysis and processing unit.

[0017] In a third aspect, an embodiment of the present application provides a computer device, including a memory and a processor, where the memory stores a computer program, and is characterized in that when the processor executes the computer program, the steps of any one of the three-dimensional pose estimation methods for fusing normal vectors are implemented.

[0018] The main contributions and innovations of the present invention are as follows:

[0019] 1) In this solution, by expanding the voxel map structure, voxels can not only store point cloud data but also preserve the normal vector information of the point cloud data, so as to evaluate the consistency of normal vectors during the nearest neighbor search and map update processes. This process can effectively distinguish the front and back sides of the wall, avoid incorrect point-to-plane constraints, and thus improve the accuracy of the map.

[0020] 2) This solution adopts a KD-tree search method with an adaptive search radius, which can dynamically adjust the search radius according to the local density of the point cloud, thereby improving the accuracy of normal vector calculation. This method effectively solves the calculation difference between high-density regions and low-density regions and improves the overall performance of the system.

[0021] 3) This solution also introduces the "Least Recently Used (LRU) cache strategy", which can effectively support the incremental update of the voxel map, reduce the waste of storage space, and accelerate the system response time. Therefore, while solving the problem of double-sided mapping in indoor environment SLAM, the present invention can also improve the overall performance and efficiency of the system.

[0022] Details of one or more embodiments of the present application are set forth in the following drawings and description to make other features, objects, and advantages of the present application more comprehensible. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation of the present application. In the drawings:

[0024] Figure 1 is a schematic flowchart of a three-dimensional pose estimation method for fusing normal vectors in an embodiment;

[0025] Figure 2 is a schematic diagram of kdtree nearest neighbor search with an adaptive search radius in an embodiment;

[0026] Figure 3 is a schematic diagram of the point-plane pose association in different map managements in an embodiment, where Figure 3wherein a is a schematic diagram of radar scans at different positions; Figure 3 wherein b is data storage under ordinary map management; Figure 3 wherein c is data storage under the map management of the present invention;

[0027] Figure 4 is a structural block diagram of three-dimensional pose estimation by fusing normal vectors in an embodiment;

[0028] Figure 5 is an internal structure diagram of a computer device in an embodiment. Detailed implementation manners

[0029] Here, exemplary embodiments will be described in detail, and examples thereof are shown in the drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementation manners described in the following exemplary embodiments do not represent all implementation manners consistent with one or more embodiments of this specification. On the contrary, they are merely examples of devices and methods consistent with some aspects of one or more embodiments of this specification as detailed in the appended claims.

[0030] It should be noted that: In other embodiments, the steps of the corresponding methods are not necessarily executed in the order shown and described in this specification. In some other embodiments, the steps included in the method may be more or less than those described in this specification. In addition, a single step described in this specification may be decomposed into multiple steps for description in other embodiments; and multiple steps described in this specification may also be combined into a single step for description in other embodiments.

[0031] Embodiment 1

[0032] This solution provides a three-dimensional pose estimation method by fusing normal vectors, including the following steps:

[0033] S1: Obtain laser point data:

[0034] Obtain laser data composed of laser data points and calculate the normal vector of each laser data point;

[0035] S2: Manage the incremental voxel map:

[0036] Expand the voxel data structure of the voxels of the voxel map with the normal vector, and re-divide the pixel blocks of the voxel map based on the normal vector;

[0037] For the laser data points where voxel blocks need to be inserted, calculate the hash index of the current laser data point. If the voxel block corresponding to the hash index does not exist in the incremental voxel map, initialize the voxel block and store the laser data point in the voxel block; if the voxel block corresponding to the hash index exists in the incremental voxel map, judge the normal vector field consistency between the normal vector based on the laser data point and the voxel data of the corresponding voxel block. If the normal vector fields are consistent, update the laser data point to the front of the current voxel block; if the normal vector fields are inconsistent, update the laser data point to the back of the current voxel block.

[0038] S3: Solve the three-dimensional pose based on the incremental voxel map.

[0039] Regarding step S1:

[0040] In step S1, in order to improve the data quality of the laser data points and at the same time reduce the data volume for subsequent calculations, step S1 further includes the steps:

[0041] S11: Obtain the laser data composed of laser data points, and filter out the laser data points whose distances from the laser emission origin are within a set distance range.

[0042] S12: Based on the distance between each laser data point and the laser emission origin, adaptively search for a radius, determine the nearest neighbor set of each laser data point based on the search radius, and calculate the normal vector of each laser data point based on the nearest neighbor set.

[0043] In step S11 of this solution, invalid data is eliminated by filtering out the laser data points whose distances from the laser emission origin are within a set distance range. Specifically, the set distance range is a distance range that is not less than the low value threshold and not greater than the high value threshold, expressed as , in other words, filter out the laser data points in the obtained laser data whose distances from the laser emission origin are less than the low value threshold and greater than the high value threshold where the low value threshold is the minimum scanning radius set artificially, and the high value threshold is the maximum scanning radius set artificially.

[0044] In step S12 of this solution, through the KD-tree search method with an adaptive search radius, the search radius is dynamically adjusted according to the local density of the laser data, so as to improve the accuracy of the normal vector calculation of each laser point data. This method effectively solves the calculation difference between the high-density area and the low-density area and improves the overall performance of the system.

[0045] Specifically, the formula for adaptively searching for a radius based on the distance between each laser data point and the laser emission origin is as follows:

[0046]

[0047] Among them are the set maximum search radius and minimum search radius are the set maximum scan radius and minimum scan radius r is the search radius is the distance from each laser data point to the laser emission origin

[0048] Figure 2 is the schematic diagram of the kdtree nearest neighbor search for the adaptive search radius of this solution, as Figure 2 shown. Different laser data points will adapt different search radii due to the density of the point cloud they are in

[0049] In addition, in the step of "calculating the normal vector of each laser data point based on the nearest neighbor set", the eigenvalues of the nearest neighbor set are solved according to the method of covariance matrix decomposition, and the smallest eigenvalue is taken as the normal vector of the current laser data point. In other words, using the method of covariance matrix decomposition, calculate the eigenvalues of the nearest neighbor set where the smallest eigenvalue is the normal vector of the laser data point

[0050] Furthermore, in order to reduce the computational complexity of subsequent calculations, in the step of "determining the nearest neighbor set of each laser data point based on the search radius", if the number of laser data points in the nearest neighbor set is less than the set number threshold, the current laser data point is located as an outlier; if the planarity of the nearest neighbor set is greater than the set judgment threshold, the current laser data point is defined as an outlier and the outlier is removed

[0051] Specifically, if the number of laser data points in the nearest neighbor set is less than the set number threshold A nearest neighbor set with too few data points indicates that the association between this laser data point and most of the surrounding laser data points is weak, and the data distribution around this laser data point is very sparse. Then this laser data point may be an outlier or a noise point, so it is removed as an outlier

[0052] If the planarity of the nearest neighbor set is greater than the set judgment threshold This means that the data distribution around this laser data point is quite different from the common planar structure, does not conform to the normal environmental characteristics, and is very likely due to noise, measurement error, or scanning a special object, so it is removed as an outlier

[0053] In some embodiments, the calculation formula for the planarity of the nearest neighbor set is

[0054] ;

[0055] Among them The eigenvalue obtained by covariance matrix decomposition of the nearest neighbor set, is the minimum value among the eigenvalues.

[0056] Regarding step S2:

[0057] Different from the traditional voxel structure, this solution uses an incremental voxel map management that fuses normal vectors to expand the voxel data structure of the voxel map, enabling the voxel data structure to store not only position information but also normal vector information. Thus, during the nearest neighbor search and map update processes, the consistency of the normal vectors is evaluated. This process can effectively distinguish between the front and back sides of the wall, avoiding incorrect point-to-plane constraints and thereby improving the accuracy of the map.

[0058] Specifically, step S2 includes the steps:

[0059] S21: Expand the voxel data structure of the voxels in the voxel map with normal vectors and re-partition the pixel blocks of the voxel map based on the normal vectors;

[0060] S22: For the laser data points that need to be inserted into the voxel block, calculate the hash index of the current laser data point. If the voxel block corresponding to the hash index does not exist in the incremental voxel map, initialize the voxel block and store the laser data point in the voxel block; if the voxel block corresponding to the hash index exists in the incremental voxel map, judge the consistency of the normal vector field based on the normal vector of the laser data point and the voxel data of the corresponding voxel block. If the normal vector fields are consistent, update the laser data point to the front of the current voxel block; if the normal vector fields are inconsistent, update the laser data point to the back of the current voxel block.

[0061] Furthermore, in step S31, expand the storage of normal vectors in the voxel data structure of the voxel map, and divide each voxel block into multiple voxel blocks corresponding to different normal vectors based on the normal vectors. This means that a more detailed division can be made inside the voxel blocks of the voxel map based on the normal vectors. In three-dimensional space, a voxel block may cover the surfaces of multiple objects in different directions. For example, at the corner of a wall, a voxel block may contain two perpendicular walls at the same time. By expanding the storage of normal vectors and dividing the sub-voxel blocks with different normal vectors, these different-direction surfaces can be represented more accurately, avoiding mixing the data of different surfaces together. Thus, the ability of the voxel map to accurately describe the object surface is improved. When performing data insertion and update subsequently, the laser data points can be accurately assigned to the corresponding sub-voxel blocks according to the normal vectors, reducing the occurrence of incorrect associations and improving the accuracy and reliability of the map.

[0062] In step S22, for the laser data points that need to be inserted into the voxel block, convert the laser data points to the global coordinate system and calculate the hash index of the current laser data point.

[0063] Among them, the formula for converting laser data points to the global coordinate system is as follows:

[0064] ;

[0065] ;

[0066] Among them and are the three-dimensional point coordinates and normal vectors of the laser data points in the laser coordinate system, , are the three-dimensional coordinate points and normal vectors of the laser data points in the global coordinate system, is the transformation matrix, is 's rotation matrix.

[0067] Among them, the formula for calculating the hash index of the current laser data point is as follows:

[0068] ;

[0069] Among them is the hash index, is the resolution of the voxel map.

[0070] Furthermore, in step S32, if there is no voxel block corresponding to the hash index in the incremental voxel map, initialize the voxel block, store the laser data points in the voxel block, and mark the current hash index as the latest active voxel block; if there is a voxel block corresponding to the hash index in the incremental voxel map, judge the normal vector field of view consistency between the normal vector of the laser data point and the voxel data of the corresponding voxel block. If the normal vector field of view is consistent, update the laser data point to the front of the current voxel block. If the normal vector field of view is inconsistent, update the laser data point to the back of the current voxel block, and mark the current hash index as the latest active voxel block.

[0071] In the embodiment of this solution, the "Least Recently Used (LRU) cache policy" is introduced by marking the current hash index as the latest active voxel block. This policy can effectively support the incremental update of the voxel map, reduce the waste of storage space, and accelerate the system response time.

[0072] Specifically, in the step of "judging the normal vector field of view consistency between the normal vector of the laser data point and the voxel data of the corresponding voxel block", obtain the main direction of the normal vector distribution of the voxel data of the current voxel. If the angle between the normal vector of the current laser data point and the main direction of the normal vector distribution is less than the angle threshold, it is judged that the normal vector field of view of the current laser data point is consistent with the existing voxel data in the current voxel, otherwise it is not.

[0073] In some embodiments, if the main direction of the normal vector distribution of the voxel data of the current voxel has been calculated, the main direction of the normal vector distribution of the voxel data of the current voxel is directly retrieved; if the main direction of the normal vector distribution of the voxel data of the current voxel has not been calculated, all the voxel data in the current voxel are traversed to construct a covariance matrix, and the main direction of the normal vector distribution of the current voxel is calculated based on the covariance matrix.

[0074] In some embodiments, the vector of the current laser data point is dot-multiplied with the main direction of the normal vector distribution to calculate the angle between the current laser data point and the main direction of the normal vector distribution. Through the judgment of the normal vector field consistency within the voxel, this solution can determine the field-of-view relationship between the new laser data point and the existing data within the voxel based on the normal vector of the laser data point, and then decide whether to insert the new laser data point into the front or back region of the voxel block. This can avoid the random storage of laser data points in different field-of-view directions within the voxel, organize the data within the voxel reasonably according to the field-of-view direction, and through the judgment of the normal vector field consistency, it can effectively distinguish the laser data points from different object surfaces or different perspectives within the same voxel block, avoiding incorrect point-surface associations.

[0075] It is judged whether the pixel point of the current laser data point and the normal vector of the corresponding pixel block are consistent. If they are consistent, it is considered that the normal vector field is consistent. It should be noted that if the normal vector field is consistent, it means that the current laser data point and the voxel data in the current voxel block come from the same surface, so the laser data point is updated to the front region of the voxel block; on the contrary, if the normal vector field is inconsistent, it means that the current laser data point and the voxel data of the current voxel block come from different surfaces, and the laser data point is updated to the back region of the voxel block.

[0076] Regarding step S3:

[0077] Step S3 of this solution further includes the following steps:

[0078] S31: Convert the acquired laser data to the global coordinate system and calculate the corresponding hash index based on each laser data point;

[0079] S32: Search for the associated voxel block of each laser data point in the incremental voxel map based on the hash index, where the associated voxel block includes the voxel block corresponding to the current index and the voxel blocks of 26 indexes adjacent to the current index;

[0080] S33: Obtain the associated data between each laser data point and the voxel data in the associated voxel block, construct an optimization problem based on the associated data, and solve the three-dimensional pose based on the optimization problem.

[0081] S34: Solve the three-dimensional pose based on the incremental voxel map.

[0082] Through the method of searching for associated voxel blocks by hash indexing, local information related to the current laser data point can be efficiently searched in the voxel map. This local information is very important for pose estimation because it can reflect the relative position and pose of the current point in the environment. At the same time, through this method of local neighborhood search, the computational amount can be greatly reduced and the efficiency of the algorithm can be improved.

[0083] Specifically, the implementation means of step S31 is the same as that of step S22. The formula for converting the laser data point to the global coordinate system is as follows:

[0084] ;

[0085] ;

[0086] Where and are the three-dimensional point coordinates and normal vectors of the laser data point in the laser coordinate system, , are the three-dimensional coordinate points and normal vectors of the laser data point in the global coordinate system, is the transformation matrix, is 's rotation matrix.

[0087] The formula for calculating the hash index of the current laser data point is as follows:

[0088] ;

[0089] Where is the hash index, is the resolution of the voxel map.

[0090] In step S33, voxel data in the associated voxel block that is less than the distance comparison threshold from the current laser data point and has an angle less than the angle comparison threshold with the normal vector of the current laser data point are obtained to form the associated data set of the current laser data point, and the voxel data in the associated data set is sorted.

[0091] In some embodiments, the voxel data in the associated data set is sorted in ascending order of distance.

[0092] In other words, the voxel data in the associated data set needs to satisfy the following two conditions simultaneously:

[0093] ① The distance between the voxel data and the current laser data point is less than the distance comparison threshold, where the distance comparison threshold is set artificially;

[0094] ② The voxel data and the current laser data point satisfy the field of view consistency, that is, the angle between the voxel data and the normal vector of the current laser data point is less than the angle comparison threshold.

[0095] It should be noted that the judgment of the field of view consistency between voxel data and laser data points is the same as that of the previous normal vector field of view consistency. Specifically, obtain the main direction of the normal vector distribution of the voxel data of the current voxel. If the included angle between the normal vector of the current laser data point and the main direction of the normal vector distribution is less than the included angle threshold, it is determined that the current laser data point and the existing voxel data in the current voxel satisfy the field of view consistency; otherwise, it does not.

[0096] Furthermore, in step S33, take multiple voxel data sorted in the front in the associated data set to construct the corresponding point cloud residual constraint, use the plane consistency to construct the weight coefficient of the current point cloud residual constraint, construct an optimization problem based on the weight coefficient weighted point cloud residual constraint, and solve the optimization problem to obtain the three-dimensional pose.

[0097] Preferably, take multiple voxel data sorted in the top 10 in the associated data set to construct the corresponding point cloud residual constraint.

[0098] Since the consistency degrees of different laser data points with the plane are different, the data with high consistency can more reliably reflect the environmental characteristics and pose information. Therefore, higher weights should be given during the optimization process; on the contrary, the data with low consistency may have large errors or noises, and giving lower weights can reduce their adverse effects on the final result. Therefore, when constructing the optimization problem, each point cloud residual constraint will be multiplied by the corresponding weight coefficient. In this way, the weight coefficient directly affects the proportion of each point cloud residual constraint in the objective function, thereby affecting the final optimization result. If the weight of a certain point cloud residual constraint is large, the optimization process will be more inclined to make this constraint satisfied to minimize the objective function.

[0099] In some embodiments, a target function is constructed as an optimization problem by comprehensively considering all point-plane constraints, and an iterative optimization algorithm is used to minimize the target function until the convergence condition is met to solve for the three-dimensional pose.

[0100] It should be noted that the application scenario of this solution is in the field of mobile device positioning technology, and it can be applied to mobile robots to obtain laser data and estimate the three-dimensional pose based on the obtained laser data. Figure 3 In [reference], a is the environmental map. Lasers observe data in different regions at different positions. If the normal vector information is not included in the voxel map, when performing data association, it is impossible to distinguish which side of the voxel map the data is observed from. Here, according to the traditional nearest neighbor search, data on the other side of the wall may be associated, which may result in Figure 3 the incorrect association in [reference] b; while adding the normal vector information and field of view consistency processing of the present invention, it can be as Figure 3As shown by c in [reference], it can be well distinguished according to the data observation field of view in the voxel map, improving the accuracy of data association and the precision of pose estimation.

[0101] In summary, the three-dimensional pose estimation method with fused normal vectors provided by this solution, by expanding the voxel map structure, not only stores laser point data but also combines normal vector information, enabling the system to evaluate the consistency of normal vectors during the nearest neighbor search and map update processes, effectively distinguishing the front and back sides of the surface, avoiding incorrect point-to-plane constraints. At the same time, this solution proposes an adaptive radius KD-tree search method that dynamically adjusts the search radius according to the local density of the point cloud. In addition, the present invention also introduces a Least Recently Used (LRU) cache strategy to improve real-time performance and storage efficiency and support incremental updates of the voxel map. The present invention can significantly improve the accuracy of pose estimation and system efficiency in indoor and outdoor environments.

[0102] Embodiment 2

[0103] Based on the same concept, referring to Figure 4 , this application also proposes a three-dimensional pose estimation device with fused normal vectors, including:

[0104] An acquisition unit 101, configured to acquire laser data composed of laser data points;

[0105] An analysis and processing unit 102, configured to calculate the normal vector of the laser data, and for the laser data points to be inserted into the voxel, determine whether the field of view of the current laser data point is consistent with the existing voxel data of the current voxel. If they are consistent, insert the current laser data point into the front of the current voxel; if they are inconsistent, insert the current laser data point into the back of the current voxel, expand the voxel data structure of the voxel of the voxel map with the normal vector, and re-divide the pixel blocks of the voxel map based on the normal vector; for the laser data points to be inserted into the voxel block, calculate the hash index of the current laser data point. If the voxel block corresponding to the hash index does not exist in the incremental voxel map, initialize the voxel block and store the laser data point in the voxel block; if the voxel block corresponding to the hash index exists in the incremental voxel map, determine the consistency of the normal vector field of view based on the normal vector of the laser data point and the voxel data of the corresponding voxel block. If the normal vector fields of view are consistent, update the laser data point to the front of the current voxel block; if the normal vector fields of view are inconsistent, update the laser data point to the back of the current voxel block, and solve the three-dimensional pose based on the incremental voxel map;

[0106] An output unit 103, configured to output the three-dimensional pose calculated by the analysis and processing unit.

[0107] For the technical feature content identical to that of Embodiment 2, refer to the introduction content of Embodiment 1.

[0108] Embodiment 3

[0109] The present solution also provides a computer device, which may be a PC (Personal Computer), or may also be a terminal device such as a smart phone, a tablet computer, a portable computer, etc. The computer device at least includes a memory, a processor, a communication bus, and a network interface. Among them, 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 and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network 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, it implements the three-dimensional pose estimation method for fusing normal vectors shown in Embodiment 1.

[0110] The display screen of the computer device may be a liquid crystal display screen or an electronic ink display screen. The input device of the computer device may be a touch layer covered on the display screen, or may also be a button, a trackball or a touchpad provided on the casing of the computer device, or may also be an external keyboard, a touchpad or a mouse, etc.

[0111] Those skilled in the art can understand that Figure 5 the structure shown in is only a block diagram of some structures related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine some components, or have different component arrangements.

[0112] In one embodiment, a computer device is provided, including a memory and a processor. A computer program is stored in the memory. When the processor executes the computer program, the following steps are implemented:

[0113] S1: Obtain laser point data:

[0114] Obtain laser data composed of laser data points and calculate the normal vector of each laser data point;

[0115] S2: Manage the incremental voxel map:

[0116] Expand the voxel data structure of the voxels of the voxel map with the normal vector, and re-divide the pixel blocks of the voxel map based on the normal vector;

[0117] For the laser data points where voxel blocks need to be inserted, calculate the hash index of the current laser data point. If the voxel block corresponding to the hash index does not exist in the incremental voxel map, initialize the voxel block and store the laser data point in the voxel block; if the voxel block corresponding to the hash index exists in the incremental voxel map, judge the normal vector field consistency between the normal vector based on the laser data point and the voxel data of the corresponding voxel block. If the normal vector fields are consistent, update the laser data point to the front of the current voxel block; if the normal vector fields are inconsistent, update the laser data point to the back of the current voxel block.

[0118] S3: Solve the three-dimensional pose based on the incremental voxel map.

[0119] The embodiments in this specification are all described in a progressive manner. For the same or similar parts between the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can refer to the partial description of the method embodiment.

Claims

1. A three-dimensional pose estimation method integrating normal vectors, characterized in that: The following steps are involved: S1: Get laser point data: Acquire laser data consisting of laser data points and calculate a normal vector for each laser data point; S2: Managing incremental voxel maps: extending a voxel data structure of a voxel of the voxel map by a normal vector and re-dividing pixel blocks of the voxel map based on the normal vector; For the laser data point that needs to be inserted into the voxel block, the hash index of the current laser data point is calculated. If the voxel block corresponding to the hash index does not exist in the incremental voxel map, the voxel block is initialized and the laser data point is stored in the voxel block. If the voxel block corresponding to the hash index exists in the incremental voxel map, the normal vector field of view consistency between the normal vector of the laser data point and the voxel data of the corresponding voxel block is determined. If the normal vector field of view is consistent, the laser data point is updated to the front side of the current voxel block. If the normal vector field of view is inconsistent, the laser data point is updated to the back side of the current voxel block. S3: Solving 3D pose based on incremental voxel maps.

2. The three-dimensional pose estimation method of fusion normal vector according to claim 1, characterized in that: Step S1 further comprises the steps of: S11: Acquire laser data consisting of laser data points, and select laser data points within a set distance range from the laser emission origin; S12: Adaptively search for a radius based on the distance between each laser data point and the laser emission origin, determine a nearest neighbor set for each laser data point based on the search radius, and calculate a normal vector for each laser data point based on the nearest neighbor set.

3. The three-dimensional pose estimation method of fusion normal vector according to claim 2, characterized in that: In the step of "determining the nearest neighbor set of each laser data point based on the search radius", if the data of the laser data point in the nearest neighbor set is less than the set quantity threshold, the current laser data point is located as an outlier; if the planarity of the nearest neighbor set is greater than the set judgment threshold, the current laser data point is defined as an outlier and is removed.

4. The three-dimensional pose estimation method of fusion normal vector according to claim 1, characterized in that: In the step of "determining the consistency of the normal vector field of view based on the normal vector of the laser data point and the normal vector field of view of the voxel data of the corresponding voxel block", the main direction of the normal vector distribution of the voxel data of the current voxel is obtained. If the angle between the normal vector of the current laser data point and the main direction of the normal vector distribution is less than the angle threshold, it is determined that the normal vector field of view of the current laser data point is consistent with the existing voxel data in the current voxel, otherwise it is not.

5. The three-dimensional pose estimation method of fusion normal vector according to claim 4, characterized in that: If the main direction of the normal vector distribution of the voxel data of the current voxel has been calculated, the main direction of the normal vector distribution of the voxel data of the current voxel is directly called; if the main direction of the normal vector distribution of the voxel data of the current voxel has not been calculated, all voxel data in the current voxel are traversed, a covariance matrix is ​​constructed with all voxel data, and the main direction of the normal vector distribution of the current voxel is calculated based on the covariance matrix.

6. The three-dimensional pose estimation method of fusion normal vector according to claim 1, characterized in that: The normal vector is stored in a voxel data structure of the voxel map, and each voxel block is divided into a plurality of voxel blocks corresponding to different normal vectors based on the normal vector.

7. The three-dimensional pose estimation method of fusion normal vector according to claim 1, characterized in that: If there is no voxel block corresponding to the hash index in the incremental voxel map, the voxel block is initialized and the laser data point is stored in the voxel block, and the current hash index is marked as the latest active voxel block; if there is a voxel block corresponding to the hash index in the incremental voxel map, the consistency of the normal vector field of view of the normal vector of the laser data point and the voxel data of the corresponding voxel block is determined. If the normal vector field of view is consistent, the laser data point is updated to the front side of the current voxel block. If the normal vector field of view is inconsistent, the laser data point is updated to the back side of the current voxel block, and the current hash index is marked as the latest active voxel block.

8. The three-dimensional pose estimation method of fusion normal vector according to claim 1, characterized in that: Step S3 further comprises the following steps: S31: converting the acquired laser data into a global coordinate system, and calculating a corresponding hash index based on each laser data point; S32: searching for an associated voxel block of each laser data point in the incremental voxel map based on a hash index, wherein the associated voxel block includes a voxel block corresponding to a current index and voxel blocks of 26 indexes adjacent to the current index; S33: Obtain association data between each laser data point and voxel data in an associated voxel block, construct an optimization problem based on the association data, and obtain a three-dimensional pose based on solving the optimization problem.

9. A three-dimensional pose estimation device integrating normal vectors, characterized in that: include: An acquisition unit, used for acquiring laser data consisting of laser data points; An analysis and processing unit is used to calculate the normal vector of the laser data, and for the laser data point to be inserted into the voxel, determine whether the field of view of the current laser data point is consistent with the field of view of the existing voxel data of the current voxel, if consistent, insert the current laser data point into the front side of the current voxel, if inconsistent, insert the current laser data point into the back side of the current voxel, expand the voxel data structure of the voxel of the voxel map with the normal vector, and re-divide the pixel blocks of the voxel map based on the normal vector; For the laser data points that need to be inserted into the voxel block, the hash index of the current laser data point is calculated. If the voxel block corresponding to the hash index does not exist in the incremental voxel map, the voxel block is initialized and the laser data point is stored in the voxel block. If the voxel block corresponding to the hash index exists in the incremental voxel map, the normal vector field of view consistency between the normal vector of the laser data point and the voxel data of the corresponding voxel block is determined. If the normal vector field of view is consistent, the laser data point is updated to the front side of the current voxel block. If the normal vector field of view is inconsistent, the laser data point is updated to the back side of the current voxel block, and the three-dimensional pose is solved based on the incremental voxel map. The output unit is used to output the three-dimensional posture calculated by the analysis and processing unit.

10. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, the steps of the three-dimensional pose estimation method by fusing normal vectors as described in any one of claims 1 to 8 are implemented.

Images