Loopback optimization method

Abstract

The invention provides a loopback optimization method. A current key frame is determined according to the key frame point cloud, a local map and a point cloud sub-graph are generated, and accurate spatial representation of a local environment is constructed. And multi-level voxel registration processing is executed by taking the point cloud sub-graph as a source point cloud and the local map as a target point cloud, so that a coarse-to-fine point cloud matching strategy is realized. By verifying the loopback matching effect and screening the effective loopback, the reliability of the point cloud corresponding relation and the associated data is ensured, and the influence of wrong loopback on mapping is avoided. And finally, performing segmentation optimization on all the effective loops, selecting a reference frame to calculate point cloud poses in the segments, performing adjustment and joint optimization on pose parameters through a bundle method, eliminating registration errors, and remarkably improving the registration precision of low-overlapping scenes. According to the method, environment structure information and statistical characteristics are comprehensively utilized, the problem that traditional loopback detection matching is inaccurate is solved, the consistency of the point cloud map is improved through a whole link from data acquisition, matching and verification to global optimization, and the ghosting phenomenon is remarkably reduced.

Description

Technical Field

[0001] This application relates to the field of point cloud map construction, and in particular to a loop closure optimization method. Background Technology

[0002] During point cloud map construction, loop closure detection is required to identify identical scenes, perform point cloud registration, reduce accumulated positioning errors, eliminate point cloud ghosting, and improve the accuracy of point cloud map construction. Loop closure detection is a key technical step in the positioning and mapping system, used to identify situations where a mobile vehicle revisits historical scenes during movement, and to address the problem of accumulated positioning errors over long periods of motion.

[0003] Loop closure detection includes traditional methods and deep learning methods. Traditional methods use descriptors to confirm environment revisits and registration algorithms to obtain accurate pose transformation relationships. Deep learning-based methods use deep neural networks to extract features, thereby describing the environment.

[0004] In summary, existing loop closure detection techniques fail to fully utilize environmental information and suffer from inaccurate matching, potentially leading to incorrect calculations of loop closure relationships and thus affecting mapping results. Therefore, a loop closure optimization method is needed to improve the effectiveness of loop closure detection, thereby enhancing the accuracy of point cloud map construction. Summary of the Invention

[0005] The purpose of this application is to at least address one of the aforementioned technical deficiencies, particularly the poor effectiveness of loop closure detection in the prior art, which leads to poor accuracy in point cloud map construction.

[0006] Firstly, this application provides a loop closure optimization method, the method comprising:

[0007] Obtain keyframe point clouds and corresponding associated data;

[0008] The associated data includes keyframe pose, position variance data, and factor graph residuals.

[0009] Based on the keyframe point cloud, determine the current keyframe, and obtain the corresponding local map and point cloud sub-map based on the current keyframe;

[0010] Based on the local map and the point cloud sub-map, using the point cloud sub-map as the source point cloud and the local map as the target point cloud, multi-level voxel registration processing is performed to determine the loop closure matching status of the current keyframe, and to verify the loop closure matching effect corresponding to the current keyframe, thereby determining the valid loop closure.

[0011] The effective loopback includes the point cloud correspondence of each current key frame, as well as the associated data of each point cloud involved.

[0012] After all the current keyframes have been matched, each valid loop is segmented, a reference frame is selected and the point cloud pose within each segment is calculated, the loop constraint relationship is determined, and the point cloud pose within each segment is optimized by bundle adjustment.

[0013] As an optional implementation, the step of determining the current keyframe based on the keyframe point cloud and obtaining the corresponding local map and point cloud sub-map based on the current keyframe includes:

[0014] Candidate keyframes are determined based on the keyframe point cloud and the associated data;

[0015] For each of the candidate keyframes, a first keyframe and a second and third keyframe corresponding to the first keyframe are determined by spatial proximity search;

[0016] If the pose number interval between the corresponding candidate keyframe and the corresponding first keyframe, second keyframe and third keyframe all exceed a preset number threshold, and the spatial distance between them is less than a preset distance threshold, then the candidate keyframe is determined as the current keyframe.

[0017] A local map is obtained by stitching together the point clouds of the first keyframe, the second keyframe, and the first number of candidate keyframes surrounding the third keyframe corresponding to the current keyframe. A point cloud sub-map is then obtained by stitching together the point clouds of the second number of candidate keyframes before and after the current keyframe.

[0018] As an optional implementation, the step of performing multi-level voxel registration processing based on the local map and the point cloud sub-map to determine the loop closure matching status of the current keyframe includes:

[0019] Determine a decreasing sequence of multiple voxel sizes;

[0020] The point cloud of the local map and the point cloud sub-map is downsampled according to the current voxel size;

[0021] Using the previous registration result as input, the probability distribution registration algorithm is executed to obtain the transformation relationship. The algorithm is iteratively executed until the registration of all voxel sizes is completed, and the final transformation relationship is output.

[0022] The final-level transformation relationship is used to determine the loop closure matching status of the current keyframe.

[0023] As an optional implementation, the verification corresponds to the loop closure matching effect of the current keyframe, and the determination of valid loop closures includes:

[0024] Calculate the matching score and occupancy ratio corresponding to the loop closure matching situation of the current keyframe;

[0025] If the matching score exceeds a preset threshold and the proportion of the match increases after matching, the corresponding loop matching relationship is determined as a valid loop.

[0026] As an optional implementation, the matching score is determined by:

[0027] Based on the loop closure matching, the registered and transformed point cloud sub-image is placed in the local map coordinate system;

[0028] The local map is divided into a first voxel grid, and the probability density contribution score of each point in the point cloud sub-map in the voxel distribution of the first voxel grid is calculated.

[0029] The probability density contribution scores of all points are negativeed and summed, and the total value is used as the matching score.

[0030] Furthermore, the method for determining the occupancy ratio includes:

[0031] Establish a second voxel grid covering the local map and the point cloud sub-map point cloud;

[0032] The occupied voxels are marked according to the spatial distribution of the local map point cloud;

[0033] The proportion of points in the point cloud sub-image that fall into occupied voxels to the total number of points in the local map point cloud is calculated as the occupancy ratio.

[0034] As an optional implementation, the step of segmenting each effective loop, selecting a reference frame, calculating the point cloud pose within each segment, and determining the loop constraint relationship includes:

[0035] Traverse the pose index of each target keyframe in each valid loop;

[0036] A new segment is created when the difference between the sequence numbers of adjacent target keyframes exceeds a preset segmentation threshold.

[0037] Within each segment, a preset number of optimized key frames are obtained by filtering based on the residuals of the factor graph, and a reference frame is selected from each of the optimized key frames based on the location variance data.

[0038] Using the reference frame pose as the origin, the pose of each optimized key frame within the segment is calculated according to the relative pose constraint chain between adjacent key frames.

[0039] As an optional implementation, the optimization of the point cloud pose within each segment using bundle adjustment includes:

[0040] Based on the optimization objective, all keyframe pose parameters and planar feature parameters within each segment are simultaneously optimized.

[0041] The optimization objective includes minimizing the sum of residuals corresponding to all planar features;

[0042] The residual of each planar feature includes: the average of the squared distances from all laser measurement points that observe the planar feature to the planar feature in the global coordinate system, wherein the distance includes the normal vector projection distance from the laser measurement point to the plane containing the planar feature;

[0043] The poses of each optimized keyframe within the segment are derived from the relative pose constraint chain between adjacent keyframes as the initial poses, and optimization calculations are performed iteratively through bundle adjustment.

[0044] The optimization calculation is used to optimize the pose parameters of each of the optimization keyframes, including rotation matrix and translation vector, as well as the feature parameters of each planar feature, including normal vector and coordinates of planar reference point;

[0045] The optimization terminates when the decrease in the sum of residuals is less than the preset convergence threshold or when the maximum number of iterations is reached.

[0046] Secondly, this application provides a loop closure optimization device, the device comprising:

[0047] The acquisition module is used to acquire keyframe point clouds and their corresponding associated data.

[0048] The associated data includes keyframe pose, position variance data, and factor graph residuals.

[0049] The processing module is used to determine the current key frame based on the key frame point cloud, and to obtain the corresponding local map and point cloud sub-map based on the current key frame.

[0050] The processing module is further configured to perform multi-level voxel registration processing based on the local map and the point cloud sub-map, using the point cloud sub-map as the source point cloud and the local map as the target point cloud, to determine the loop closure matching status of the current keyframe, and to verify the loop closure matching effect corresponding to the current keyframe, and to determine the valid loop closure.

[0051] The effective loopback includes the point cloud correspondence of each current key frame, as well as the associated data of each point cloud involved.

[0052] The processing module is further configured to segment each valid loop after all current keyframes have been matched, select a reference frame and calculate the point cloud pose within each segment, determine the loop constraint relationship, and optimize the point cloud pose within each segment using bundle adjustment.

[0053] Thirdly, this application provides a computer device including one or more processors and a memory storing computer-readable instructions that, when executed by the one or more processors, perform the steps of the method described in the first aspect.

[0054] Fourthly, this application provides a storage medium storing computer-readable instructions that, when executed by one or more processors, cause the one or more processors to perform the steps of the method described in the first aspect.

[0055] As can be seen from the above technical solutions, the embodiments of this application have the following advantages:

[0056] Based on any of the above embodiments, the loop closure optimization method of this application first filters the current keyframe based on spatiotemporal constraints and constructs a local map and point cloud sub-map to provide a highly complete environmental representation for registration. Secondly, a multi-level voxel registration strategy is adopted, iteratively optimizing the transformation relationship through a decreasing voxel size sequence and a probability distribution registration algorithm, significantly improving registration robustness. Next, the loop closure effect is verified using both matching score and occupancy ratio indicators to ensure the reliability of point cloud correspondence and associated data. Finally, effective loop closures are segmented and optimized, with the reference frame as the origin for chain-like pose calculation, and keyframe pose and planar feature parameters are jointly optimized through bundle adjustment, fully utilizing environmental structural constraints to eliminate registration errors and significantly improve registration accuracy in low-overlap scenes. This method breaks through traditional limitations in four dimensions: data filtering, registration robustness, verification reliability, and global optimization, effectively solving the mapping ghosting problem, improving the effectiveness of loop closure detection, and thus improving the accuracy of point cloud map construction, providing a high-precision point cloud map foundation for applications such as autonomous driving and robot navigation. Attached Figure Description

[0057] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0058] Figure 1 A schematic flowchart of a loop closure optimization method provided in one embodiment of this application;

[0059] Figure 2 This is a schematic diagram illustrating the effect of a loop closure optimization method provided in one embodiment of this application;

[0060] Figure 3 This is an internal structural diagram of a computer device provided in an embodiment of this application. Detailed Implementation

[0061] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0062] First, let's explain the following concepts:

[0063] Simultaneous Localization and Mapping (SLAM) refers to the technology of robots or vehicles locating their own position and building environmental maps in real time in unknown environments. It is widely used in fields such as autonomous driving and robot navigation.

[0064] Iterative Closest Point (ICP) algorithm: It achieves alignment between two point clouds by iteratively calculating the correspondence between the closest points and optimizing the transformation matrix. It is the core registration algorithm in laser SLAM.

[0065] Normal Distributions Transform (NDT) registration: A point cloud registration algorithm that achieves efficient matching by representing point clouds as probability distribution models (such as Gaussian distributions) and is insensitive to noise and initial position.

[0066] Global Navigation Satellite System (GNSS) provides global positioning services (such as GPS and BeiDou) through satellite signals. It is often used in conjunction with SLAM to provide global position constraints to reduce accumulated errors.

[0067] Bundle Adjustment (BA): By optimizing camera pose and map point coordinates, it minimizes reprojection error and is a core technology for backend optimization in visual SLAM.

[0068] Loop Closure Detection (LCD), also known as loop closure detection, is a core technology in SLAM (Simultaneous Localization and Mapping) used to solve the localization drift problem caused by error accumulation in robots or autonomous driving systems during long-term motion. LCD refers to the robot identifying previously visited locations within the current scene and establishing constraints between the current pose and historical poses, thus "closing" the loop of the motion trajectory. It determines whether the robot has returned to the origin by comparing data similarity, such as comparing the matching degree between the current image / point cloud and historical data. Essentially, it is a problem of cross-temporal and spatiotemporal data association.

[0069] In loop closure detection, "source point cloud" and "target point cloud" are core concepts for scene matching in LiDAR SLAM. They represent current perception data and historical map data, respectively, and achieve cross-temporal and spatiotemporal correlation through geometric feature comparison. The source point cloud refers to the point cloud data collected in real time by the robot using LiDAR, reflecting the local environmental geometry from the current sensor perspective. The target point cloud is a point cloud stored in the historical keyframe database, representing the locations the robot has previously traversed, and constitutes part of the global map. In the registration task of this application, the "point cloud sub-image" is used as the source point cloud, and the "local map" is used as the target point cloud. The specific generation and implementation methods can be found in the description of the relevant location.

[0070] With the development of autonomous driving and robotics, SLAM (Simultaneous Localization and Mapping) technology is widely used for point cloud map construction. Loop closure detection is a crucial module in SLAM technology, used to identify identical scenes, perform point cloud registration, reduce accumulated localization errors, and thus eliminate point cloud ghosting. In laser SLAM systems, loop closure detection can be divided into traditional methods and deep learning-based methods. Traditional methods often use descriptors to confirm environmental revisits and employ methods such as NDT or ICP for registration transformation to obtain accurate pose transformation relationships. Commonly used descriptors include Scan Context, NDD, etc. Deep learning-based methods use deep neural networks to extract features to describe the environment. Commonly used methods include LCDNet, OverlapNet, etc. In large-scale outdoor mapping tasks, the mapping scenes are rich and diverse, such as urban canyons, tunnels, and tree-lined paths, which poses a significant challenge to loop closure detection. If loop closure detection is incorrect or the established loop closure relationships are inaccurate, accumulated errors may not be eliminated, leading to ghosting in the constructed point cloud map.

[0071] In loop closure detection, NDT or ICP are widely used. However, NDT or ICP may be inaccurate in scenes with low overlap rates, resulting in incorrect loop closure relationships and preventing the elimination of accumulated errors, leading to map ghosting. Therefore, how to improve loop closure accuracy and eliminate map ghosting is a problem worthy of research.

[0072] This application, as an improved method in the field of SLAM, will be described based on NDT and BA in its specific implementation.

[0073] In summary, the technical concept of this application lies in its loop closure optimization method. First, based on spatiotemporal constraints, the current keyframe is selected and a local map and point cloud sub-map are constructed, providing a highly complete environmental representation for registration. Second, a multi-level voxel registration strategy is adopted, iteratively optimizing the transformation relationship through a decreasing voxel size sequence and a probability distribution registration algorithm, significantly improving registration robustness. Next, the loop closure effect is verified using both matching score and occupancy ratio indicators, ensuring the reliability of point cloud correspondence and associated data. Finally, effective loop closures are segmented and optimized, with the reference frame as the origin, and the pose is calculated in a chain-like manner. The keyframe pose and planar feature parameters are jointly optimized through bundle adjustment, fully utilizing environmental structural constraints to eliminate registration errors and significantly improve registration accuracy in low-overlap scenes. This method breaks through traditional limitations in four dimensions: data selection, registration robustness, verification reliability, and global optimization. It effectively solves the mapping ghosting problem, improves the effectiveness of loop closure detection, and thus improves the accuracy of point cloud map construction, providing a high-precision point cloud map foundation for applications such as autonomous driving and robot navigation.

[0074] The methods provided in this application will be described in detail below based on the corresponding implementation methods in some practical application scenarios.

[0075] Figure 1 This is a flowchart illustrating a loop closure optimization method provided in one embodiment of this application, as shown below. Figure 1 As shown, this application provides a loop closure optimization method, the method comprising:

[0076] S101. Obtain keyframe point clouds and corresponding associated data;

[0077] The associated data includes keyframe pose, position variance data, and factor graph residuals.

[0078] The data used in this application includes keyframe point clouds and their poses, the GNSS variance of each point, and factor map residuals. Keyframe point clouds are obtained through laser odometry. The criteria for dividing keyframe point clouds can be a pose angle change exceeding 0.5° or an odometry change exceeding one meter (or other thresholds). The GNSS variance is derived from the GNSS data closest to the keyframe in terms of location. The factor map residuals are the difference between the observed and estimated values. In the factor map, the observed value is the GNSS location (if misaligned, it is interpolated to the keyframe time using GNSS data from two consecutive frames; the interpolation method is not explained further), and the estimated value is the result of factor map optimization.

[0079] The factor map integrates GNSS data, laser odometry data, and other data to optimize the keyframe pose.

[0080] Keyframe poses and point clouds can be derived from a single set of data or from different sets of data.

[0081] Each keyframe pose has its own ID, which increases over time. Adjacent IDs indicate that the point cloud was acquired at adjacent time points.

[0082] S102. Determine the current key frame based on the key frame point cloud, and obtain the corresponding local map and point cloud sub-map based on the current key frame.

[0083] S103. Based on the local map and the point cloud sub-map, using the point cloud sub-map as the source point cloud and the local map as the target point cloud, perform multi-level voxel registration processing to determine the loop closure matching status of the current keyframe, verify the loop closure matching effect corresponding to the current keyframe, and determine the valid loop closure.

[0084] The effective loopback includes the point cloud correspondence of each current key frame, as well as the associated data of each point cloud involved.

[0085] S104. After all the current keyframes have been matched, each valid loop is segmented, a reference frame is selected and the point cloud pose in each segment is calculated, the loop constraint relationship is determined, and the point cloud pose in each segment is optimized by bundle adjustment.

[0086] The loop closure optimization method provided in this embodiment offers multi-dimensional environmental information support for loop closure detection by acquiring associated data including keyframe pose, position variance data, and factor map residuals. Based on the keyframe point cloud, the current keyframe is determined, and a local map and point cloud sub-map are generated, constructing an accurate spatial representation of the local environment. Multi-level voxel registration is performed using the point cloud sub-map as the source point cloud and the local map as the target point cloud, implementing a coarse-to-fine point cloud matching strategy and significantly improving registration robustness. By verifying the loop closure matching effect and filtering valid loops, the reliability of point cloud correspondences and associated data is ensured, avoiding the impact of erroneous loops on mapping. Finally, all valid loops are segmented and optimized, and the point cloud pose within each segment is calculated using a reference frame. Pose parameters are jointly optimized through bundle adjustment to eliminate registration errors and significantly improve registration accuracy in low-overlap scenes. This method comprehensively utilizes environmental structural information and statistical characteristics to solve the problem of inaccurate matching in traditional loop closure detection. It improves the consistency of the point cloud map throughout the entire chain from data acquisition, matching, verification to global optimization, significantly reducing ghosting phenomena.

[0087] As an optional implementation, the step of determining the current keyframe based on the keyframe point cloud and obtaining the corresponding local map and point cloud sub-map based on the current keyframe includes:

[0088] Candidate keyframes are determined based on the keyframe point cloud and the associated data;

[0089] For each of the candidate keyframes, a first keyframe and a second and third keyframe corresponding to the first keyframe are determined by spatial proximity search;

[0090] If the pose number interval between the corresponding candidate keyframe and the corresponding first keyframe, second keyframe and third keyframe all exceed a preset number threshold, and the spatial distance between them is less than a preset distance threshold, then the candidate keyframe is determined as the current keyframe.

[0091] A local map is obtained by stitching together the point clouds of the first keyframe, the second keyframe, and the first number of candidate keyframes surrounding the third keyframe corresponding to the current keyframe. A point cloud sub-map is then obtained by stitching together the point clouds of the second number of candidate keyframes before and after the current keyframe.

[0092] Specifically, for the candidate keyframes determined by the specific conditions described in S101, the nearest neighbor search method is used to obtain the current keyframe a1, the nearest neighbor keyframe b1, and two keyframes b2 and b3 near the nearest neighbor. The requirement is that the difference between the ID of a1 and the IDs of b1, b2, and b3 exceeds 100, and the distance is within 70 meters. Candidate keyframes meeting these conditions can be used as the current keyframe. Then, a local map t1 is obtained from the point cloud of the 50 keyframes before and after b1, b2, and b3. The point cloud sub-map s1 is generated by stitching together the 5 keyframes before and after a1. Specifically, multi-resolution NDT can be used for matching.

[0093] This implementation uses spatial proximity search to filter the current keyframe, requiring that the pose index interval between the candidate keyframe and its neighboring keyframes exceed a preset threshold and the spatial distance is less than a threshold. This ensures the keyframe has spatiotemporal representativeness and avoids introducing noisy data during local map construction. A local map is formed by stitching together a first number of keyframes before and after the first keyframe, and a point cloud sub-map is formed by stitching together a second number of keyframes before and after the current keyframe. This allows the local map to cover broader historical environmental information, while the point cloud sub-map focuses on the current local area. This strategy provides highly complete source and target point clouds for subsequent registration, enhancing the richness of environmental features and thus improving the initial alignment accuracy of multi-level registration, laying a data foundation for loopback verification.

[0094] As an optional implementation, the step of performing multi-level voxel registration processing based on the local map and the point cloud sub-map to determine the loop closure matching status of the current keyframe includes:

[0095] Determine a decreasing sequence of multiple voxel sizes;

[0096] The point cloud of the local map and the point cloud sub-map is downsampled according to the current voxel size;

[0097] Using the previous registration result as input, the probability distribution registration algorithm is executed to obtain the transformation relationship. The algorithm is iteratively executed until the registration of all voxel sizes is completed, and the final transformation relationship is output.

[0098] The final-level transformation relationship is used to determine the loop closure matching status of the current keyframe.

[0099] The voxel size sequence, i.e., the grid size, can be set to 1, 0.5, 0.4, and 0.3 respectively. The grid size refers to the downsampled voxel size of the output point cloud. In detail, the registration process is as follows:

[0100] 1. Downsample point clouds s1 and t1 respectively, with a voxel size of 1 meter. After sampling, perform NDT matching to obtain the transformation relationship tran0.

[0101] 2. Downsample point clouds s1 and t1 respectively, with a voxel size of 0.5 meters. After sampling, use tran0 as the initial value for NDT registration to perform NDT matching and obtain the transformation relationship tran1.

[0102] 3. Downsample point clouds s1 and t1 respectively, with a voxel size of 0.4 meters. After sampling, use tran1 as the initial value for NDT registration to perform NDT matching and obtain the transformation relationship tran2.

[0103] 4. Downsample point clouds s1 and t1 respectively, with a voxel size of 0.3 meters. After sampling, use tran2 as the initial value for NDT registration to perform NDT matching and obtain the transformation relationship tran3.

[0104] trans3 represents the matching results obtained from multi-resolution NDT. At this point, the search and matching of loop closure pairs has been established, but the matching effectiveness of the loop closures still needs to be checked. The checking process can refer to the implementation methods of the matching score and occupancy ratio metrics.

[0105] This implementation employs a decreasing voxel size sequence for multi-level registration, achieving iterative optimization from low to high precision by progressively reducing the downsampled voxel size. In multi-level voxel registration, each iteration uses the registration result of the previous level as input to the probabilistic distribution registration algorithm. A probabilistic model is used to model the point cloud distribution characteristics, effectively overcoming the influence of initial pose deviations on registration. Through the gradual convergence of multi-level voxel sizes, the stability of point cloud matching is improved while maintaining computational efficiency, making it particularly suitable for feature-sparse scenes such as urban canyons and tunnels. The final output transformation relationship provides high-precision pose constraints for loop closure matching.

[0106] As an optional implementation, the verification corresponds to the loop closure matching effect of the current keyframe, and the determination of valid loop closures includes:

[0107] Calculate the matching score and occupancy ratio corresponding to the loop closure matching situation of the current keyframe;

[0108] If the matching score exceeds a preset threshold and the proportion of the match increases after matching, the corresponding loop matching relationship is determined as a valid loop.

[0109] This implementation verifies the loop closure effect using two metrics: matching score and occupancy ratio. The matching score is calculated based on the voxel distribution probability density of the local map, objectively reflecting the degree of similarity in the point cloud distribution after registration. The occupancy ratio is the percentage of voxels occupied by the point cloud sub-map that fall into the target point cloud, measuring the actual improvement in scene overlap. A valid loop closure is only determined when the matching score exceeds a threshold and the occupancy ratio increases. This dual verification mechanism significantly reduces the risk of false matching, avoids false loop closures caused by environmental similarity, and ensures the reliability of loop closure constraints in subsequent optimizations.

[0110] As an optional implementation, the matching score is determined by:

[0111] Based on the loop closure matching, the registered and transformed point cloud sub-image is placed in the local map coordinate system;

[0112] The local map is divided into a first voxel grid, and the probability density contribution score of each point in the point cloud sub-map in the voxel distribution of the first voxel grid is calculated.

[0113] The probability density contribution scores of all points are negativeed and summed, and the total value is used as the matching score.

[0114] Furthermore, the method for determining the occupancy ratio includes:

[0115] Establish a second voxel grid covering the local map and the point cloud sub-map point cloud;

[0116] The occupied voxels are marked according to the spatial distribution of the local map point cloud;

[0117] The proportion of points in the point cloud sub-image that fall into occupied voxels to the total number of points in the local map point cloud is calculated as the occupancy ratio.

[0118] The first check condition can be a matching score greater than 8, and the second check condition can be an increase in the overlap rate after optimization. Here, overlap rate refers to the proportion of point cloud s1 in point cloud t1.

[0119] The match score is the result of the last match in the multi-resolution NDT. The formula for the match score is as follows:

[0120]

[0121] in, It is the k-th point, and pose is the pose of the point cloud in this frame. It uses pose to place the point Transform to the target coordinate system. It represents the probability that all points fall on the point cloud surface.

[0122] The contribution of one point to the NDT score is:

[0123]

[0124] in, yes The mean coordinates falling on the NDT voxel grid. yes Mean and variance of coordinates falling on the NDT voxel grid. , It is a logarithmic function, and the calculation of this probability value uses a hybrid form of normal and uniform distribution, which is robust to outliers and outliers.

[0125] The second check condition is overlap_rate. The overlap_rate is calculated as follows: First, a voxel mesh is created, which must contain both the target point cloud and the source point cloud. Let the voxel mesh size be r, then the dimension of the voxel mesh in the x-direction is...

[0126]

[0127] The x-direction index of p within the voxel grid is

[0128]

[0129] The same applies to the y and z directions.

[0130] The above method is used to obtain the 3D indices of all points in the target point cloud in the voxel space, and the number of points corresponding to the 3D index is recorded to determine whether the voxel is occupied, and the occupancy status is updated (occupied is 1, unoccupied is 0). A hash function is used to quickly sort the point indices.

[0131] Traverse the source point cloud and calculate whether each point falls within an occupied grid cell. The overlap rate is calculated using the following formula:

[0132]

[0133] num1 is the number of points that fall into the occupied grid, and num2 is the total number of points in the target point cloud.

[0134] The increase in overlap_rate after matching indicates that more points fall into the occupied grid, meaning that the source point cloud and the target point cloud have a higher degree of overlap and fit more closely. This proves that the matching result makes the point cloud move closer to the direction of overlap, which is a valid match.

[0135] The loop relationship is stored in the data structure. The most basic element is a loop pair, which includes the source point cloud ID, the target point cloud ID, the pose a corresponding to the source point cloud, the pose b corresponding to the target point cloud, the transformation relationship between a and b, which is the result of multi-resolution matching, GNSS variance, and the residual of the node in the factor graph (the difference between the measured value and the estimated value in the factor graph).

[0136] This implementation specifically defines the calculation logic for the matching score and occupancy ratio. The matching score is obtained by accumulating the probability density contribution scores of point cloud sub-map points in the local map voxel grid, using a probabilistic model to quantify the consistency of the spatial distribution of the point cloud. The occupancy ratio is obtained by establishing a voxel grid covering the entire world and statistically analyzing the ratio of point cloud sub-map points falling into the target point cloud's occupied voxels, objectively reflecting the overlap of scene structures. The combination of these two methods evaluates the registration quality from both probabilistic distribution and geometric occupancy dimensions, providing quantifiable objective evidence for loop closure verification, replacing subjective threshold settings, and enhancing the method's universality and automation.

[0137] As an optional implementation, the step of segmenting each effective loop, selecting a reference frame, calculating the point cloud pose within each segment, and determining the loop constraint relationship includes:

[0138] Traverse the pose index of each target keyframe in each valid loop;

[0139] A new segment is created when the difference between the sequence numbers of adjacent target keyframes exceeds a preset segmentation threshold.

[0140] Within each segment, a preset number of optimized key frames are obtained by filtering based on the residuals of the factor graph, and a reference frame is selected from each of the optimized key frames based on the location variance data.

[0141] Using the reference frame pose as the origin, the pose of each optimized key frame within the segment is calculated according to the relative pose constraint chain between adjacent key frames.

[0142] After identifying valid loop closures, the matching results are further refined. First, the data is segmented. The target point cloud IDs are traversed; if the difference between the current ID and the previous ID exceeds 50, the data is segmented. This results in several segments, each containing m loop closure pairs, forming the point cloud data set `cloud_m` for that segment. These segments are located within a region, typically an intersection or adjacent lane area. To fully utilize the point cloud information, the keyframe IDs contained in all loop closures of each segment are counted. The point clouds and their poses of the first and last 50 frames of each keyframe ID are loaded, with duplicates retained only once, resulting in the point cloud data set `cloud_n` for that segment. `cloud_n` contains `cloud_m`. First, the 10 nodes with the smallest node residuals in each `cloud_m` segment are selected. Then, the keyframe with the smallest GNSS variance is selected as the first fixed point cloud `cloud0`. The poses of all other keyframes in `cloud_n` are inferred from `cloud0`.

[0143] Please see Figure 2 , Figure 2 This diagram illustrates the effect of a loop closure optimization method provided in one embodiment of this application, showing the inter-frame constraints. As shown, there are inter-frame constraints (derived from the factor graph optimization results) between x1, x2, x3, and x4. That is, x2 can be derived from x1, x3 can be derived from x2, and x4 can be derived from x3, and vice versa. x3 and x6 form a loop closure pair with loop closure constraints. Assuming x3 is the pose of the fixed point cloud in the first frame, then x1, x2, and x4 are derived from x3, x6 is derived from x3, and x5, x7, and x8 are calculated from x6.

[0144] This implementation divides continuous environmental regions into independent optimization units by segmenting effective loop closures according to pose sequence differences, adapting to the local consistency requirements of large-scene mapping. Within each segment, keyframes are selected for optimization based on factor map residuals to ensure high confidence in the data participating in optimization. A reference frame is selected based on position variance data, using a low-variance pose as the origin to improve calculation stability. The pose within each segment is calculated in a chain-like manner using relative pose constraints between adjacent keyframes, maintaining local topological invariance, providing high-precision initial values ​​for bundle adjustment, and avoiding optimization getting trapped in local optima.

[0145] As an optional implementation, the optimization of the point cloud pose within each segment using bundle adjustment includes:

[0146] Based on the optimization objective, all keyframe pose parameters and planar feature parameters within each segment are simultaneously optimized.

[0147] The optimization objective includes minimizing the sum of residuals corresponding to all planar features;

[0148] The residual of each planar feature includes: the average of the squared distances from all laser measurement points that observe the planar feature to the planar feature in the global coordinate system, wherein the distance includes the normal vector projection distance from the laser measurement point to the plane containing the planar feature;

[0149] The poses of each optimized keyframe within the segment are derived from the relative pose constraint chain between adjacent keyframes as the initial poses, and optimization calculations are performed iteratively through bundle adjustment.

[0150] The optimization calculation is used to optimize the pose parameters of each of the optimization keyframes, including rotation matrix and translation vector, as well as the feature parameters of each planar feature, including normal vector and coordinates of planar reference point;

[0151] The optimization terminates when the decrease in the sum of residuals is less than the preset convergence threshold or when the maximum number of iterations is reached.

[0152] After inferring the poses of all keyframes, bundle adjustment (BA) is used for optimization. Bundle adjustment refers to simultaneously determining the poses and feature parameters of all keyframes to achieve the best consistency in the constructed map. The BA formula is as follows:

[0153]

[0154] This represents the map inconsistency caused by the i-th feature. These are the feature parameters, and T represents the pose of all keyframes.

[0155] In the bundle adjustment method of this application, the rich planar features in the point cloud are utilized to minimize the Euclidean distance between each measured original lidar point and its plane. Assuming that the i-th feature of the j-th keyframe measures a total of n lidar points, and each point is denoted as p, then its position in the global coordinate system is:

[0156]

[0157] Planar characteristic parameters It can be used Let be an expression, where n is the plane normal vector and q is any point on the plane, both in the global coordinate system. The Euclidean distance from the measurement point to the plane is:

[0158]

[0159] The formula for the residual from all points to the plane can be expressed as:

[0160]

[0161] It is the total number of laser points on planar feature i observed in all keyframes.

[0162] This implementation employs bundle adjustment to jointly optimize keyframe pose and planar feature parameters. The optimization objective is to minimize the sum of squared normal projection distances from all laser measurement points to their corresponding planar features, fully utilizing environmental planar structural features to enhance constraint strength. Using chained pose estimation as initial values, the rotation matrix, translation vector, and planar normal vector are iteratively optimized along with the reference point coordinates to achieve coordinated convergence of pose and map features. This strategy significantly improves the robustness of optimization in complex scenarios. Termination occurs when the total residual decreases below a threshold or reaches the maximum number of iterations, balancing computational efficiency and accuracy requirements.

[0163] In summary, this application employs a coarse matching + local optimization approach, which effectively improves loop closure accuracy. Local optimization considers all local point clouds from a data perspective and, in principle, optimizes using point-to-surface distance, demonstrating robustness in low overlap scenarios. The two designed loop closure checking methods effectively eliminate invalid or erroneous loops. The key point of this application lies in using multi-resolution NDT to obtain loop closure constraints, employing two checking conditions, and then performing batch optimization through BA (Balanced Assortment) based on the local point clouds to achieve optimal data consistency. Firstly, the two checking conditions eliminate erroneous loops. Then, the BA optimization based on the local point clouds demonstrates robustness in registration for low overlap scenarios and optimizes loop closure constraints. One of the important innovations is that the checking condition `overlap_rate` judges the matching effect by the change in the occupancy ratio.

[0164] This application also provides a loop closure optimization device, the device comprising:

[0165] The acquisition module is used to acquire keyframe point clouds and their corresponding associated data.

[0166] The associated data includes keyframe pose, position variance data, and factor graph residuals.

[0167] The processing module is used to determine the current key frame based on the key frame point cloud, and to obtain the corresponding local map and point cloud sub-map based on the current key frame.

[0168] The processing module is further configured to perform multi-level voxel registration processing based on the local map and the point cloud sub-map, using the point cloud sub-map as the source point cloud and the local map as the target point cloud, to determine the loop closure matching status of the current keyframe, and to verify the loop closure matching effect corresponding to the current keyframe, and to determine the valid loop closure.

[0169] The effective loopback includes the point cloud correspondence of each current key frame, as well as the associated data of each point cloud involved.

[0170] The processing module is further configured to segment each valid loop after all current keyframes have been matched, select a reference frame and calculate the point cloud pose within each segment, determine the loop constraint relationship, and optimize the point cloud pose within each segment using bundle adjustment.

[0171] The loop closure optimization method provided in this embodiment offers multi-dimensional environmental information support for loop closure detection by acquiring associated data including keyframe pose, position variance data, and factor map residuals. Based on the keyframe point cloud, the current keyframe is determined, and a local map and point cloud sub-map are generated, constructing an accurate spatial representation of the local environment. Multi-level voxel registration is performed using the point cloud sub-map as the source point cloud and the local map as the target point cloud, implementing a coarse-to-fine point cloud matching strategy and significantly improving registration robustness. By verifying the loop closure matching effect and filtering valid loops, the reliability of point cloud correspondences and associated data is ensured, avoiding the impact of erroneous loops on mapping. Finally, all valid loops are segmented and optimized, and the point cloud pose within each segment is calculated using a reference frame. Pose parameters are jointly optimized through bundle adjustment to eliminate registration errors and significantly improve registration accuracy in low-overlap scenes. This method comprehensively utilizes environmental structural information and statistical characteristics to solve the problem of inaccurate matching in traditional loop closure detection. It improves the consistency of the point cloud map throughout the entire chain from data acquisition, matching, verification to global optimization, significantly reducing ghosting phenomena.

[0172] As an optional implementation, the specific method by which the processing module determines the current keyframe based on the keyframe point cloud and obtains the corresponding local map and point cloud sub-map based on the current keyframe includes:

[0173] Candidate keyframes are determined based on the keyframe point cloud and the associated data;

[0174] For each of the candidate keyframes, a first keyframe and a second and third keyframe corresponding to the first keyframe are determined by spatial proximity search;

[0175] If the pose number interval between the corresponding candidate keyframe and the corresponding first keyframe, second keyframe and third keyframe all exceed a preset number threshold, and the spatial distance between them is less than a preset distance threshold, then the candidate keyframe is determined as the current keyframe.

[0176] A local map is obtained by stitching together the point clouds of the first keyframe, the second keyframe, and the first number of candidate keyframes surrounding the third keyframe corresponding to the current keyframe. A point cloud sub-map is then obtained by stitching together the point clouds of the second number of candidate keyframes before and after the current keyframe.

[0177] This implementation uses spatial proximity search to filter the current keyframe, requiring that the pose index interval between the candidate keyframe and its neighboring keyframes exceed a preset threshold and the spatial distance is less than a threshold. This ensures the keyframe has spatiotemporal representativeness and avoids introducing noisy data during local map construction. A local map is formed by stitching together a first number of keyframes before and after the first keyframe, and a point cloud sub-map is formed by stitching together a second number of keyframes before and after the current keyframe. This allows the local map to cover broader historical environmental information, while the point cloud sub-map focuses on the current local area. This strategy provides highly complete source and target point clouds for subsequent registration, enhancing the richness of environmental features and thus improving the initial alignment accuracy of multi-level registration, laying a data foundation for loopback verification.

[0178] As an optional implementation, the processing module performs multi-level voxel registration processing based on the local map and the point cloud sub-map to determine the specific method for the loop closure matching of the current keyframe, including:

[0179] Determine a decreasing sequence of multiple voxel sizes;

[0180] The point cloud of the local map and the point cloud sub-map is downsampled according to the current voxel size;

[0181] Using the previous registration result as input, the probability distribution registration algorithm is executed to obtain the transformation relationship. The algorithm is iteratively executed until the registration of all voxel sizes is completed, and the final transformation relationship is output.

[0182] The final-level transformation relationship is used to determine the loop closure matching status of the current keyframe.

[0183] This implementation employs a decreasing voxel size sequence to perform multi-level registration, achieving iterative optimization from low to high precision by progressively reducing the downsampled voxel size. Each iteration uses the registration result from the previous stage as input to a probabilistic distribution registration algorithm, utilizing a probabilistic model to model the point cloud distribution characteristics, effectively overcoming the influence of initial pose deviations on registration. Through the gradual convergence of multi-level voxel sizes, the stability of point cloud matching is improved while maintaining computational efficiency, making it particularly suitable for feature-sparse scenes such as urban canyons and tunnels. The final output transformation relationship provides high-precision pose constraints for loop closure matching.

[0184] As an optional implementation, the processing module verifies the loop closure matching effect corresponding to the current keyframe and determines the specific method of valid loop closure, including:

[0185] Calculate the matching score and occupancy ratio corresponding to the loop closure matching situation of the current keyframe;

[0186] If the matching score exceeds a preset threshold and the proportion of the match increases after matching, the corresponding loop matching relationship is determined as a valid loop.

[0187] This implementation verifies the loop closure effect using two metrics: matching score and occupancy ratio. The matching score is calculated based on the voxel distribution probability density of the local map, objectively reflecting the degree of similarity in the point cloud distribution after registration. The occupancy ratio is the percentage of voxels occupied by the point cloud sub-map that fall into the target point cloud, measuring the actual improvement in scene overlap. A valid loop closure is only determined when the matching score exceeds a threshold and the occupancy ratio increases. This dual verification mechanism significantly reduces the risk of false matching, avoids false loop closures caused by environmental similarity, and ensures the reliability of loop closure constraints in subsequent optimizations.

[0188] As an optional implementation, the processing module performs the matching score determination in the following ways:

[0189] Based on the loop closure matching, the registered and transformed point cloud sub-image is placed in the local map coordinate system;

[0190] The local map is divided into a first voxel grid, and the probability density contribution score of each point in the point cloud sub-map in the voxel distribution of the first voxel grid is calculated.

[0191] The probability density contribution scores of all points are negativeed and summed, and the total value is used as the matching score.

[0192] Furthermore, the method for determining the occupancy ratio of the processing module includes:

[0193] Establish a second voxel grid covering the local map and the point cloud sub-map point cloud;

[0194] The occupied voxels are marked according to the spatial distribution of the local map point cloud;

[0195] The proportion of points in the point cloud sub-image that fall into occupied voxels to the total number of points in the local map point cloud is calculated as the occupancy ratio.

[0196] This implementation specifically defines the calculation logic for the matching score and occupancy ratio. The matching score is obtained by accumulating the probability density contribution scores of point cloud sub-map points in the local map voxel grid, using a probabilistic model to quantify the consistency of the spatial distribution of the point cloud. The occupancy ratio is obtained by establishing a voxel grid covering the entire world and statistically analyzing the ratio of point cloud sub-map points falling into the target point cloud's occupied voxels, objectively reflecting the overlap of scene structures. The combination of these two methods evaluates the registration quality from both probabilistic distribution and geometric occupancy dimensions, providing quantifiable objective evidence for loop closure verification, replacing subjective threshold settings, and enhancing the method's universality and automation.

[0197] As an optional implementation, the processing module segments each valid loop closure, selects a reference frame, calculates the point cloud pose within each segment, and determines the specific method of loop closure constraint relationships, including:

[0198] Traverse the pose index of each target keyframe in each valid loop;

[0199] A new segment is created when the difference between the sequence numbers of adjacent target keyframes exceeds a preset segmentation threshold.

[0200] Within each segment, a preset number of optimized key frames are obtained by filtering based on the residuals of the factor graph, and a reference frame is selected from each of the optimized key frames based on the location variance data.

[0201] Using the reference frame pose as the origin, the pose of each optimized key frame within the segment is calculated according to the relative pose constraint chain between adjacent key frames.

[0202] This implementation divides continuous environmental regions into independent optimization units by segmenting effective loop closures according to pose sequence differences, adapting to the local consistency requirements of large-scene mapping. Within each segment, keyframes are selected for optimization based on factor map residuals to ensure high confidence in the data participating in optimization. A reference frame is selected based on position variance data, using a low-variance pose as the origin to improve calculation stability. The pose within each segment is calculated in a chain-like manner using relative pose constraints between adjacent keyframes, maintaining local topological invariance, providing high-precision initial values ​​for bundle adjustment, and avoiding optimization getting trapped in local optima.

[0203] As an optional implementation, the processing module optimizes the point cloud pose within each segment using bundle adjustment, including:

[0204] Based on the optimization objective, all keyframe pose parameters and planar feature parameters within each segment are simultaneously optimized.

[0205] The optimization objective includes minimizing the sum of residuals corresponding to all planar features;

[0206] The residual of each planar feature includes: the average of the squared distances from all laser measurement points that observe the planar feature to the planar feature in the global coordinate system, wherein the distance includes the normal vector projection distance from the laser measurement point to the plane containing the planar feature;

[0207] The poses of each optimized keyframe within the segment are derived from the relative pose constraint chain between adjacent keyframes as the initial poses, and optimization calculations are performed iteratively through bundle adjustment.

[0208] The optimization calculation is used to optimize the pose parameters of each of the optimization keyframes, including rotation matrix and translation vector, as well as the feature parameters of each planar feature, including normal vector and coordinates of planar reference point;

[0209] The optimization terminates when the decrease in the sum of residuals is less than the preset convergence threshold or when the maximum number of iterations is reached.

[0210] This implementation employs bundle adjustment to jointly optimize keyframe pose and planar feature parameters. The optimization objective is to minimize the sum of squared normal projection distances from all laser measurement points to their corresponding planar features, fully utilizing environmental planar structural features to enhance constraint strength. Using chained pose estimation as initial values, the rotation matrix, translation vector, and planar normal vector are iteratively optimized along with the reference point coordinates to achieve coordinated convergence of pose and map features. This strategy significantly improves the robustness of optimization in complex scenarios. Termination occurs when the total residual decreases below a threshold or reaches the maximum number of iterations, balancing computational efficiency and accuracy requirements.

[0211] It should be noted that the division of the various modules in the above device is merely a logical functional division. In actual implementation, they can be fully or partially integrated into a single physical entity, or they can be physically separated. Furthermore, these modules can be implemented entirely in software via processing element calls; they can be fully implemented in hardware; or some modules can be implemented by processing element calls to software, while others are implemented in hardware. For example, a processing module can be a separate processing element, or it can be integrated into a chip within the device. Alternatively, it can be stored as program code in the device's memory, and its functions can be called and executed by a processing element. The implementation of other modules is similar. Moreover, these modules can be fully or partially integrated together, or they can be implemented independently. The processing element here can be an integrated circuit with signal processing capabilities. During implementation, each step of the above method or each of the above modules can be completed through integrated logic circuits in the hardware of the processor element or through software instructions.

[0212] Indicatively, such as Figure 3 As shown, Figure 3 This is a schematic diagram of the internal structure of a computer device 300 provided in an embodiment of this application. The computer device 300 can be provided as a server. (Refer to...) Figure 3 The computer device 300 includes a processing component 302, which further includes one or more processors, and memory resources represented by memory 301 for storing instructions, such as application programs, that can be executed by the processing component 302. The application programs stored in memory 301 may include one or more modules, each corresponding to a set of instructions. Furthermore, the processing component 302 is configured to execute instructions to perform the methods of any of the embodiments described above.

[0213] The computer device 300 may also include a power supply component 303 configured to perform power management of the computer device 300, a wired or wireless network interface 304 configured to connect the computer device 300 to a network, and an input / output (I / O) interface 305. The computer device 300 may operate on an operating system stored in memory 301, such as Windows Server™, Mac OS X™, Unix™, Linux™, Free BSD™, or similar.

[0214] Those skilled in the art will understand that Figure 3 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0215] This application provides a storage medium storing computer-readable instructions that, when executed by one or more processors, cause the one or more processors to perform the method provided in any embodiment.

[0216] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0217] The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. The various embodiments can be combined as needed, and the same or similar parts can be referred to each other.

[0218] The above description of the disclosed embodiments enables those skilled in the art to make or use this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A loop closure optimization method, characterized in that, The method includes: Obtain keyframe point clouds and corresponding associated data; The associated data includes keyframe pose, position variance data, and factor graph residuals. Based on the keyframe point cloud, determine the current keyframe, and obtain the corresponding local map and point cloud sub-map based on the current keyframe; Based on the local map and the point cloud sub-map, using the point cloud sub-map as the source point cloud and the local map as the target point cloud, multi-level voxel registration processing is performed to determine the loop closure matching status of the current keyframe, and to verify the loop closure matching effect corresponding to the current keyframe, thereby determining the valid loop closure. The effective loopback includes the point cloud correspondence of each current key frame, as well as the associated data of each point cloud involved. After all the current keyframes have been matched, each valid loop is segmented, a reference frame is selected and the point cloud pose within each segment is calculated, the loop constraint relationship is determined, and the point cloud pose within each segment is optimized by bundle adjustment.

2. The method according to claim 1, characterized in that, The step of determining the current keyframe based on the keyframe point cloud and obtaining the corresponding local map and point cloud sub-map based on the current keyframe includes: Candidate keyframes are determined based on the keyframe point cloud and the associated data; For each of the candidate keyframes, a first keyframe and a second and third keyframe corresponding to the first keyframe are determined by spatial proximity search; If the pose number interval between the corresponding candidate keyframe and the corresponding first keyframe, second keyframe and third keyframe all exceed a preset number threshold, and the spatial distance between them is less than a preset distance threshold, then the candidate keyframe is determined as the current keyframe. A local map is obtained by stitching together the point clouds of the first keyframe, the second keyframe, and the first number of candidate keyframes surrounding the third keyframe corresponding to the current keyframe. A point cloud sub-map is then obtained by stitching together the point clouds of the second number of candidate keyframes before and after the current keyframe.

3. The method according to claim 2, characterized in that, The step of performing multi-level voxel registration processing based on the local map and the point cloud sub-map to determine the loop closure matching status of the current keyframe includes: Determine a decreasing sequence of multiple voxel sizes; The point cloud of the local map and the point cloud sub-map is downsampled according to the current voxel size; Using the previous registration result as input, the probability distribution registration algorithm is executed to obtain the transformation relationship. The algorithm is iteratively executed until the registration of all voxel sizes is completed, and the final transformation relationship is output. The final-level transformation relationship is used to determine the loop closure matching status of the current keyframe.

4. The method according to claim 1, characterized in that, The verification corresponds to the loop closure matching effect of the current keyframe, determining valid loop closures, including: Calculate the matching score and occupancy ratio corresponding to the loop closure matching situation of the current keyframe; If the matching score exceeds a preset threshold and the proportion of the match increases after matching, the corresponding loop matching relationship is determined as a valid loop.

5. The method according to claim 4, characterized in that, The matching score is determined in the following ways: Based on the loop closure matching, the registered and transformed point cloud sub-image is placed in the local map coordinate system; The local map is divided into a first voxel grid, and the probability density contribution score of each point in the point cloud sub-map in the voxel distribution of the first voxel grid is calculated. The probability density contribution scores of all points are negativeed and summed, and the total value is used as the matching score. Furthermore, the method for determining the occupancy ratio includes: Establish a second voxel grid covering the local map and the point cloud sub-map point cloud; The occupied voxels are marked according to the spatial distribution of the local map point cloud; The proportion of points in the point cloud sub-image that fall into occupied voxels to the total number of points in the local map point cloud is calculated as the occupancy ratio.

6. The method according to claim 1, characterized in that, The step of segmenting each valid loop closure, selecting a reference frame, calculating the point cloud pose within each segment, and determining the loop closure constraint relationship includes: Traverse the pose index of each target keyframe in each valid loop; A new segment is created when the difference between the sequence numbers of adjacent target keyframes exceeds a preset segmentation threshold. Within each segment, a preset number of optimized key frames are obtained by filtering based on the residuals of the factor graph, and a reference frame is selected from each of the optimized key frames based on the location variance data. Using the reference frame pose as the origin, the pose of each optimized key frame within the segment is calculated according to the relative pose constraint chain between adjacent key frames.

7. The method according to claim 6, characterized in that, The optimization of the point cloud pose within each segment using bundle adjustment includes: Based on the optimization objective, all keyframe pose parameters and planar feature parameters within each segment are simultaneously optimized. The optimization objective includes minimizing the sum of residuals corresponding to all planar features; The residual of each planar feature includes: the average of the squared distances from all laser measurement points that observe the planar feature to the planar feature in the global coordinate system, wherein the distance includes the normal vector projection distance from the laser measurement point to the plane containing the planar feature; The poses of each optimized keyframe within the segment are derived from the relative pose constraint chain between adjacent keyframes as the initial poses, and optimization calculations are performed iteratively through bundle adjustment. The optimization calculation is used to optimize the pose parameters of each of the optimization keyframes, including rotation matrix and translation vector, as well as the feature parameters of each planar feature, including normal vector and coordinates of planar reference point; The optimization terminates when the decrease in the sum of residuals is less than the preset convergence threshold or when the maximum number of iterations is reached.

8. A loop closure optimization device, characterized in that, The device includes: The acquisition module is used to acquire keyframe point clouds and their corresponding associated data. The associated data includes keyframe pose, position variance data, and factor graph residuals. The processing module is used to determine the current key frame based on the key frame point cloud, and to obtain the corresponding local map and point cloud sub-map based on the current key frame. The processing module is further configured to perform multi-level voxel registration processing based on the local map and the point cloud sub-map, using the point cloud sub-map as the source point cloud and the local map as the target point cloud, to determine the loop closure matching status of the current keyframe, and to verify the loop closure matching effect corresponding to the current keyframe, and to determine the valid loop closure. The effective loopback includes the point cloud correspondence of each current key frame, as well as the associated data of each point cloud involved. The processing module is further configured to segment each valid loop after all current keyframes have been matched, select a reference frame and calculate the point cloud pose within each segment, determine the loop constraint relationship, and optimize the point cloud pose within each segment using bundle adjustment.

9. A computer device, characterized in that, The method includes one or more processors and a memory storing computer-readable instructions that, when executed by the one or more processors, perform the steps of the method as described in any one of claims 1-7.

10. A storage medium, characterized in that, The storage medium stores computer-readable instructions that, when executed by one or more processors, cause the one or more processors to perform the steps of the method as described in any one of claims 1-7.

Images