Three-dimensional scene modeling method based on octant-voxel compact map representation

Abstract

This invention discloses a 3D scene modeling method based on octet compact map representation. It uses LiDAR and an inertial measurement unit (IMU) for map construction, employing an octet compact map representation method. The method performs real-time, incremental fusion and noise reduction on the LiDAR point cloud, and efficiently associates data through a symmetry-accelerated priority voxel search strategy. Before the search, a predictive search warm-up mechanism based on motion consistency is introduced. Sub-maps are obtained from the constructed global map, and pixels are extracted from these sub-maps and encoded into joint descriptors. The queried sub-map is used as the query sub-map, and candidate joint descriptors for all candidate sub-maps are obtained based on the query sub-map. It is then determined whether the candidate sub-map is a valid loop closure detection result; if so, the global map is updated. This invention is less prone to mapping loss and can improve the stability and efficiency of mapping in large scenes.

Description

Technical Field

[0001] This invention belongs to the field of 3D modeling technology, and relates to a 3D scene modeling method based on octet compact map representation. Background Technology

[0002] Loop closure detection in 3D scene modeling is a core component of simultaneous localization and mapping (SLR) technology. It aims to identify revisited historical scenes by converting raw point clouds into highly discriminative feature descriptors, thereby correcting trajectory drift and map distortion caused by sensor noise and accumulated errors. However, the performance of a robust loop closure detection system highly depends on the quality of the real-time map and pose estimation provided by the front-end odometry. In practical systems, the front-end odometry faces two major engineering challenges: traditional map representation methods easily lead to uneven point cloud density and unlimited memory growth in complex large scenes; and nearest neighbor search in data association inherently contradicts efficiency and accuracy. These problems, along with inherent sensor noise, collectively cause the cumulative error in pose estimation to accumulate continuously, posing significant difficulties for accurate matching in back-end loop closure detection. Summary of the Invention

[0003] The purpose of this invention is to provide a 3D scene modeling method based on octet compact map representation. By using octet compact map representation and a priority voxel search strategy accelerated by symmetry, the method solves the problems of front-end map management and data association. Furthermore, it is deeply integrated with the binary and triangle joint descriptor loop closure detection mechanism, which is less prone to mapping loss and can improve the stability and efficiency of large-scale scene mapping based on large batches of LiDAR point cloud data.

[0004] The technical solution to achieve the purpose of this invention is as follows: A 3D scene modeling method based on octet compact map representation includes the following steps: A global map based on octet is constructed using lidar point cloud and inertial measurement unit data; The relative motion increment calculated by the inertial measurement unit is input into the iterative error state Kalman filter to obtain the system state. Based on the obtained system state, the corresponding nearest neighbor map point is searched in the global map based on octet according to the current laser point cloud. The distance from the laser point cloud to the corresponding map plane is calculated as the observation error. By minimizing the observation error of all laser point clouds, the optimal estimate of the system state is obtained. Based on the optimal estimate of the system state, the current frame laser point cloud is correlated with the global map based on octet voxels to obtain a fused global map. Obtain sub-maps from the obtained fused global map, and extract pixels from the sub-maps to encode them into joint descriptors; The query submap is obtained from the acquired submap. Loop closure detection is performed on the query submap based on the joint descriptor. The global map of the 3D scene is updated based on the loop closure detection results.

[0005] In the preferred technical solution, the constructed global map based on octet voxels is managed using a hash table, which divides the three-dimensional space into segments with side length r. v The basic voxel grid, each basic voxel V i Indexed by hash key k, each basic voxel is further subdivided into 8 sub-voxels V. {i,s} s=0,...,7, each sub-voxel V {i,s} It does not store any original point cloud data, but only maintains two sets of data: one is the three-dimensional representative point coordinates μ. s The other is that the record has been fused into μ. s The number of laser points n s ; During the lidar scanning update process, when a new lidar point p new When falling into a certain daughter voxel, it is related to the existing representative point μ of that daughter voxel. s The following rules apply to the merging and updating process:

[0006] Where N represents the number of times the representative point has been updated.

[0007] In the preferred technical solution, after obtaining the system state, the following is also included: The pose of the obtained system state is transformed from the lidar sensor coordinate system to the global world coordinate system; for a point located in the world coordinate system after the pose transformation... p w By discretizing the calculations to determine the coarse grid positions of the points, the sub-voxel index k is obtained. sub Obtain the sub-voxel index k sub Then, the coordinates of the base voxels are obtained from the coordinates of the sub-voxels.

[0008] In the preferred technical solution, the daughter voxel index k is obtained. sub Afterwards, the hash key k of the basic voxel V is determined by k sub By shifting one position to the right arithmetic right, we get k = k sub >>1, obtain the coordinates of the basic voxels, point p w The binary representation of the local sub-voxel number s within the basal voxel V is ( i %2, j %2, l %2), i , j , l These are the three-dimensional network coordinates of the daughter voxels.

[0009] In the preferred technical solution, a symmetry-accelerated priority voxel search strategy is used to search for the corresponding nearest neighbor map point in the octet-based global map. The symmetry-accelerated priority voxel search strategy includes: Utilizing the regular and uniform sub-voxel structure of the global map, a priority traversal list is pre-computed offline during the system initialization phase. This list defines the following: for a given central sub-voxel, when searching its K nearest neighbor representative points, all other sub-voxels are grouped and accessed in ascending order according to the geometric distance from their center point to the center of the central sub-voxel. Since the relative positions of the 8 child voxels within the base voxel are symmetrical, the priority traversal list of one child voxel type is calculated as the base list, and the priority traversal lists of the other 7 child voxel types are obtained by performing a coordinate axis symmetric reflection transformation on the base list. During the search, each sub-voxel group is visited sequentially according to the pre-calculated priority traversal list, and representative points are taken out as candidate points. The search is terminated when the farthest distance of the collected K candidate points is less than the lower bound distance of the next sub-voxel group to be visited; if the number of candidate points obtained is less than K, the search is backed up to the global priority search centered on the original point.

[0010] In the preferred technical solution, a predictive search warm-up mechanism based on motion consistency is introduced before performing the search using a priority voxel search strategy accelerated by symmetry. This mechanism includes: The system maintains the optimized pose of the LiDAR from the previous frame. T last And the inter-frame motion estimate Δ obtained through pre-integration of the inertial measurement unit or the uniform velocity model. T pred For any point in the current laser frame P curr Predict its location on the global map:

[0011] Simultaneously, the preheating radius is determined dynamically based on the inertial measurement unit noise model or motion speed:

[0012] in, R base Based on the radius, k v Here, v is the velocity sensitivity coefficient, and v is the current linear velocity. Δt The time interval between the previous frame and the current frame; Then, with Centered on, Rwarm Using a radius of , a priority voxel search strategy with symmetry acceleration is employed for the search.

[0013] In the preferred technical solution, during the iterative optimization process of the Kalman filter in the iterative error state, as the pose estimation gradually converges, the warm-up search radius of subsequent iterations decays exponentially:

[0014] in, i For the number of iterations, λ This is the attenuation coefficient.

[0015] This invention also discloses a 3D scene modeling system based on octet compact map representation and joint descriptors, used to perform the above-mentioned 3D scene modeling method, including: The global map building module uses LiDAR point cloud and inertial measurement unit data to build a global map based on octet voxels; The iterative error state Kalman filter module inputs the relative motion increment calculated by the inertial measurement unit into the iterative error state Kalman filter to obtain the system state. Based on the obtained system state, the corresponding nearest neighbor map point is searched in the global map based on octet based on the current laser point cloud. The distance from the laser point cloud to the corresponding map plane is calculated as the observation error. By minimizing the observation error of all laser point clouds, the optimal estimate of the system state is obtained. The data association module associates the current frame laser point cloud with the global map based on octet voxels based on the optimal estimate of the system state to obtain a fused global map. The joint descriptor encoding module obtains sub-maps from the obtained fused global map, extracts pixels from the sub-maps, and encodes them into joint descriptors. The loop closure detection module retrieves a query submap from the acquired submap, performs loop closure detection on the query submap based on the joint descriptor, and updates the global map of the 3D scene based on the loop closure detection results.

[0016] The present invention also discloses a computer storage medium storing a computer program, wherein the computer executes the computer program to implement the above-mentioned three-dimensional scene modeling method based on octet compact map representation.

[0017] The present invention also discloses an electronic device, including a memory and a processor. The memory stores a computer program, and the processor runs the computer program stored in the memory. When the computer program is executed, it implements the above-mentioned three-dimensional scene modeling method based on octet compact map representation.

[0018] Compared with the prior art, the significant advantages of this invention are: (1) The accuracy of mapping is better; the mapping loss problem is less likely to occur when modeling large scenes; (2) Based on the compact map representation method of octet, the lidar point cloud is fused and denoised in real time, and a heuristic priority nearest neighbor search strategy is used for efficient data association, thereby achieving real-time odometer tracking with low computational overhead and high stability. (3) The stability of large-scale mapping of large batches of point cloud data is improved. This invention has good practical application value and engineering significance in improving the accuracy of large batches of lidar point cloud mapping. (4) The mapping module of this invention adopts a lightweight and highly stable compact voxel map structure, which enables the front-end odometer to achieve real-time and low-power operation on embedded platforms with limited computing resources (such as mobile robots and drones), providing a more reliable and low-drift initial trajectory and map for back-end loop closure detection, and improving the overall stability and practicality of the system in long-term operation in complex large scenes. Attached Figure Description

[0019] Figure 1 A flowchart of a 3D scene modeling method based on octet compact map representation; Figure 2 This is the overall system flowchart; Figure 3 Flowchart of the voxel search strategy algorithm for symmetry acceleration; Figure 4 This is a modeling effect diagram of the method of the present invention. Detailed Implementation

[0020] The principle of this invention is as follows: First, a compact global map based on octet vox is constructed and maintained, and tightly coupled with inertial measurement unit (IMU) data is optimized to achieve low drift and high efficiency in real-time localization and map building. Then, a binary and triangle joint descriptor is constructed, and binary similarity checks and geometric verification of triangles are performed to determine whether the candidate sub-map is a valid loop closure detection result. Effective loop closure detection can be performed, and mapping loss is less likely to occur when modeling large scenes. It can also improve the stability of large-scale scene mapping based on large batches of LiDAR point cloud data.

[0021] Example: like Figure 1 As shown, a 3D scene modeling method based on octet compact map representation includes the following steps: A global map based on octet is constructed using lidar point cloud and inertial measurement unit data; The relative motion increment calculated by the inertial measurement unit is input into the iterative error state Kalman filter to obtain the system state. Based on the obtained system state, the corresponding nearest neighbor map point is searched in the global map based on octet according to the current laser point cloud. The distance from the laser point cloud to the corresponding map plane is calculated as the observation error. By minimizing the observation error of all laser point clouds, the optimal estimate of the system state is obtained. Based on the optimal estimate of the system state, the current frame laser point cloud is correlated with the global map based on octet voxels to obtain a fused global map. Obtain sub-maps from the obtained fused global map, and extract pixels from the sub-maps to encode them into joint descriptors; The query submap is obtained from the acquired submap. Loop closure detection is performed on the query submap based on the joint descriptor. The global map of the 3D scene is updated based on the loop closure detection results.

[0022] In a preferred embodiment, the constructed octet-based global map is managed using a hash table with a least recently used replacement strategy, which divides the three-dimensional space into segments of length r. v The basic voxel grid, each basic voxel V i Indexed by hash key k, each basic voxel is further subdivided into 8 sub-voxels V. {i,s} s=0,...,7, each sub-voxel V {i,s} It does not store any original point cloud data, but only maintains two sets of data: one is the three-dimensional representative point coordinates μ. s The other is that the record has been fused into μ. s The number of laser points n s ; During the lidar scanning update process, when a new lidar point p new When falling into a certain daughter voxel, it is related to the existing representative point μ of that daughter voxel. s The following rules apply to the merging and updating process:

[0023] Where N represents the number of times the representative point has been updated.

[0024] In a preferred embodiment, after obtaining the system state, the method further includes: For a point located in the world coordinate system after pose transformation p w By discretizing the calculations to determine the coarse grid positions of the points, the sub-voxel index k is obtained. sub Obtain the sub-voxel index k sub Then, the base voxel coordinates are obtained from the daughter voxel coordinates; in a preferred embodiment, the daughter voxel index k is obtained. sub Afterwards, the hash key k of the basic voxel V is determined by ksub By shifting one position to the right arithmetic right, we get k = k sub >>1, obtain the coordinates of the basic voxels, point p w The binary representation of the local sub-voxel number s within the basal voxel V is ( i %2, j %2, l %2), i , j , l These are the three-dimensional network coordinates of the daughter voxels.

[0025] In a preferred embodiment, a symmetry-accelerated priority voxel search strategy is employed to search for the corresponding nearest neighbor map point in an octet-based global map. The symmetry-accelerated priority voxel search strategy includes: Utilizing the regular and uniform sub-voxel structure of the global map, a priority traversal list is pre-computed offline during the system initialization phase. This list defines the following: for a given central sub-voxel, when searching its K nearest neighbor representative points, all other sub-voxels are grouped and accessed in ascending order according to the geometric distance from their center point to the center of the central sub-voxel. Since the relative positions of the 8 child voxels within the base voxel are symmetrical, the priority traversal list of one child voxel type is calculated as the base list, and the priority traversal lists of the other 7 child voxel types are obtained by performing a coordinate axis symmetric reflection transformation on the base list. During the actual search, each sub-voxel group is visited sequentially according to the pre-calculated priority traversal list, and representative points are taken out as candidate points. When the farthest distance of the collected K candidate points is less than the lower bound distance of the next sub-voxel group to be visited, the search is terminated; if the number of candidate points obtained is less than K, the search is backed up to the global priority search centered on the original point.

[0026] In a preferred embodiment, a motion-consistency-based predictive search warm-up mechanism is introduced before performing the search using a symmetry-accelerated priority voxel search strategy. This mechanism includes: The system maintains the optimized pose of the LiDAR from the previous frame. T last And the inter-frame motion estimate Δ obtained through pre-integration of the inertial measurement unit or the uniform velocity model. T pred For any point in the current laser frame P curr Predict its location on the global map:

[0027] Simultaneously, the preheating radius is determined dynamically based on the inertial measurement unit noise model or motion speed:

[0028] in, R base Based on the radius, k v Here, v is the velocity sensitivity coefficient, and v is the current linear velocity. Δt The time interval between the previous frame and the current frame; Then, with Centered on, R warm Using a radius of , a priority voxel search strategy with symmetry acceleration is employed for the search.

[0029] In a preferred embodiment, during the iterative optimization of the Kalman filter for the iterative error state, as the pose estimation gradually converges, the warm-up search radius of subsequent iterations decays exponentially:

[0030] in, i For the number of iterations, λ This is the attenuation coefficient.

[0031] In a preferred embodiment, obtaining the optimal pose estimate includes: The system state is predicted using the pre-integration results of the inertial measurement unit. For each point in the current laser frame, a priority voxel search strategy with symmetry acceleration is used to find its corresponding nearest neighbor map point and the normal vector of its plane in the global map. The distance from the point to the corresponding map plane is calculated as the observation error. Finally, the optimal pose estimation is solved iteratively by minimizing the observation error of all laser points.

[0032] Another embodiment provides a 3D scene modeling system based on octet compact map representation and joint descriptors, used to perform the above-described 3D scene modeling method, comprising: The global map building module uses LiDAR point cloud and inertial measurement unit data to build a global map based on octet voxels; The iterative error state Kalman filter module inputs the relative motion increment calculated by the inertial measurement unit into the iterative error state Kalman filter to obtain the system state. Based on the obtained system state, the corresponding nearest neighbor map point is searched in the global map based on octet based on the current laser point cloud. The distance from the laser point cloud to the corresponding map plane is calculated as the observation error. By minimizing the observation error of all laser point clouds, the optimal estimate of the system state is obtained. The data association module associates the current frame laser point cloud with the global map based on octet voxels based on the optimal estimate of the system state to obtain a fused global map. The joint descriptor encoding module obtains sub-maps from the obtained fused global map, extracts pixels from the sub-maps, and encodes them into joint descriptors. The loop closure detection module retrieves a query submap from the acquired submap, performs loop closure detection on the query submap based on the joint descriptor, and updates the global map of the 3D scene based on the loop closure detection results.

[0033] The following example illustrates the workflow of a 3D scene modeling system based on octet compact map representation and joint descriptors. Figure 2 As shown, it includes the following steps: Step 1: Reconstruct the batch data acquired by the LiDAR and camera. Based on the timestamps corresponding to the batch data acquired by the camera, reconstruct the timestamps of the batch data acquired by the LiDAR to ensure that the frequency of the batch data acquired by the camera is consistent with the frequency of the batch data acquired by the LiDAR. During the reconstruction process, IMU data backpropagation is used for distortion correction.

[0034] Step 2: Map construction is performed using LiDAR / Inertial Odometry (IMU). The map construction process adopts a compact map representation method based on octet vox to perform real-time incremental fusion and noise reduction of LiDAR point clouds. A symmetric acceleration priority voxel search strategy is used for efficient data association. Before the search, a predictive search warm-up mechanism based on motion consistency is introduced to achieve real-time odometry tracking with low computational overhead and high stability, and output global map and pose information.

[0035] Step 3: Obtain all sub-maps from the constructed global map, extract key points from each sub-map, and encode them into binary and triangle joint descriptors. Store the encoded joint descriptors in the database for management.

[0036] Step 4: Query all candidate descriptors from the database, perform binary similarity checks and triangle geometric verification to determine whether the candidate submap is a valid loop closure detection result.

[0037] Step 5: If the loop closure detection result is valid, eliminate the accumulated error in Step 2 and output a high-precision global map and pose information.

[0038] Specifically, step 1 includes: Step 1.1: Scan with one frame from the lidar For example, the scan begins ,finally A total of 6 camera images were captured during this period, taken at [times not specified]. , , , , and At any moment, but only when The camera image at any given moment contains a single frame of LiDAR point cloud.

[0039] Step 1.2: Based on the temporal characteristics of continuous scanning by lidar, arrive The batch point cloud collected during this period is defined as a reconstruction scan. This scan corresponds to Camera images captured at any given time. The reconstructed scan can be derived similarly. , ,……, These scans correspond to , ,……, The reconstructed radar scan frequency of the camera image at any given moment is kept consistent with the frequency of the input camera image.

[0040] Specifically, step 2 includes: Step 2.1: The system state is defined as a vector containing position, attitude, velocity, sensor bias, etc. Using the angular velocity and linear acceleration measurements taken by the IMU between two consecutive LiDAR scans, the relative motion increment is calculated using IMU pre-integration technology. This increment serves as prior information for state prediction and is input into the optimization framework based on iterative error state Kalman filtering.

[0041] Step 2.2: The global map M is managed by a hash table that uses a least recently used replacement strategy. This hash table divides the three-dimensional space into sections with side length r. v A grid of basal voxels, each basal voxel V i Indexed by hash key k, each basic voxel is further subdivided into 2×2×2=8 sub-voxels V. {i,s} (s=0,...,7), with a daughter voxel side length of r s =r v / 2, V per daughter voxel {i,s} It does not store any original point cloud data, but only maintains two sets of data: one is a 3D representative point coordinate system denoted by μ. s One is the position estimate of all point clouds falling into that sub-voxel after incremental fusion; the other is an integer fusion counter, denoted as n. s The records have been integrated into μ s The number of laser points in the system.

[0042] After transforming the optimal pose estimate optimized by the Iterative Error State Kalman Filter (IESKF) framework from the lidar sensor coordinate system to the global world coordinate system, for a point located in the world coordinate system after the pose transformation... A unique sub-voxel can be located in constant time complexity O(1) using an efficient spatial indexing mechanism. The specific steps of this efficient spatial indexing mechanism are to determine the coarse grid position of the point through discretization calculation; specifically, this is achieved through the formula... The discrete three-dimensional index of its sub-voxels is calculated. .

[0043] Where x, y, z represent p w The three-dimensional coordinates i, j, l It is the three-dimensional network coordinate of the daughter voxel, r s For the preset sub-voxel resolution, the floor(·) function performs a floor operation on each component of the vector, thereby mapping continuous 3D coordinates to discrete grid coordinates.

[0044] Obtain the sub-voxel index k sub Subsequently, the system further derives its position in a higher-level storage structure through efficient bitwise operations. The hash key k of the basic voxel V can be directly derived from k. sub By shifting one position to the right arithmetic right, we get k = k sub >>1, this operation is mathematically equivalent to dividing each component by two and then rounding down, which can quickly extract the base voxel coordinates from the sub-voxel coordinates. Meanwhile, the local sub-voxel number s within the base voxel V is determined by k. sub The least significant bit of each component value determines that the binary representation of the number s (b2, b1, b0) corresponds to the calculation result of (i%2, j%2, l%2), respectively. For example, if If the corresponding sub-voxel number s is binary (1,0,1), then the decimal value is 5. Through the above two calculations, the system can directly map from three-dimensional spatial coordinates to precise locations in multi-layer map data structures without any time-consuming searches or traversals, laying a solid foundation for subsequent rapid data association and fusion.

[0045] During the lidar scanning update process, when a new lidar point p new When falling into a certain daughter voxel, it is related to the existing representative point μ of that daughter voxel. s The following rules apply to the merging and updating process:

[0046] Where N represents the number of times the representative point has been updated.

[0047] This process is mathematically equivalent to an online low-pass filter, which in the initial update phase (n) s (Smaller), new points can quickly correct the positions of representative points, accelerating map building, while in the later stages of the update (n) sThe larger value indicates that the point position is highly stable, and the random measurement noise in the new observation is effectively smoothed, realizing real-time map denoising under the global frame and improving the consistency and quality of the map.

[0048] Step 2.3: Within the internal loop of processing single-frame laser point cloud data under the iterative error state Kalman filter framework, each loop requires re-association and residual calculation using the latest pose estimation. This necessitates matching the current frame's laser point cloud with the global OctVox map constructed in Step 2.2 (i.e., data association). This invention introduces a motion-consistency-based predictive search warm-up mechanism before data association. In this mechanism, the system maintains the optimized pose of the previous frame's laser radar. T last And the inter-frame motion estimation Δ obtained through IMU pre-integration or uniform velocity model T pred For any point in the current laser frame P curr Predict its location on the global map:

[0049] Simultaneously, the preheating radius is dynamically determined based on the IMU noise model or motion speed:

[0050] in R base The base radius is selected based on the sensor's performance. k v For speed sensitivity coefficient, in Δt Within a given time, the robot travels a distance of ||v|| Δt If the motion model prediction is perfectly accurate, the search center should be exactly at the true location. However, due to errors in motion estimation, the true location may fall within a certain range around the predicted location, k. v It is the coefficient used to scale this error range, usually k v >1, because the motion model error is usually proportional to the motion distance and is often greater than the actual motion distance, where v is the current linear velocity. Δt The time interval between the previous frame and the current frame.

[0051] Then, to predict Centered on, radius R warm Using a radius, a priority voxel search strategy with symmetry acceleration is employed for the search, such as... Figure 3As shown, this strategy utilizes the regular and uniform sub-voxel structure of OctVox and pre-computes a "priority traversal list" offline during system initialization. This list defines the access order of all other sub-voxels in ascending order of their geometric distance from their center point to the center of the central sub-voxel when searching for its K nearest neighbor representative points, given a central sub-voxel. Since the relative positions of the 8 sub-voxels within the base voxel are symmetrical, only one sub-voxel type needs to have its complete priority traversal list computed. The search lists for the other 7 sub-voxel types can be quickly obtained by performing a simple coordinate axis symmetric reflection transformation on the baseline list.

[0052] Finally, during the actual search, the algorithm visits each sub-voxel group sequentially according to the pre-calculated priority traversal list, and extracts representative points from them as candidates. Once the furthest distance of the collected K candidate points is less than the lower bound distance of the next sub-voxel group to be visited (i.e., the minimum possible distance from the center of all sub-voxels in that group to the query point), the algorithm immediately terminates the search; if the number of candidate points obtained from the preheating search is less than K, it backtracks to the original point P. curr A global priority search centered on the pose estimation ensures the completeness of the search. During the IESKF iterative optimization process, as the pose estimation gradually converges, the warm-up search radius of subsequent iterations decays exponentially.

[0053] in i For the number of iterations, λ The attenuation coefficient is controlled. R warm The decay rate is adjusted according to the specific scenario, especially in the context of large-scale mapping. λ Take a value between 0.5 and 0.8.

[0054] Step 2.4: Using the OctVox map as the global map model, a symmetric-accelerated priority voxel search strategy with a search warm-up mechanism is introduced as a data correlator, embedding an iterative error state Kalman filter framework. First, using the IMU pre-integration results obtained in Step 2.1, the system state is predicted. For each point in the current laser frame, the symmetric-accelerated priority voxel search strategy is used to quickly find its corresponding nearest neighbor map point (and the normal vector of its plane) in the OctVox map. The distance from the point to the corresponding map plane is calculated as the observation error. Finally, by minimizing the observation errors of all laser points, the optimal estimate of the system state (pose, velocity, etc.) is iteratively solved.

[0055] Step 2.5: Apply the optimized pose from Step 2.4 to the current frame's laser point cloud, transforming and fusing it into the global OctVox map (i.e., executing the update rules from Step 2.2). This process is performed in real-time and incrementally at the laser frame rate (typically 10-20Hz). Ultimately, the system continuously outputs high-frequency, low-drift robot trajectories (pose sequences) and a globally consistent, compact, and denoised OctVox map.

[0056] Specifically, step 3 includes: Step 3.1: Key point extraction is achieved through planar detection, then a reference plane is generated, height encoding is performed on this plane, and key points are extracted on each height-encoded image. Finally, a binary and triangle joint descriptor is constructed.

[0057] Step 3.2: Manage the extracted key points using a hash table. Construct a simple and efficient hash function to calculate the hash key from the quantized triangle side lengths. Specifically, to quantize the triangle, a fixed resolution is used, and the quantized side lengths are calculated, as shown in Equation (1.1): (1.1) in, It calculates the hash value of list L. It is the execution layer function that calculates the hash value. These are the original side lengths of the three sides of the triangle. It is the quantized side length (processed with a fixed resolution to map continuous side lengths to discrete values). It is the scaling factor. This is the number of hash buckets.

[0058] Step 3.1 includes the following specific implementation: Step 3.1.1: Given a point cloud sub-map, first perform planar detection using region growing. Divide the entire point cloud into sub-maps of size [size missing]. (For example The voxels are denoted by a set of points. Then, the point covariance matrix is ​​calculated for each voxel. As shown in equation (1.2): (1.2) in, It is the centroid (mean point) of the point cloud within the voxel. It is the i-th three-dimensional coordinate within the voxel. This represents the total number of points contained within a voxel. Planar features can be identified by calculating the eigenvalues ​​and eigenvectors of the covariance matrix.

[0059] The specific method involves determining the planarity of a point cloud by the relative magnitudes of its eigenvalues. If a eigenvalue is significantly smaller than the other two, the points within that voxel are considered to form a plane. This process is performed across the entire point cloud submap, identifying all possible planar regions.

[0060] Step 3.1.2: After obtaining the list of planes, continue generating reference planes. This involves merging adjacent planes to form a larger plane. Plane merging begins by selecting an initial plane voxel and progressively checks planes in adjacent voxels. If planes in adjacent voxels have similar normal vectors and their distance is close to zero, they are merged with the initial plane. If the initial plane voxel... and adjacent planar voxels With a center point and and normal vector and The merging criteria are as shown in equation (1.3): (1.3) in, and These are two preset thresholds. If these conditions are met, then adjacent planar voxels will be... With the initial planar voxel Merge. After merging, the new plane... center point and normal vector It can be calculated using equation (1.4): (1.4) in, and Representing adjacent planar voxels respectively With the initial planar voxel The number of points, and Representing adjacent planar voxels respectively With the initial planar voxel The center point, and Representing adjacent planar voxels respectively With the initial planar voxel covariance, It's a new plane. The covariance.

[0061] In addition, through the By performing eigenvalue decomposition, the normal vector of the merging plane can be calculated. This merging process continues in the form of region growth until there are no more planes in the adjacent voxels.

[0062] Step 3.1.3: After obtaining the reference plane, Point cloud is projected onto each reference plane to create A highly encoded image, each pixel region is... Square meters. Select. A trade-off needs to be struck between computational efficiency and the ability to capture sufficient detail in highly encoded images. To encode height information, the maximum height of a pixel on each plane is selected. and divide it into Layers, each with a fixed resolution. For each pixel, calculate a... A binary string composed of bits If the corresponding layer contains any point within that height range, this bit is set to 1; otherwise, it is set to 0. The pixel intensity is obtained by summing all m-bit values, and this intensity, along with a binary string, is stored in each pixel.

[0063] Step 3.1.4: When you have After generating highly encoded images, key points are extracted from each image by identifying local features. Keypoints are determined by the pixels with the highest intensity within a region. These local maxima represent densely populated areas of points, thus preserving the original... The most information in the point cloud. To suppress the number of keypoints, we set a threshold on the local maximum intensity. Only pixels with the highest local intensity exceeding this threshold are selected as keypoints. Once a keypoint is identified in the highly coded image, its location within the submap is determined. Coordinates. First, by mapping all points above the pixel used for height encoding. The coordinates are averaged to determine the point positions on the reference plane. Then, using these in-plane positions, the complete keypoints are calculated based on the plane parameters. Location.

[0064] Step 3.1.5: The binary and triangular joint descriptor combines the advantages of binary and triangular descriptors, consisting of a triangular descriptor and three binary descriptors corresponding to the three key points (i.e., vertices) that form the triangle. This combination allows the triangular descriptor to provide pose invariance and the correspondence between vertices, while the binary descriptors provide the local point cloud distribution, thereby further improving matching accuracy.

[0065] Specifically, step 4 includes the following implementation: Step 4.1: The sub-maps obtained by sequentially querying the sub-map database built from the previous global map are used as query sub-maps. For the query sub-map, the binary and triangle joint descriptors are extracted according to the method in step 3.1, their hash keys are calculated (as shown in equation (1.5)), the descriptor database is queried, and the corresponding container is found in the hash table.

[0066] (1.5)

[0067] in, Represents two joint descriptors and The overall similarity score is calculated, and the final output range is between [0,1]. The larger the value, the higher the similarity. Calculate the similarity between two binary feature blocks. and Representing two joint descriptors respectively and The k-th binary feature block This represents the local similarity of the k-th binary feature block. This represents the number of 1s in the binary feature block. & The & operator is used for bitwise AND operations on binary feature blocks.

[0068] Next, for all candidate descriptors in the container Perform binary descriptor similarity checks to eliminate possible outliers. For a candidate descriptor... If it is a query descriptor For a true match to occur, the vertices of the triangle should have similar binary descriptors. Furthermore, since the side lengths are stored in unique ascending order, checking the similarity of the binary descriptors helps verify the validity of the match.

[0069] Step 4.2: To enhance robustness, the RANSAC algorithm is used to find a transformation that maximizes the number of correctly matching vertices. This transformation is called the coarse transformation and is denoted as: . Let be the rigid body transformation matrix. Let be a rotation matrix. This is a translation vector. When a coarse transformation is applied to the triangle vertices in the query submap, represented in the submap's local coordinate system, they are directly aligned with the candidate submap.

[0070] Step 4.3: Obtain the rigid body transformation matrix using a coarse transformation. The planar overlap between the query submap and candidate submaps is calculated for geometric verification. The list of planes for the query submap is obtained as follows: These planes have been extracted, and the list of planes for the candidate submap is as follows: Using the center point of the candidate submap plane Build a Tree. Then, for each query sub-map plane center point , For a single query submap's set of planes, a coarse transformation is first applied. Conversion Then Search for the nearest point in the tree The overlap of two planes is determined by checking the difference between their planar distance and normal vector, as shown in equation (1.6): (1.6) in, and These represent the unit normal vectors in the query submap plane and the candidate submap plane, respectively. Indicates querying the coordinate system of the sub-map To candidate submap coordinate system transformation, and These represent querying points from the submap plane list and the candidate submap plane list, respectively. and These represent the distance and angle thresholds, respectively. Indicates querying the coordinate system of the sub-map To candidate submap coordinate system The rotation part. If a pair of planes satisfies the plane distance and normal vector constraints in equation (1.6), then the pair of planes are considered to overlap.

[0071] After examining all planes of the query submap, calculate the query submap. and candidate submap Percentage of planar overlap between As shown in equation (1.7): (1.7) For each candidate submap, determine whether the calculated percentage of planar overlap exceeds a certain threshold. If the number of loop closures exceeds the threshold for a candidate submap, it is considered valid. If multiple candidate submaps pass geometric verification, the overlap percentage is selected. The highest value is selected as the best candidate. By performing geometric verification, the chance of detecting erroneous loop closures is significantly reduced, thereby improving the accuracy of the system.

[0072] Specifically, step 5 includes: Step 5.1: After passing the geometric verification, the loop is determined to be a true loop closure. The loop closure constraint is added to the sub-map for optimization. This can be achieved using existing methods, which will not be elaborated here. The accumulated error of the historical trajectory is corrected, and the global map of the 3D scene is updated.

[0073] In another embodiment, a computer storage medium stores a computer program thereon, and when the computer executes the computer program, it implements the above-described 3D scene modeling method based on octet compact map representation.

[0074] In another embodiment, an electronic device includes a memory and a processor, wherein the memory stores a computer program, and the processor runs the computer program stored in the memory, wherein the computer program, when executed, implements the above-described 3D scene modeling method based on octet compact map representation.

[0075] from Figure 4 As can be seen, when using the method of this invention for batch modeling, the mapping effect is better than that of traditional mapping algorithms, and it can realize batch 3D reconstruction of large scenes.

[0076] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A 3D scene modeling method based on octet compact map representation, characterized in that, Includes the following steps: A global map based on octet is constructed using lidar point cloud and inertial measurement unit data; The relative motion increment calculated by the inertial measurement unit is input into the iterative error state Kalman filter to obtain the system state. Based on the obtained system state, the corresponding nearest neighbor map point is searched in the global map based on octet according to the current laser point cloud. The distance from the laser point cloud to the corresponding map plane is calculated as the observation error. By minimizing the observation error of all laser point clouds, the optimal estimate of the system state is obtained. Based on the optimal estimate of the system state, the current frame laser point cloud is correlated with the global map based on octet voxels to obtain a fused global map. Obtain sub-maps from the obtained fused global map, and extract pixels from the sub-maps to encode them into joint descriptors; The query submap is obtained from the acquired submap. Loop closure detection is performed on the query submap based on the joint descriptor. The global map of the 3D scene is updated based on the loop closure detection results.

2. The 3D scene modeling method based on octet compact map representation according to claim 1, characterized in that, The constructed octo-voxel-based global map is managed by a hash table which divides the three-dimensional space into a base voxel grid with edge length r v Each base voxel V i is indexed by a hash key k, while each base voxel is further uniformly subdivided into 8 sub-voxels V {i,s} , s = 0,..., 7, and no raw point cloud is stored within each sub-voxel V {i,s} , but only two data are maintained, one is the three-dimensional representative point coordinate μ s , and the other is the number of laser points n s recorded which have been fused into μ s ; During the lidar scanning update process, when a new lidar point p new When falling into a certain daughter voxel, it is related to the existing representative point μ of that daughter voxel. s The following rules apply to the merging and updating process: Where N represents the number of times the representative point has been updated.

3. The 3D scene modeling method based on octet compact map representation according to claim 2, characterized in that, After obtaining the system status, the following is also included: The pose of the obtained system state is transformed from the lidar sensor coordinate system to the global world coordinate system; for a point located in the world coordinate system after the pose transformation... p w By discretizing the calculations to determine the coarse grid positions of the points, the sub-voxel index k is obtained. sub Obtain the sub-voxel index k sub Then, the coordinates of the base voxels are obtained from the coordinates of the sub-voxels.

4. The 3D scene modeling method based on octet compact map representation according to claim 3, characterized in that, Obtain the sub-voxel index k sub Afterwards, the hash key k of the basic voxel V is determined by k sub By shifting one position to the right arithmetic right, we get k = k sub >>1, obtain the coordinates of the basic voxels, point p w The binary representation of the local sub-voxel number s within the basal voxel V is ( i %2, j %2, l %2), i , j , l These are the three-dimensional network coordinates of the daughter voxels.

5. The 3D scene modeling method based on octet compact map representation according to claim 1, characterized in that, A symmetry-accelerated priority voxel search strategy is employed to search for the nearest neighbor map point in an octet-based global map. This symmetry-accelerated priority voxel search strategy includes: Utilizing the regular and uniform sub-voxel structure of the global map, a priority traversal list is pre-computed offline during the system initialization phase. This list defines the following: for a given central sub-voxel, when searching its K nearest neighbor representative points, all other sub-voxels are grouped and accessed in ascending order according to the geometric distance from their center point to the center of the central sub-voxel. Since the relative positions of the 8 child voxels within the base voxel are symmetrical, the priority traversal list of one child voxel type is calculated as the base list, and the priority traversal lists of the other 7 child voxel types are obtained by performing a coordinate axis symmetric reflection transformation on the base list. During the search, each sub-voxel group is visited sequentially according to the pre-calculated priority traversal list, and representative points are taken out as candidate points. The search is terminated when the farthest distance of the collected K candidate points is less than the lower bound distance of the next sub-voxel group to be visited; if the number of candidate points obtained is less than K, the search is backed up to the global priority search centered on the original point.

6. The 3D scene modeling method based on octet compact map representation according to claim 5, characterized in that, Before employing a symmetry-accelerated priority voxel search strategy, a predictive search warm-up mechanism based on motion consistency is introduced. This mechanism includes: The system maintains the optimized pose of the LiDAR from the previous frame. T last And the inter-frame motion estimate Δ obtained through pre-integration of the inertial measurement unit or the uniform velocity model. T pred For any point in the current laser frame P curr Predict its location on the global map: Simultaneously, the preheating radius is determined dynamically based on the inertial measurement unit noise model or motion speed: in, R base Based on the radius, k v Here, v is the velocity sensitivity coefficient, and v is the current linear velocity. Δt The time interval between the previous frame and the current frame; Then, with Centered on, R warm Using a radius of 0, a priority voxel search strategy with symmetry acceleration is employed for the search.

7. The 3D scene modeling method based on octet compact map representation according to claim 6, characterized in that, In the iterative optimization process of the iterative error state Kalman filter, as the pose estimation gradually converges, the warm-up search radius of subsequent iterations decays exponentially: in, i For the number of iterations, λ This is the attenuation coefficient.

8. A 3D scene modeling system based on octet compact map representation and joint descriptors, used to perform the method as described in any one of claims 1-7, characterized in that, include: The global map building module uses LiDAR point cloud and inertial measurement unit data to build a global map based on octet voxels; The iterative error state Kalman filter module inputs the relative motion increment calculated by the inertial measurement unit into the iterative error state Kalman filter to obtain the system state. Based on the obtained system state, the corresponding nearest neighbor map point is searched in the global map based on octet based on the current laser point cloud. The distance from the laser point cloud to the corresponding map plane is calculated as the observation error. By minimizing the observation error of all laser point clouds, the optimal estimate of the system state is obtained. The data association module associates the current frame laser point cloud with the global map based on octet voxels based on the optimal estimate of the system state to obtain a fused global map. The joint descriptor encoding module obtains sub-maps from the obtained fused global map, extracts pixels from the sub-maps, and encodes them into joint descriptors; The loop closure detection module retrieves a query submap from the acquired submap, performs loop closure detection on the query submap based on the joint descriptor, and updates the global map of the 3D scene based on the loop closure detection results.

9. A computer storage medium having a computer program stored thereon, characterized in that, When the computer executes the computer program, it implements the three-dimensional scene modeling method based on octet compact map representation as described in any one of claims 1-7.

10. An electronic device comprising a memory and a processor, characterized in that, The memory stores a computer program, and the processor runs the computer program stored in the memory. When the computer program is executed, it implements the three-dimensional scene modeling method based on octet compact map representation as described in any one of claims 1-7.

Images