A real-time grid map reconstruction method

Abstract

The present invention relates to a real-time grid map reconstruction method, which belongs to the technical field of grid map reconstruction and solves the problem of high time consumption of grid map reconstruction 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 the type of the voxel unit is determined to be a surface, determining the surface reconstruction direction of the voxel unit; for each surface reconstruction direction, using Gaussian process estimation based on the three-dimensional points of the voxel unit to obtain a reconstruction point, and triangulating the reconstruction point to obtain a reconstruction result of the surface reconstruction direction. Efficient real-time grid map reconstruction is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of grid map reconstruction, and in particular to a real-time grid map reconstruction 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, it is necessary to build and update Mesh maps 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, and tactile interaction. However, the computing power and storage of XR devices are limited, which makes it impossible for existing mesh map reconstruction methods to 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 reconstruction method to solve the problem of high time consumption of the existing grid map reconstruction method.

[0005] On the one hand, an embodiment of the present invention provides a real-time grid map reconstruction 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, determining the surface reconstruction direction of the voxel unit;

[0008] For each surface reconstruction direction, a reconstruction point is estimated by using a Gaussian process based on the three-dimensional points of the voxel unit, and the reconstruction point is triangulated to obtain a reconstruction result of the surface reconstruction direction.

[0009] 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:

[0010] 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;

[0011] 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.

[0012] Based on the further improvement of the above method, the following formula is used to calculate the number of the voxel unit to which each 3D point belongs:

[0013]

[0014] 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 max =(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.

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

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

[0017] 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.

[0018] Based on the further improvement of the above method, the surface reconstruction direction of the voxel unit is determined in the following manner:

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

[0020] 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;

[0021] 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;

[0022] 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;

[0023] 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.

[0024] Based on the further improvement of the above method, for each surface reconstruction direction, the three-dimensional points of the voxel unit are estimated using a Gaussian process to obtain the reconstruction points, including:

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

[0026] The three-dimensional point is used as an observation point, and based on the value of the observation point in the surface reconstruction direction, a Gaussian process is used to estimate the value of the grid point in the surface reconstruction direction to obtain a reconstruction point.

[0027] Based on the further improvement of the above method, for each surface reconstruction direction, the three-dimensional points of the voxel unit are estimated using a Gaussian process to obtain the reconstruction points, including:

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

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

[0030] 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;

[0031] A three-dimensional point in the observation point set is used as an observation point, and based on the value of the observation point in the surface reconstruction direction, a Gaussian process is used to estimate the value of the grid point in the surface reconstruction direction to obtain a reconstruction point.

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

[0033]

[0034] Among them, Pred m×1 Represents the numerical vector of grid points in the direction of surface reconstruction, 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, and I represents the identity matrix.

[0035] Based on the further improvement of the above method, the reconstruction points are triangulated to obtain the reconstruction result of the surface reconstruction direction, including:

[0036] 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.

[0037] Based on the further improvement of the above method, the following formula is used to 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:

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

[0039] 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 from the positioning device to the depth sensor, and Dt represents the point cloud data of the depth sensor at the current moment.

[0040] 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 thus 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 efficiency of reconstruction; for each voxel unit, if it is determined that its type is a surface type, surface reconstruction is performed on it, and for non-surface type voxel units, reconstruction is not performed, thereby further simplifying the processing process and improving efficiency; for surface types, not every direction needs to be reconstructed, and by determining the direction in which surface reconstruction is required, the processing is further simplified to improve efficiency; for each surface reconstruction direction, a Gaussian process is used to estimate the three-dimensional points of the voxel unit to obtain a reconstruction point, and triangulation is performed based on the reconstruction point, so that the triangulation connection relationship can be quickly obtained without a large amount of calculation, saving the time of triangulation. The real-time grid map reconstruction method of the present invention has low time consumption and high efficiency, and can realize real-time reconstruction of grid maps on devices with high requirements for real-time performance and storage space.

[0041] 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

[0042] 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;

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

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

[0045] 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;

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

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

[0048] The preferred embodiments of the present invention are 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.

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

[0050] 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;

[0051] 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, determine the surface reconstruction direction of the voxel unit;

[0052] S3. For each surface reconstruction direction, a reconstruction point is obtained by estimating the three-dimensional point of the voxel unit using a Gaussian process; and the reconstruction point is triangulated to obtain a reconstruction result of the surface reconstruction direction.

[0053] Compared with the prior art, the real-time grid map reconstruction method provided by this embodiment obtains the point cloud data of the current moment at each moment, determines the voxel unit to which each three-dimensional point belongs, and thus 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 efficiency of reconstruction; for each voxel unit, if it is determined that its type is a surface type, surface reconstruction is performed on it, and for non-surface type voxel units, reconstruction is not performed, thereby further simplifying the processing process and improving efficiency; for surface types, not every direction needs to be reconstructed, and by determining the direction in which surface reconstruction is required, the processing is further simplified to improve efficiency; for each surface reconstruction direction, a Gaussian process is used to estimate the three-dimensional points of the voxel unit to obtain a reconstruction point, and triangulation is performed based on the reconstruction point, so that the triangulation connection relationship can be quickly obtained without a large amount of calculation, saving the time of triangulation. The real-time grid map reconstruction method of the present invention has low time consumption and high efficiency, and can realize real-time reconstruction of grid maps on devices such as XR devices that have high requirements for real-time performance and storage space. The application scenarios of the present invention are not limited to XR devices. Devices with a posture estimation unit that can perform posture and position estimation and a depth image sensor for depth data acquisition can all apply the method of the present invention to perform real-time grid map reconstruction.

[0054] During implementation, point cloud data is obtained at each moment, and the corresponding timestamp is t.

[0055] 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.

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

[0057] 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;

[0058] 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.

[0059] 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.

[0060] 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:

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

[0062] 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 from the positioning device to the depth sensor, and Dt represents the point cloud data of the depth sensor at the current moment.

[0063] 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.

[0064] 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.

[0065] Specifically, the following formula is used to calculate the number of the voxel unit to which each 3D point belongs:

[0066]

[0067] 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.

[0068] 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 maxRepresents 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.

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

[0070] 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.

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

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

[0073] 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.

[0074] 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.

[0075] 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.

[0076] 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.

[0077] For a voxel unit of surface type, 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:

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

[0079] 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;

[0080] 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;

[0081] 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;

[0082] 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.

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

[0084] 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.

[0085] 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.

[0086] 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.

[0087] 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.

[0088] After the surface reconstruction direction is determined, for each surface reconstruction direction, a reconstruction point is obtained by estimating the three-dimensional points of the voxel unit using a Gaussian process. Specifically, the method includes:

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

[0090] 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 value of the grid point in the surface reconstruction direction, to obtain a reconstruction point.

[0091] 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.

[0092] 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.

[0093] During implementation, the zero plane in the surface reconstruction direction is first meshed to obtain mesh points. For example, if the surface reconstruction direction is the X-axis direction, the YOZ plane is meshed to obtain mesh 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 are estimated using a 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 are obtained.

[0094] 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 may be downsampled, including:

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

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

[0097] 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;

[0098] S2424. Take a three-dimensional point in the observation point set as an observation point, and use Gaussian process estimation based on the value of the observation point in the surface reconstruction direction to obtain the value of the grid point in the surface reconstruction direction, so as to obtain a reconstruction point.

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

[0100] 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.

[0101] 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.

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

[0103] Specifically, the value of the grid point in the surface reconstruction direction is obtained by using the following formula based on the value of the observation point in the surface reconstruction direction:

[0104]

[0105] Among them, Pred m×1 Represents the numerical vector of grid points in the direction of surface reconstruction, m represents the number of grid points, n represents the number of observation points, obs n×1Represents 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, and I represents the unit matrix.

[0106] 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.

[0107] After predicting the value of each grid point in the surface reconstruction direction, the two-dimensional grid point and the value of its surface reconstruction direction are combined to form a three-dimensional reconstruction point.

[0108] 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.

[0109] After obtaining the reconstruction points, the reconstruction points are triangulated to obtain the reconstruction results of the surface reconstruction direction, including:

[0110] 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.

[0111] 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.

[0112] It should be noted that according to the zero plane grid division process of the surface reconstruction direction of the present invention, adjacent voxel units in the same surface reconstruction direction will share grid points, and the reconstruction points corresponding to the shared grid points of the two voxel units may have slight differences in the surface reconstruction direction. Therefore, they need to be modified to a unified value, for example, the value of one of the voxel units is taken as the final value, or the average value is taken as the final value. Figure 5As shown, 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; 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; 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.

[0113] 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.

[0114] During implementation, after acquiring the point cloud data at the current moment, the real-time grid map reconstruction method of the present invention can be executed on a dedicated chip or a graphics processor to improve processing efficiency.

[0115] 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.

[0116] 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.

Claims

1. A real-time grid map reconstruction 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, determining the surface reconstruction direction of the voxel unit; For each surface reconstruction direction, a reconstruction point is obtained by estimating the three-dimensional point of the voxel unit using a Gaussian process, and the reconstruction point is triangulated to obtain a reconstruction result of the surface reconstruction direction; The surface reconstruction direction 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 ​​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 reconstruction 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.

3. The real-time grid map reconstruction method according to claim 2, characterized in that: The following formula is used to calculate the number of the voxel unit to which each 3D point belongs: 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 max =(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.

4. The real-time grid map reconstruction 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.

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

6. The real-time grid map reconstruction method according to claim 1, characterized in that: For each surface reconstruction direction, a Gaussian process is used to estimate the three-dimensional points of the voxel unit to obtain a reconstruction point, 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; A three-dimensional point in the observation point set is used as an observation point, and based on the value of the observation point in the surface reconstruction direction, a Gaussian process is used to estimate the value of the grid point in the surface reconstruction direction to obtain a reconstruction point.

7. The real-time grid map reconstruction method according to claim 5 or 6, characterized in that: Based on the value of the observation point in the surface reconstruction direction, the value of the grid point in the surface reconstruction direction is obtained by using the following formula: Among them, Pred m×1 Represents the numerical vector of grid points in the direction of surface reconstruction, 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, and I represents the identity matrix.

8. The real-time grid map reconstruction method according to claim 5, characterized in that: The reconstruction points are triangulated to obtain the reconstruction results of the surface reconstruction direction, including: 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.

9. The real-time grid map reconstruction method according to claim 2, characterized in that: 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: WDt=T wb *T BD *Dt 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 from the positioning device to the depth sensor, and Dt represents the point cloud data of the depth sensor at the current moment.

Images