A real-time grid map updating method

Abstract

The present invention relates to a real-time grid map updating method, which belongs to the technical field of grid maps and solves the problem of high time consumption of grid map updating in the prior art. The method comprises: obtaining point cloud data at the current moment, determining the voxel unit to which each three-dimensional point in the point cloud data belongs; for each voxel unit corresponding to the point cloud data at the current moment, if it is determined that the type of the voxel unit is a surface, then based on the three-dimensional point of the voxel unit, Gaussian process estimation is used to perform surface reconstruction on the voxel unit to obtain the reconstruction result of the voxel unit at the current moment; for each voxel unit corresponding to the point cloud data at the current moment, if the voxel unit does not exist in the voxel unit history library, then the voxel unit is added to the voxel unit history library; otherwise, the voxel unit history library is updated based on the three-dimensional point of the voxel unit at the current moment or the reconstruction result. Real-time grid map updating is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of grid maps, and in particular to a real-time grid map updating method. Background Art

[0002] Commonly used 3D model representation methods include point cloud maps, placeholder grid maps, TSDF maps, Surfel patch maps and mesh maps. Among them, mesh maps have the advantages of being able to approximate most complex 3D model structures, avoiding discontinuities, low storage costs and high scalability compared to other maps.

[0003] However, existing methods need to continuously retriangulate when building and updating Mesh maps. The time consumption of this process is exponentially related to the number of 3D points and is very time-consuming. Therefore, it is mainly suitable for offline processing scenarios, which limits its popularity in real-time application scenarios and is not suitable for application scenarios with high real-time requirements. For example, in XR (extended reality) application scenarios, Mesh maps need to be updated in real time to meet spatial computing and human-computer interaction requirements such as virtual-real occlusion, virtual-real collision, light field estimation, somatosensory interaction, tactile interaction, etc. The existing mesh map update method cannot run in real time on XR devices. Summary of the invention

[0004] In view of the above analysis, an embodiment of the present invention aims to provide a real-time grid map updating method to solve the problem of high time consumption of the existing grid map updating method.

[0005] On the one hand, an embodiment of the present invention provides a real-time grid map updating method, comprising the following steps:

[0006] Obtain the point cloud data at the current moment, and determine the voxel unit to which each three-dimensional point in the point cloud data belongs;

[0007] For each voxel unit corresponding to the point cloud data at the current moment, if it is determined that the type of the voxel unit is a surface, a surface reconstruction is performed on the voxel unit using Gaussian process estimation based on the three-dimensional points of the voxel unit to obtain a reconstruction result of the voxel unit at the current moment;

[0008] For each voxel unit corresponding to the point cloud data at the current moment, if the voxel unit does not exist in the voxel unit history library, the voxel unit is added to the voxel unit history library; otherwise, the voxel unit history library is updated based on the three-dimensional point or reconstruction result of the voxel unit at the current moment.

[0009] Based on the further improvement of the above method, the reconstruction result includes the surface reconstruction direction, the reconstruction point of each surface reconstruction direction and the uncertainty of each reconstruction point;

[0010] The voxel unit history library is updated based on the three-dimensional point or reconstruction result of the voxel unit at the current moment, including:

[0011] If the voxel unit in the voxel unit history library has not been reconstructed, the three-dimensional point of the voxel unit in the voxel unit history library and the three-dimensional point of the voxel unit at the current moment are merged, and based on the merged three-dimensional point, it is determined whether the voxel unit is a surface type, and if so, the voxel unit is reconstructed by using Gaussian process estimation to obtain the reconstruction result of the voxel unit, and the voxel unit in the voxel unit history library is updated;

[0012] If the voxel unit in the voxel unit history library has undergone surface reconstruction, the voxel unit in the voxel unit history library is updated according to the uncertainty of the reconstruction result at the current moment.

[0013] Based on the further improvement of the above method, the voxel unit in the voxel unit history library is updated according to the uncertainty of the reconstruction result at the current moment, including:

[0014] Each reconstruction point in each surface reconstruction direction in the reconstruction result of the voxel unit at the current moment is traversed in turn. If the uncertainty of the reconstruction point and the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library are both less than the fourth threshold, the reconstruction point and uncertainty corresponding to the voxel unit in the voxel unit history library are updated using the following formula:

[0015] mGpP=(cGpP*mGpVar+mGpP*cGpVar) / (cGpVar+mGpVar);

[0016] mGpVar=cGpVar*mGpVar / (cGpVar+mGpVar);

[0017] Otherwise, if the uncertainty of the reconstruction point is less than the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library, the reconstruction point and uncertainty corresponding to the voxel unit in the voxel unit history library are updated using the following formula:

[0018] mGpP=cGpP;

[0019] mGpVar=cGpVar;

[0020] Among them, cGpP represents the reconstruction point of the voxel unit at the current moment; cGpVar represents the uncertainty of the reconstruction point of the voxel unit at the current moment, mGpP represents the reconstruction point corresponding to the voxel unit in the voxel unit history library, and mGpVar represents the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library.

[0021] Based on the further improvement of the above method, the surface of the voxel unit is reconstructed by using Gaussian process estimation based on the three-dimensional points of the voxel unit to obtain the reconstruction result of the voxel unit at the current moment, including:

[0022] Determining the surface reconstruction direction of the voxel unit based on principal component analysis;

[0023] For each surface reconstruction direction, a Gaussian process is used to estimate the reconstruction points and the uncertainty of each reconstruction point based on the three-dimensional points of the voxel unit to obtain a reconstruction result of the surface reconstruction direction.

[0024] Based on the further improvement of the above method, the surface reconstruction direction of the voxel unit is determined in the following manner based on principal component analysis:

[0025] Performing principal component analysis on all three-dimensional points in the voxel unit to obtain corresponding eigenvalues ​​and eigenvectors;

[0026] Calculate the angles between the eigenvector corresponding to the minimum eigenvalue and the three coordinate axes and sort them from small to large to obtain the first angle, the second angle and the third angle;

[0027] If the difference between the second angle and the first angle is greater than or equal to the second threshold, the coordinate axis direction corresponding to the first angle is the surface reconstruction direction;

[0028] If the difference between the second angle and the first angle is less than the second threshold, and the difference between the third angle and the second angle is greater than or equal to the third threshold, the coordinate axis directions corresponding to the first angle and the second angle are both surface reconstruction directions;

[0029] If the difference between the second angle and the first angle is smaller than the second threshold, and the difference between the third angle and the second angle is smaller than the third threshold, the directions of the three coordinate axes are all surface reconstruction directions.

[0030] Based on the further improvement of the above method, for each surface reconstruction direction, the reconstruction point and the uncertainty of each reconstruction point are estimated by using Gaussian process based on the three-dimensional points of the voxel unit, including:

[0031] Meshing the zero plane in the surface reconstruction direction to obtain grid points;

[0032] The three-dimensional point is used as the observation point, and based on the value of the observation point in the surface reconstruction direction, the numerical value and uncertainty of the grid point in the surface reconstruction direction are estimated using a Gaussian process to obtain the reconstruction point and the uncertainty of each reconstruction point.

[0033] Based on the further improvement of the above method, for each surface reconstruction direction, the reconstruction point and the uncertainty of each reconstruction point are estimated by using Gaussian process based on the three-dimensional points of the voxel unit, including:

[0034] Meshing the zero plane in the surface reconstruction direction to obtain grid points;

[0035] Projecting the three-dimensional point onto the zero plane of the surface reconstruction direction to obtain a projection point;

[0036] For each grid point, find the nearest projection point, and add the three-dimensional point corresponding to the nearest projection point to the observation point set;

[0037] Taking the three-dimensional points in the observation point set as observation points, the numerical values ​​and uncertainties of the grid points in the surface reconstruction direction are obtained by using Gaussian process estimation based on the values ​​of the observation points in the surface reconstruction direction, and the reconstruction points and the uncertainty of each reconstruction point are obtained.

[0038] Based on a further improvement of the above method, the value and uncertainty of the grid point in the surface reconstruction direction are obtained by using the following formula based on the value of the observation point in the surface reconstruction direction:

[0039]

[0040] Among them, Pred m×1 The numerical vector representing the grid point in the direction of surface reconstruction, Var m×1 Represents the uncertainty vector of the value of the grid point in the surface reconstruction direction, m represents the number of grid points, n represents the number of observation points, obs n×1 represents the numerical vector of the observation point in the direction of surface reconstruction, noise represents the noise of the observation point, K represents the kernel function matrix of Gaussian process estimation, I represents the identity matrix, and diag(·) represents taking the diagonal elements of the matrix.

[0041] Based on the further improvement of the above method, the voxel unit to which each 3D point in the point cloud data belongs is determined in the following way:

[0042] If the point cloud data at the current moment is not in the world coordinate system, the point cloud data at the current moment is converted to the world coordinate system to obtain the point cloud data at the current moment in the world coordinate system;

[0043] The number of the voxel unit to which each three-dimensional point belongs is calculated according to the coordinates of each three-dimensional point of the point cloud data at the current moment in the world coordinate system and the side length of the voxel unit.

[0044] Based on the further improvement of the above method, the type of the voxel unit is determined in the following manner:

[0045] Performing principal component analysis on all three-dimensional points in the voxel unit to obtain corresponding eigenvalues;

[0046] If the ratio of the maximum eigenvalue to the second largest eigenvalue is greater than or equal to a first threshold, the type of the voxel unit is determined to be non-surface; otherwise, the type of the voxel unit is surface.

[0047] Compared with the prior art, the present invention obtains the point cloud data of the current moment at each moment, determines the voxel unit to which each three-dimensional point belongs, and thereby divides the point cloud data of the current moment into voxel units. These voxel units are independent of each other and can be processed in parallel, thereby improving the updating efficiency; for each voxel unit, if it is judged that its type is a surface type, the surface is reconstructed, while the non-surface type voxel units are not reconstructed, thereby further simplifying the processing process and improving the efficiency; for each voxel unit, if it does not exist in the history library, it is directly added to the history library for updating; if the voxel unit exists in the history library, the voxel unit history library is updated based on the reconstruction result of the voxel unit at the current moment, thereby quickly updating the grid map.

[0048] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The accompanying drawings are only used for the purpose of illustrating specific embodiments and are not to be considered as limiting the present invention. In the entire drawings, the same reference symbols represent the same components;

[0050] Figure 1 A flowchart of a real-time grid map method according to an embodiment of the present invention;

[0051] Figure 2 A schematic diagram of zero plane mesh division according to an embodiment of the present invention;

[0052] Figure 3 It is a schematic diagram of projection of a three-dimensional point on the zero plane according to an embodiment of the present invention;

[0053] Figure 4 A triangulated schematic diagram of an embodiment of the present invention;

[0054] Figure 5 Schematic diagram of triangulated connections between adjacent voxels according to an embodiment of the present invention. DETAILED DESCRIPTION

[0055] The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0056] A specific embodiment of the present invention discloses a real-time grid map updating method, such as Figure 1 As shown, the following steps are included:

[0057] S1, obtaining the point cloud data at the current moment, and determining the voxel unit to which each three-dimensional point in the point cloud data belongs;

[0058] S2. For each voxel unit corresponding to the point cloud data at the current moment, if it is determined that the type of the voxel unit is a surface, a surface reconstruction is performed on the voxel unit using Gaussian process estimation based on the three-dimensional points of the voxel unit to obtain a reconstruction result of the voxel unit at the current moment;

[0059] S3. For each voxel unit corresponding to the point cloud data at the current moment, if the voxel unit does not exist in the voxel unit history library, the reconstruction result of the voxel unit at the current moment is added to the voxel unit history library; otherwise, the voxel unit history library is updated based on the reconstruction result of the voxel unit at the current moment.

[0060] It should be noted that the voxel unit history library is empty at the beginning. During implementation, the voxel unit history library is used to store the latest reconstruction results of the voxel units of the grid map. The grid map is updated by updating the voxel unit history library. The spatial calculation modules such as map rendering, collision detection, and illumination estimation can directly read the latest grid map reconstruction results from the voxel unit history library to quickly and accurately perform the corresponding calculation tasks.

[0061] Compared with the prior art, the real-time grid map updating method provided in this embodiment obtains the point cloud data at the current moment at each moment, determines the voxel unit to which each three-dimensional point belongs, and thus divides the point cloud data at the current moment into voxel units. These voxel units are independent of each other and can be processed in parallel, thereby improving the efficiency of the update; for each voxel unit, if it is determined that its type is a surface type, the surface is reconstructed, and for non-surface type voxel units, no reconstruction is performed, thereby further simplifying the processing process and improving efficiency; for each voxel unit, if it does not exist in the history library, it is directly added to the history library for updating, and if the voxel unit exists in the history library, the voxel unit history library is updated based on the reconstruction result of the voxel unit at the current moment, thereby quickly updating the grid map. The real-time grid map updating method of the present invention has low time consumption and high efficiency, and can realize real-time update of grid maps on devices such as XR devices that have high requirements for real-time performance and storage space.

[0062] The application scenarios of the present invention are not limited to XR devices. Any device with a device capable of performing position and posture estimation and a depth image sensor for collecting depth data can apply the method of the present invention to perform real-time grid map updates.

[0063] During implementation, the point cloud data obtained at each moment corresponds to the timestamp t.

[0064] During implementation, in order to reduce the influence of noise, a voxel filtering algorithm may be used to filter the point cloud data at time t. For example, the pclVoxelFilter method of the PCL library may be used for filtering.

[0065] Specifically, the voxel unit to which each 3D point in the point cloud data belongs is determined in the following manner:

[0066] S11. If the point cloud data at the current moment is not in the world coordinate system, convert the point cloud data at the current moment into the world coordinate system to obtain the point cloud data at the current moment in the world coordinate system;

[0067] S12. Calculate the number of the voxel unit to which each three-dimensional point belongs according to the coordinates of each three-dimensional point of the point cloud data at the current moment in the world coordinate system and the side length of the voxel unit.

[0068] If the acquired point cloud data is depth data in the depth sensor coordinate system, not in the world coordinate system, it needs to be converted to the world coordinate system in order to accurately determine the voxel unit to which the three-dimensional point belongs.

[0069] Specifically, the point cloud data at the current moment is converted to the world coordinate system using the following formula to obtain the point cloud data at the current moment in the world coordinate system:

[0070] WDt=T wb *T BD *Dt

[0071] Among them, WDt represents the point cloud data at the current moment in the world coordinate system, T wb Indicates the position data of the positioning device in the world coordinate system at the current moment, T BD It represents the pose parameters of the positioning device to the depth sensor, and Dt represents the point cloud data of the depth sensor at the current moment.

[0072] The point cloud data WDt at the current time t converted to the world coordinate system is divided into voxel cells Cell with corresponding numbers Cid. Each Cell is independent of each other, so that surface reconstruction can be performed in parallel.

[0073] It should be noted that the voxel unit is a cube and its number is globally unique. For any three-dimensional point, its voxel unit can be uniquely determined based on its coordinates.

[0074] Specifically, the number of the voxel unit to which each 3D point belongs is calculated using the following formula:

[0075]

[0076] Among them, WDt min =(x min ,y min ,z min ) represents the minimum coordinate value of the point cloud data at the current moment, WDt min =(x max ,y max ,z max ) represents the maximum coordinate value of the point cloud data at the current moment, Gs represents the side length of the voxel unit, (x c0 ,y c0 ,z c0 ) represents the starting coordinate point of the starting voxel unit of the point cloud data at the current moment, ng x Indicates the number of voxel units in the X-axis direction of the point cloud data at the current moment, ng y Indicates the number of voxel units in the Y-axis direction of the point cloud data at the current moment, ng z Indicates the number of voxel units in the Z-axis direction of the point cloud data at the current moment, Cid indicates the number of the voxel unit, floor(·) indicates rounding down, ceil(·) indicates rounding up, and (x, y, z) indicates the coordinates of the three-dimensional point.

[0077] It should be noted that WDt min Represents the minimum value of all 3D points on the X-axis, Y-axis, and Z-axis of the point cloud data (in the world coordinate system) at the current moment, WDt max Represents the maximum value of all 3D points in the Z axis, Y axis, and Z axis of the point cloud data at the current moment.

[0078] During implementation, the side length of the voxel unit may be 1.2 m.

[0079] For each voxel unit corresponding to the point cloud data at the current moment, its type is first determined, and the types are divided into two types: surface and non-surface. Since the point cloud data at the current moment is the points obtained by scanning the depth sensor at the current moment, these points are usually surface points; however, due to the existence of slender objects or noise in the depth sensor measurement, there will be non-surface voxel units, usually straight lines. After principal component analysis, for the straight line type, its first eigenvalue will be much larger than the other two eigenvalues. Therefore, the type of voxel unit can be determined by the eigenvalue, and only the three-dimensional points in the voxel unit of the surface type need to be reconstructed, thereby simplifying the processing and improving efficiency. For non-surfaces, surface reconstruction is not required at the current moment, and the depth sensor will rescan and measure at subsequent moments until the voxel unit meets the surface judgment condition, that is, it is judged to be a surface type before reconstruction is performed, thereby improving the accuracy of reconstruction.

[0080] Specifically, the type of voxel unit is determined in the following manner:

[0081] S21, performing principal component analysis on all three-dimensional points in the voxel unit to obtain corresponding eigenvalues;

[0082] S22. If the ratio of the maximum eigenvalue to the second largest eigenvalue is greater than or equal to a first threshold, determine that the type of the voxel unit is non-surface; otherwise, the type of the voxel unit is surface.

[0083] During implementation, principal component analysis is performed on all three-dimensional points in the extracted voxel unit. Since each data point has three dimensions, the eigenvalues ​​l1, l2, l3 and eigenvectors vec1, vec2, vec3 corresponding to the first, second, and third principal components are obtained through principal component analysis. From the characteristics of the PCA analysis algorithm, it can be seen that l1>l2>l3.

[0084] If the ratio of the maximum eigenvalue l1 to the second largest eigenvalue l2 is greater than or equal to the first threshold, the voxel unit is a non-surface type, otherwise, the voxel unit is a surface type. In implementation, the first threshold can be set to 50.

[0085] According to the characteristics of principal component analysis, the variance of the three-dimensional point cloud in the direction of the first eigenvector vec1 is the largest, representing the main trend of the data, the variance in the direction of the second eigenvector vec1 is the second largest, and the variance in the direction of the third eigenvector vec1 is the smallest. Therefore, the angle between the eigenvector vec3 corresponding to the minimum eigenvalue and the X, Y, and Z axes is calculated. The smaller the angle, the larger the projection area perpendicular to the axis, so that the surface reconstruction direction can be accurately determined.

[0086] For a voxel unit of surface type, the surface of the voxel unit is reconstructed by using Gaussian process estimation based on the three-dimensional points of the voxel unit to obtain the reconstruction result of the voxel unit at the current moment, specifically including:

[0087] S23, determining the surface reconstruction direction of the voxel unit based on principal component analysis;

[0088] S24. For each surface reconstruction direction, a Gaussian process is used to estimate the reconstruction points and the uncertainty of each reconstruction point based on the three-dimensional points of the voxel unit to obtain a reconstruction result of the surface reconstruction direction.

[0089] Before performing surface reconstruction, it is necessary to determine the surface reconstruction direction. Specifically, the surface reconstruction direction of the voxel unit is determined in the following manner:

[0090] S231, performing principal component analysis on all three-dimensional points in the voxel unit to obtain corresponding eigenvalues ​​and eigenvectors;

[0091] S232, calculating the angles between the eigenvector corresponding to the minimum eigenvalue and the three coordinate axes and sorting them from small to large to obtain a first angle, a second angle, and a third angle;

[0092] S233: If the difference between the second angle and the first angle is greater than or equal to a second threshold, the coordinate axis direction corresponding to the first angle is the surface reconstruction direction;

[0093] If the difference between the second angle and the first angle is less than the second threshold, and the difference between the third angle and the second angle is greater than or equal to the third threshold, the coordinate axis directions corresponding to the first angle and the second angle are both surface reconstruction directions;

[0094] If the difference between the second angle and the first angle is smaller than the second threshold, and the difference between the third angle and the second angle is smaller than the third threshold, the directions of the three coordinate axes are all surface reconstruction directions.

[0095] During implementation, the process of performing principal component analysis in step S231 may refer to step S21.

[0096] Calculate the angles between the third principal component (i.e., the smallest eigenvalue) eigenvector vec3 and the X, Y, and Z coordinate axes and sort the angles in ascending order to obtain the first angle angle_1, the second angle angle_2, and the third angle angle_3.

[0097] If angle_2-angle_1 is greater than or equal to the second threshold, the coordinate axis direction corresponding to angle_1 is the surface reconstruction direction. During implementation, the second threshold may be set to 0.75.

[0098] If angle_2-angle_1 is less than the second threshold, and angle_3-angle_2 is greater than or equal to the third threshold, the coordinate axis directions corresponding to angle_1 and angle_2 are both surface reconstruction directions. During implementation, the third threshold may be set to 0.25.

[0099] If angle_2-angle_1 is less than the second threshold, and angle_3-angle_2 is less than the third threshold, the coordinate axis directions corresponding to angle_1, angle_2 and angle_3 are all surface reconstruction directions, that is, the three coordinate axis directions are all surface reconstruction directions.

[0100] For a surface type, not every direction needs to be reconstructed. By determining the direction in which the surface reconstruction is required, the processing is further simplified to improve efficiency.

[0101] After the surface reconstruction direction is determined, for each surface reconstruction direction, the reconstruction point and the uncertainty of each reconstruction point are estimated using a Gaussian process based on the three-dimensional points of the voxel unit. Specifically, it includes:

[0102] S2411, meshing the zero plane in the surface reconstruction direction to obtain grid points;

[0103] S2412. Taking the three-dimensional point as the observation point, and using Gaussian process estimation based on the value of the observation point in the surface reconstruction direction to obtain the numerical value and uncertainty of the grid point in the surface reconstruction direction, and obtain the reconstruction point and the uncertainty of each reconstruction point.

[0104] During implementation, Gaussian process estimation is performed on each reconstruction direction of the voxel unit in turn to obtain reconstruction points, thereby performing surface reconstruction.

[0105] For example, if the surface reconstruction direction is the X-axis direction, the corresponding zero plane is the YOZ plane; if the surface reconstruction direction is the Y-axis direction, the corresponding zero plane is the XOZ plane; if the surface reconstruction direction is the Z-axis direction, the corresponding zero plane is the XOY plane.

[0106] During implementation, the zero plane in the surface reconstruction direction is first meshed to obtain grid points. For example, if the surface reconstruction direction is the X-axis direction, only the YOZ plane is meshed to obtain grid points, such as Figure 2 As shown, the Y axis and Z axis of the area corresponding to the voxel unit in the YOZ plane are equally divided into num_test parts, so num_test*num_test two-dimensional grid points are formed on the YOZ plane. For example, if num_test is 6, there are 36 two-dimensional grid points on the YOZ plane. Then, the three-dimensional points in the voxel unit are used as observation points, and the values ​​of the two-dimensional grid points in the X-axis direction and the uncertainty are estimated by using Gaussian process based on the values ​​of the observation points in the X-axis direction. The two-dimensional grid points and their estimated values ​​on the X-axis constitute reconstruction points, that is, num_test*num_test reconstruction points and the corresponding uncertainties are obtained.

[0107] During implementation, in order to further improve the computational efficiency, for each surface reconstruction direction, when the reconstruction points are obtained by using Gaussian process estimation based on the three-dimensional points of the voxel unit, the three-dimensional grid points can be downsampled, thereby reducing the computational scale. Specifically, it includes:

[0108] S2421, meshing the zero plane in the surface reconstruction direction to obtain grid points;

[0109] S2422, projecting the three-dimensional point onto the zero plane in the surface reconstruction direction to obtain a projection point;

[0110] S2423. For each grid point, find the nearest projection point, and add the three-dimensional point corresponding to the nearest projection point to the observation point set;

[0111] S2424. Take the three-dimensional points in the observation point set as observation points, and use Gaussian process estimation based on the values ​​of the observation points in the surface reconstruction direction to obtain the numerical values ​​and uncertainties of the grid points in the surface reconstruction direction, and obtain the reconstruction points and the uncertainties of each reconstruction point.

[0112] Step S2421 is the same as step S2411 and will not be repeated here.

[0113] Project all the 3D points in the voxel unit onto the zero plane in the surface reconstruction direction. For example, if the surface reconstruction direction is the X-axis direction, the projection points obtained after the 3D points are projected onto the YOZ plane are as follows: Figure 3 As shown, Figure 3 The solid and hollow points in are projection points.

[0114] Search for the nearest projection point for each grid point in turn. If a projection point is the nearest point to a grid point, add the 3D point corresponding to the projection point to the observation point set. Figure 3 As shown in the figure, the solid points are the nearest points of a grid point, and the hollow points are not the nearest points of any grid point. Only the 3D points in the observation point set are used as observation points to estimate the reconstruction points, thereby downsampling the 3D points and improving the computational efficiency.

[0115] Based on the observation points, Gaussian process estimation is used to predict the value of the grid points in the direction of surface reconstruction.

[0116] Specifically, based on the value of the observation point in the surface reconstruction direction, the following formula is used to obtain the value and uncertainty of the grid point in the surface reconstruction direction:

[0117]

[0118] Among them, Pred m×1 The numerical vector representing the grid point in the direction of surface reconstruction, Var m×1 Represents the uncertainty vector of the value of the grid point in the surface reconstruction direction, m represents the number of grid points, n represents the number of observation points, obs n×1 represents the numerical vector of the observation point in the direction of surface reconstruction, noise represents the noise of the observation point, K represents the kernel function matrix of Gaussian process estimation, I represents the identity matrix, and diag(·) represents taking the diagonal elements of the matrix.

[0119] During implementation, the noise can be set to 0.02. The kernel function used can be k(i,j)=exp(-m1*|ij|), where m1 is the kernel function parameter and k(i,j) represents the value of the i-th row and j-th column of the kernel function matrix. n×n and K n×m The matrices are of different sizes.

[0120] Since the value of the zero plane is known, only the value of the surface reconstruction direction is predicted, thus saving calculation time and improving reconstruction efficiency.

[0121] After predicting the value of each grid point in the surface reconstruction direction, the two-dimensional grid point and its surface reconstruction direction value are combined to form a three-dimensional reconstruction point. The uncertainty of the value of the grid point in the surface reconstruction direction is the uncertainty of the corresponding three-dimensional reconstruction point. For each surface reconstruction direction, the three-dimensional point based on the voxel unit is estimated using a Gaussian process to obtain the reconstruction point, and then triangulation is performed based on the reconstruction point, so that the triangulated connection relationship can be quickly obtained without a lot of calculations, saving triangulation time.

[0122] After obtaining the reconstruction points, the reconstruction points are triangulated to obtain triangular facets in the surface reconstruction direction, which specifically includes:

[0123] Each grid point is traversed in turn. If there is a diagonal grid point for the current grid point, the reconstruction points corresponding to the current grid point, the grid points adjacent to the current grid point, and the diagonal grid points of the current grid point form a triangular mesh.

[0124] During triangulation, the grid points of the zero plane are used to determine the reconstruction points that need to be connected. Figure 4 As shown, each grid point is traversed from left to right and from top to bottom. For example, the first grid point A in the upper left corner has a diagonal grid point D (a grid point in the lower right corner), then the triple formed by the reconstruction points corresponding to the grid point A, the adjacent grid point B, and the diagonal grid point D is a triangular face, and the triple formed by the reconstruction points corresponding to the grid point A, the adjacent grid point C, and the diagonal grid point D is another triangular face. If the traversal order is from right to left and from top to bottom, the diagonal grid is the grid point to the lower left of the current grid point, and so on for other traversal orders.

[0125] Compared with the existing triangulation methods that are time-consuming, the triangulation strategy of the present invention can directly determine the connection relationship of the three-dimensional reconstruction points in the triangulation process through the zero-plane grid relationship without any calculation conditions, thus saving the triangulation time. Moreover, the zero-plane two-dimensional grid is evenly distributed, thereby completely avoiding the problem of narrow and long triangular faces appearing in the triangulation process, which affects the spatial calculation of the application scene.

[0126] It should be noted that adjacent voxel units in the same surface reconstruction direction share grid points, and the reconstruction points corresponding to the shared grid points in the two voxel units may have slight differences in the surface reconstruction direction. Therefore, they need to be modified to a unified value, such as taking the value of one of the voxel units as the final value, or taking the average value as the final value. Figure 5As shown in the figure, the grid point E of the left voxel unit and the grid point F of the right voxel unit are actually one grid point, and the corresponding reconstruction point should also be the same point. However, since the reconstruction is performed in different voxel units, the values ​​in the surface reconstruction direction may be slightly different, so they need to be unified into one point. During implementation, the coordinate value of the reconstruction point corresponding to point F can be modified to the value of the reconstruction point corresponding to point E, and the coordinate value of the reconstruction point corresponding to point H can be modified to the value of the reconstruction point corresponding to point G, so as to unify them; the coordinate value of the reconstruction point corresponding to point E can also be modified to the value of the reconstruction point corresponding to point F, and the coordinate value of the reconstruction point corresponding to point G can be modified to the value of the reconstruction point corresponding to point H, so as to unify them; the mean value of the reconstruction point corresponding to point E and the reconstruction point corresponding to point F can also be taken as the value of the reconstruction point corresponding to point E and the reconstruction point corresponding to point F, and the mean value of the reconstruction point corresponding to point G and the reconstruction point corresponding to point H can be taken as the value of the reconstruction point corresponding to point G and the reconstruction point corresponding to point H, so as to unify them. .

[0127] After the surface type voxel unit is reconstructed, for each voxel unit corresponding to the point cloud data at the current moment, its information in the voxel unit history library is updated.

[0128] During implementation, the voxel unit history library stores voxel unit information through a dictionary Map. The key of the dictionary Map is Cid, which is the number of the voxel unit, and the value is a CellMap structure. The CellMap structure is defined as follows:

[0129] struct CellMap{

[0130] std::uint64_t timestamp;

[0131] std::vector <eigen::vector3d>P;

[0132] std::vector <eigen::vector3d>GpP[3];

[0133] std::vector <double>GpVar[3];

[0134] bool b_surface;

[0135] bool reconstruct_dir[3];

[0136] bool b_reconstruct;

[0137] }

[0138] Where timestamp is the timestamp corresponding to the current depth data; P is an array used to store the 3D points belonging to the Cell; GpP is a 3D array used to store the 3D point list predicted by Gaussian process estimation in the three surface reconstruction directions of X, Y, and Z; GpVar is a 3D array used to store the variance value, i.e., uncertainty, of the 3D points predicted by Gaussian process estimation in the three surface reconstruction directions of X, Y, and Z. The variance value is used to describe the uncertainty of the 3D points estimated by Gaussian process. The smaller the value, the lower the uncertainty of the 3D points estimated by Gaussian process; b_surface represents whether the Cell is a surface, and the default value is false; reconstruct_dir[0], reconstruct_dir[1], reconstruct_dir[2] represent whether the surface reconstruction is performed in the X-axis direction, Y-axis direction, and Z-axis direction, respectively, and the default values ​​are all false; b_reconstruct represents whether the Cell has been reconstructed on the surface, and the default value is false.

[0139] During implementation, each voxel unit corresponding to the point cloud data at the current moment is traversed in turn. If the voxel unit does not exist in the voxel unit history library, it means that the voxel unit is a newly scanned voxel unit. Therefore, it is directly added to the voxel unit history library, and the CellMap structure corresponding to the voxel unit is constructed. The number Cid of the voxel unit is used as the key, and CellMap is added to the dictionary Map as the Value. If the voxel unit is the number of the voxel unit in the library, the information in the voxel unit history library needs to be updated based on the 3D point or reconstruction result of the voxel unit at the current moment.

[0140] Specifically, the reconstruction result of the voxel unit includes a surface reconstruction direction, a reconstruction point in each surface reconstruction direction, and an uncertainty of each reconstruction point.

[0141] Specifically, in step S3, the voxel unit history library is updated based on the three-dimensional point or reconstruction result of the voxel unit at the current moment, including:

[0142] If the voxel unit in the voxel unit history library has not been reconstructed, the three-dimensional point of the voxel unit in the voxel unit history library and the three-dimensional point of the voxel unit at the current moment are merged, and based on the merged three-dimensional point, it is determined whether the voxel unit is a surface type, and if so, the voxel unit is reconstructed by using Gaussian process estimation to obtain the reconstruction result of the voxel unit, and the voxel unit in the voxel unit history library is updated;

[0143] If the voxel unit in the voxel unit history library has undergone surface reconstruction, the voxel unit in the voxel unit history library is updated according to the uncertainty of the reconstruction result at the current moment.

[0144] Specifically, if a voxel unit is recorded in the voxel unit history library as not having undergone surface reconstruction, it means that the voxel unit was judged as a non-surface type at the historical moment and did not meet the surface conditions, so no surface reconstruction was performed. The three-dimensional point of the voxel unit in the voxel unit history library and the three-dimensional point of the voxel unit at the current moment are merged, that is, the three-dimensional point of the voxel unit at the current moment is added to the three-dimensional point array of the CellMap structure of the voxel unit in the voxel unit history library. The merged three-dimensional point is used as the three-dimensional point of the voxel unit. According to steps S21 to S22, it is judged again whether the voxel unit is a surface type. If it is not a surface type, the timestamp is updated to the current moment, and the update of the voxel unit is completed. If it is a surface type, the surface reconstruction direction is determined according to steps S23 to S25 and reconstruction is performed in the surface reconstruction direction. The CellMap structure of the voxel unit in the voxel unit history library is updated according to the reconstruction result, that is, the timestamp is updated to the current moment, the reconstruction point and the corresponding uncertainty are put into GpP and GpVar, b_surface is set to true, the corresponding surface reconstruction direction in reconstruct_dir is set to true, b_reconstruct is set to true, and the update of the voxel unit is completed.

[0145] If the voxel unit has undergone surface reconstruction in the voxel unit history library, that is, the surface has been reconstructed at a historical moment, the information in the voxel unit history library is updated according to the uncertainty of the reconstruction result at the current moment.

[0146] Specifically, updating the voxel unit in the voxel unit history library according to the uncertainty of the reconstruction result at the current moment includes:

[0147] Each reconstruction point in each surface reconstruction direction in the reconstruction result of the voxel unit at the current moment is traversed in turn. If the uncertainty of the reconstruction point and the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library are both less than the fourth threshold, the reconstruction point and uncertainty corresponding to the voxel unit in the voxel unit history library are updated using the following formula:

[0148] mGpP=(cGpP*mGpVar+mGpP*cGpVar) / (cGpVar+mGpVar);

[0149] mGpVar=cGpVar*mGpVar / (cGpVar+mGpVar);

[0150] If the uncertainty of the reconstruction point and / or the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library is greater than or equal to the fourth threshold, the reconstruction point and uncertainty corresponding to the voxel unit in the voxel unit history library are updated using the following formula:

[0151] mGpP=cGpP;

[0152] mGpVar=cGpVar;

[0153] Among them, cGpP represents the reconstruction point of the voxel unit at the current moment; cGpVar represents the uncertainty of the reconstruction point of the voxel unit at the current moment, mGpP represents the reconstruction point corresponding to the voxel unit in the voxel unit history library, and mGpVar represents the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library.

[0154] During implementation, the fourth threshold may be set to 0.5.

[0155] It should be noted that, according to the reconstruction process in step S2, for a voxel unit, the reconstruction points at each moment in each reconstruction direction are one-to-one corresponding, so each reconstruction point in the reconstruction result at the current moment is traversed in turn, if the uncertainty of the reconstruction point at the current moment is less than the fourth threshold, and the uncertainty of the corresponding reconstruction point in the voxel unit history library is also less than the fourth threshold, the smaller the uncertainty, the higher the credibility, so the reconstruction point in the history library and the reconstruction point at the current moment are merged to update the reconstruction point. If one of the uncertainty of the reconstruction point at the current moment and the uncertainty of the corresponding reconstruction point in the voxel unit history library is greater than or equal to the fourth threshold, then if the uncertainty of the reconstruction point at the current moment is less than the uncertainty of the corresponding reconstruction point in the voxel unit history library, the reconstruction point at the current moment is assigned to the corresponding reconstruction point in the history library, thereby updating the voxel unit history library, and if the uncertainty of the reconstruction point at the current moment is greater than or equal to the uncertainty of the corresponding reconstruction point in the voxel unit history library, the corresponding reconstruction point in the voxel unit history library is not updated.

[0156] After traversing all reconstruction points, the voxel unit history library is updated.

[0157] During implementation, the voxel unit in the current voxel unit history library is detected at each moment. If the number of three-dimensional points of the voxel unit is less than the set fifth threshold value P_num_thresh, and the storage time delta_t of the voxel unit is greater than the set sixth threshold value exist_t, the voxel unit is deleted from the dictionary Map, so as to facilitate the management of the voxel unit. Exemplarily, the fifth threshold value P_num_thresh is equal to 5, and the sixth threshold value exist_t is equal to 60s.

[0158] The real-time grid map updating method of the present invention can query and update the grid map quickly and efficiently, meeting the real-time requirements. The real-time grid map updating method of the present invention can be executed on a dedicated chip or a graphics processor to improve processing efficiency.

[0159] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0160] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.< / double>

Claims

1. A real-time grid map updating method, characterized in that: The following steps are involved: Obtain the point cloud data at the current moment, and determine the voxel unit to which each three-dimensional point in the point cloud data belongs; For each voxel unit corresponding to the point cloud data at the current moment, if it is determined that the type of the voxel unit is a surface, a surface reconstruction is performed on the voxel unit using Gaussian process estimation based on the three-dimensional points of the voxel unit to obtain a reconstruction result of the voxel unit at the current moment; For each voxel unit corresponding to the point cloud data at the current moment, if the voxel unit does not exist in the voxel unit history library, the voxel unit is added to the voxel unit history library; otherwise, the voxel unit history library is updated based on the three-dimensional point or reconstruction result of the voxel unit at the current moment; Based on the three-dimensional points of the voxel unit, the surface of the voxel unit is reconstructed by using Gaussian process estimation to obtain the reconstruction result of the voxel unit at the current moment, including: Determining the surface reconstruction direction of the voxel unit based on principal component analysis; For each surface reconstruction direction, a reconstruction point and the uncertainty of each reconstruction point are estimated by using a Gaussian process based on the three-dimensional points of the voxel unit to obtain a reconstruction result of the surface reconstruction direction; The surface reconstruction direction of the voxel unit is determined based on principal component analysis in the following manner: Performing principal component analysis on all three-dimensional points in the voxel unit to obtain corresponding eigenvalues ​​and eigenvectors; Calculate the angles between the eigenvector corresponding to the minimum eigenvalue and the three coordinate axes and sort them from small to large to obtain the first angle, the second angle and the third angle; If the difference between the second angle and the first angle is greater than or equal to the second threshold, the coordinate axis direction corresponding to the first angle is the surface reconstruction direction; If the difference between the second angle and the first angle is less than the second threshold, and the difference between the third angle and the second angle is greater than or equal to the third threshold, the coordinate axis directions corresponding to the first angle and the second angle are both surface reconstruction directions; If the difference between the second angle and the first angle is smaller than the second threshold, and the difference between the third angle and the second angle is smaller than the third threshold, the directions of the three coordinate axes are all surface reconstruction directions.

2. The real-time grid map updating method according to claim 1, characterized in that: The reconstruction result includes a surface reconstruction direction, a reconstruction point in each surface reconstruction direction, and an uncertainty of each reconstruction point; The voxel unit history library is updated based on the three-dimensional point or reconstruction result of the voxel unit at the current moment, including: If the voxel unit in the voxel unit history library has not been reconstructed, the three-dimensional point of the voxel unit in the voxel unit history library and the three-dimensional point of the voxel unit at the current moment are merged, and based on the merged three-dimensional point, it is determined whether the voxel unit is a surface type, and if so, the voxel unit is reconstructed by using Gaussian process estimation to obtain the reconstruction result of the voxel unit, and the voxel unit in the voxel unit history library is updated; If the voxel unit in the voxel unit history library has undergone surface reconstruction, the voxel unit in the voxel unit history library is updated according to the uncertainty of the reconstruction result at the current moment.

3. The real-time grid map updating method according to claim 2, characterized in that: The voxel unit in the voxel unit history library is updated according to the uncertainty of the reconstruction result at the current moment, including: Each reconstruction point in each surface reconstruction direction in the reconstruction result of the voxel unit at the current moment is traversed in turn. If the uncertainty of the reconstruction point and the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library are both less than the fourth threshold, the reconstruction point and uncertainty corresponding to the voxel unit in the voxel unit history library are updated using the following formula: mGpP=(cGpP*mGpVar+mGpP*cGpVar) / (cGpVar+mGpVar); mGpVar=cGpVar*mGpVar / (cGpVar+mGpVar); Otherwise, if the uncertainty of the reconstruction point is less than the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library, the reconstruction point and uncertainty corresponding to the voxel unit in the voxel unit history library are updated using the following formula: mGpP=cGpP; mGpVar=cGpVar; Among them, cGpP represents the reconstruction point of the voxel unit at the current moment; cGpVar represents the uncertainty of the reconstruction point of the voxel unit at the current moment, mGpP represents the reconstruction point corresponding to the voxel unit in the voxel unit history library, and mGpVar represents the uncertainty of the reconstruction point corresponding to the voxel unit in the voxel unit history library.

4. The real-time grid map updating method according to claim 1, characterized in that: For each surface reconstruction direction, the reconstruction points and the uncertainty of each reconstruction point are estimated using a Gaussian process based on the three-dimensional points of the voxel unit, including: Meshing the zero plane in the surface reconstruction direction to obtain grid points; The three-dimensional point is used as the observation point, and based on the value of the observation point in the surface reconstruction direction, the numerical value and uncertainty of the grid point in the surface reconstruction direction are estimated using a Gaussian process to obtain the reconstruction point and the uncertainty of each reconstruction point.

5. The real-time grid map updating method according to claim 1, characterized in that: For each surface reconstruction direction, the reconstruction points and the uncertainty of each reconstruction point are estimated using a Gaussian process based on the three-dimensional points of the voxel unit, including: Meshing the zero plane in the surface reconstruction direction to obtain grid points; Projecting the three-dimensional point onto the zero plane of the surface reconstruction direction to obtain a projection point; For each grid point, find the nearest projection point, and add the three-dimensional point corresponding to the nearest projection point to the observation point set; Taking the three-dimensional points in the observation point set as observation points, the numerical values ​​and uncertainties of the grid points in the surface reconstruction direction are obtained by using Gaussian process estimation based on the values ​​of the observation points in the surface reconstruction direction, and the reconstruction points and the uncertainty of each reconstruction point are obtained.

6. The real-time grid map updating method according to claim 4 or 5, characterized in that: Based on the value of the observation point in the surface reconstruction direction, the following formula is used to obtain the value and uncertainty of the grid point in the surface reconstruction direction: Among them, Pred m×1 The numerical vector representing the grid point in the direction of surface reconstruction, Var m×1 Represents the uncertainty vector of the value of the grid point in the surface reconstruction direction, m represents the number of grid points, n represents the number of observation points, obs n×1 represents the numerical vector of the observation point in the direction of surface reconstruction, noise represents the noise of the observation point, K represents the kernel function matrix of Gaussian process estimation, I represents the identity matrix, and diag(·) represents taking the diagonal elements of the matrix.

7. The real-time grid map updating method according to claim 1, characterized in that: The voxel unit to which each 3D point in the point cloud data belongs is determined in the following way: If the point cloud data at the current moment is not in the world coordinate system, the point cloud data at the current moment is converted to the world coordinate system to obtain the point cloud data at the current moment in the world coordinate system; The number of the voxel unit to which each three-dimensional point belongs is calculated according to the coordinates of each three-dimensional point of the point cloud data at the current moment in the world coordinate system and the side length of the voxel unit.

8. The real-time grid map updating method according to claim 1, characterized in that: The type of the voxel unit is determined in the following manner: Performing principal component analysis on all three-dimensional points in the voxel unit to obtain corresponding eigenvalues; If the ratio of the maximum eigenvalue to the second largest eigenvalue is greater than or equal to a first threshold, the type of the voxel unit is determined to be non-surface; otherwise, the type of the voxel unit is surface.

Images