Lidar loop detection method and system based on multi-level feature representation and spatio-temporal joint mining

Abstract

The application discloses a laser radar loop detection method and system based on multi-level feature representation and space-time joint mining, and the method comprises the following steps: 1) performing preprocessing on a current frame of original three-dimensional point cloud collected by a laser radar to obtain a structured sparse point cloud; 2) performing double-driven key point detection on the point cloud after preprocessing to obtain a three-dimensional key point set with high geometric discriminability; 3) constructing a distance-angle double-mode local feature descriptor for each three-dimensional key point; 4) constructing a multi-level global scene representation to obtain a frame-level statistical aggregation descriptor; 5) performing layered retrieval and geometric consistency verification; and 6) performing space-time joint consistency mining and frame-by-frame optimal constraint output. The application has good performance in the three dimensions of feature distinguishability, retrieval efficiency and time sequence modeling.

Description

Technical Field

[0001] This invention relates to the field of environmental perception technology for autonomous driving and mobile robots, and in particular to a method and system for realizing loop closure detection using lidar. Background Technology

[0002] In Simultaneous Localization and Mapping (SLAM) systems for autonomous vehicles and mobile robots, loop closure detection is a core component for eliminating cumulative localization drift and building a globally consistent map. When a mobile platform revisits a previously visited area, the loop closure detection system needs to accurately identify the "revisit" event and generate spatial constraints between the current observation and historical observations, thereby providing closed-loop edges for backend graph optimization.

[0003] In existing technologies, loop closure detection methods based on LiDAR are mainly divided into two categories. One is based on local geometric feature matching, which extracts key points from point clouds and calculates local descriptors, using descriptor matching and geometric consistency verification to detect loop closures. However, when facing large-scale outdoor scenes, the sheer number of key points leads to excessively high matching computation costs, and local descriptors at a single scale are difficult to simultaneously ensure discriminability and viewpoint invariance.

[0004] The second approach is scene recognition based on global descriptors, which compresses the entire frame's point cloud into a global feature vector and uses vector distance to quickly retrieve similar frames to detect loop closures. Although this type of method has high retrieval efficiency, the high compression of 3D scene information inevitably leads to information loss. It also lacks the ability to discriminate complex scenes such as reverse driving, partial occlusion, and interference from dynamic objects, and is prone to producing incorrect matches.

[0005] More importantly, both of the above methods treat single-frame point clouds as independent processing units, ignoring the inherent continuity of loop events in the temporal dimension—real loops are usually a series of consecutive frames sharing similar geometric constraints. Although some existing technologies have attempted to introduce temporal information, they mostly employ simple post-processing smoothing or threshold filtering, lacking in-depth modeling of temporal structures and effective means to handle complex temporal patterns such as brief loops (e.g., passing through intersections) and inter-frame interval fluctuations caused by speed changes. Summary of the Invention

[0006] This invention aims to address the shortcomings of existing technologies, such as insufficient local feature distinguishability, difficulty in balancing global retrieval accuracy and efficiency, and high short loop detection rate due to lack of temporal structure modeling. It provides a lidar loop detection method and system based on multi-level feature representation and spatiotemporal joint mining.

[0007] This invention addresses the aforementioned technical deficiencies by constructing a multi-level progressive processing architecture of "key point detection—local feature description—global scene representation—hierarchical retrieval and verification—spatiotemporal joint mining." It also introduces key point detection driven by dual saliency, distance-angle dual-modal local features, annular-sector partitioning multi-attribute global representation, and a multi-level temporal post-processing mechanism that includes optimal path dynamic programming trajectory mining and independent rescue channels for neighborhood consistency voting. This fundamentally overcomes the shortcomings of existing technologies in feature discriminativeness, retrieval efficiency, and temporal modeling.

[0008] The core idea of ​​this invention is as follows: At the feature level, high-discriminative 3D keypoints are screened through a dual saliency measure of geometric offset and regional density, and nonlinear enhancement. Rotation-adaptive local feature descriptors are constructed using a distance-angle dual-modal histogram. At the retrieval level, rapid coarse-grained candidate recall is achieved through multi-attribute global scene representation of ring-sector partitioning, followed by fine re-sorting using frame-level statistical descriptors, forming a progressive retrieval from coarse to fine. At the verification level, a rotation-invariant backoff mechanism with descriptor angle dimension folding and iterative nearest-point refinement are introduced to enhance the robustness of geometric verification. At the temporal level, a multi-level spatiotemporal joint post-processing system is constructed, including optimal path dynamic programming trajectory mining, homogeneous trajectory completion, elastic prediction corridor extension, and independent rescue channels for neighborhood consensus voting. This enables the system to obtain high-confidence loop closures through long-term temporal constraints of continuous trajectories and capture transient, isolated loop closure events through local neighborhood voting.

[0009] To achieve the above objectives, this invention provides a lidar loop closure detection method based on multi-level feature representation and spatiotemporal joint mining, comprising the following steps: S1: Preprocess the original 3D point cloud of the current frame acquired by the lidar, including ground point cloud segmentation and removal, region of interest filtering, voxel downsampling, and outlier removal, to obtain a structured sparse point cloud; the region of interest refers to a region centered on the lidar with a horizontal radius of... Vertical height range The cylindrical spatial range; S2: Perform geometric saliency dual-driven keypoint detection on the preprocessed point cloud to obtain a set of 3D keypoints with high geometric discriminative power; S3: For each 3D keypoint, construct a descriptor for the local features in a distance-angle dual-modality configuration; S4: Constructing a multi-level global scene representation: On the one hand, projecting non-ground point clouds onto a polar coordinate bird's-eye view plane to construct a representation of the scene. a radial ring and A ring-sector partitioned grid is formed by sectors at various angles. Each grid cell calculates a multi-attribute joint encoding including height, density, span, contrast, and reliability features. Based on the statistical information of each ring and sector, ring feature vectors and sector feature vectors are generated respectively. On the other hand, the local feature descriptors of all key points in the current frame are averaged dimension by dimension to obtain frame-level statistical aggregated descriptors. S5: Perform hierarchical retrieval and geometric consistency verification, filter candidate frames using the loop feature vector and frame-level statistical aggregation descriptor, and perform matching and pose estimation based on the local feature descriptor to determine whether it is a valid geometric loop closure; S6: Perform spatiotemporal joint consistency mining and frame-by-frame optimal constraint output. Search for temporal consistency trajectories through dynamic programming, and evaluate isolated candidates through a neighborhood consistency voting mechanism. Finally, output an optimal loop constraint for each frame query.

[0010] Preferably, the preprocessing in step S1 specifically includes: S11: Ground segmentation based on polar coordinate grid: The original point cloud is mapped to a polar coordinate grid according to the geometric characteristics of the LiDAR scan. Line segments are fitted to points in the radial interval within each angular sector, and the distance fitted line segments are less than a first preset threshold. The points were marked as ground points and then removed; S12: Filtered cylindrical region of interest: Centered on the lidar, only the horizontal projection radius within the cylindrical region is retained. Within the range and the vertical height is within the preset height range The point between; S13: Voxel hash grid downsampling: Divide the point cloud space within the region of interest into sections with sides equal to the voxel downsampling size. The three-dimensional mesh cells retain only one representative point per cell. Fast deduplication is achieved through hash mapping, and the point cloud resolution parameters are adaptively calculated based on the voxel size. ; S14: Outlier Removal Based on Radius Search: For each point in the downsampled point cloud, the outlier search radius is used. Perform a neighborhood search; if the number of points in the neighborhood is less than the minimum number of neighbors of the outlier, then... If so, mark the point as an outlier and remove it.

[0011] Preferably, step S2 includes: calculating the geometric offset significance and regional structural complexity of each spatial point, performing nonlinear contrast enhancement on both separately, fusing them into a joint significance, and then using adaptive thresholding and spatial nonmaximum suppression.

[0012] Preferably, step S2 specifically includes the following steps: S21: Calculate the significance of geometric offset For the i-th point in the point cloud With that point as the center, the geometric offset search radius Search for neighboring points within a spherical neighborhood and calculate their spatial centroids. The normalized Euclidean distance from the point to the centroid is used as... Measurement The value is large at corners and edges, and small in flat areas; S22: Calculate the structural complexity of the domain : Utilizing pre-computed gridded neighborhood density information, in the context of Centered on the search radius of the region Within a spherical neighborhood, the exponential decay model is used to calculate... The model is significant based on the average geometric shift within the neighborhood. Local point density of the spatial unit The ratio is an exponentially decaying variable. It approaches 1 in structurally complex regions and approaches 0 in open regions; S23: Double significance nonlinear enhancement: respectively for and Perform a nonlinear transformation, and let the global maximum value be... The mean of the remaining local maxima is The square of the difference between the global maximum and the mean of the remaining local extrema is used as an enhancement factor to weight and amplify the original significance, highlighting the contrast between the structure and the background. S24: Screening Joint Saliency Fusion and Key Points: This will enhance the... and Summation yields joint significance The global mean of joint significance For adaptive thresholding, filtering Not less than Candidate points, and suppressing radius at nonmaximum values. Non-maximum suppression in the execution space is used to preserve local maxima.

[0013] Preferably, step S3 includes: estimating the local surface normal vector of each point in the point cloud, then generating a neighborhood distribution histogram by statistically analyzing the distance distribution and the angle distribution between the normal vectors of the points in the neighborhood of each key point, and then performing neighborhood aggregation weighted fusion and angle dimension folding to enhance rotation invariance on the neighborhood distribution histogram of each key point to obtain the descriptor.

[0014] Preferably, the calculation of the distance-angle bimodal local feature descriptor in step S3 specifically includes: S31: Estimate the local surface normal vector: For each point, estimate the radius of the normal vector. Within the spherical neighborhood, the covariance matrix is ​​constructed and eigenvalue decomposition is performed. The eigenvector corresponding to the smallest eigenvalue is taken as the normal vector and uniformly flipped to point to the origin of the sensor coordinate system. S32: Construct a neighborhood distribution histogram: For each key point, within the feature search radius... Search for neighboring points within a spherical neighborhood; calculate the normalized distance and divide it uniformly. Each distance interval; calculate the cosine value of the direction angle and divide it into equal-angle intervals using non-uniform cosine division. Each distance-angle interval is defined; the number of neighboring points falling into each distance-angle interval combination is counted, and the result is normalized to form... OK Two-dimensional neighborhood distribution histogram of columns ; S33: Neighborhood Aggregation Weighted Fusion: For each key point, its own histogram... and The histograms of all neighboring keypoints are weighted and summed according to distance decay weights, with closer neighboring keypoints having higher weights. After fusion and normalization, a distance-angle bimodal local feature descriptor is obtained. ; S34: Descriptor Angular Dimension Folding: For each distance interval The histogram statistics of two intervals that are centrally symmetric in the angular dimension are summed and merged to give the descriptor tolerance to viewpoint rotation.

[0015] Preferably, the ring-sector partitioned grid in step S4 consists of multiple radial rings and multiple angular sectors. The multi-attribute joint encoding includes height features, density features, span features, contrast features, and reliability features, and generates ring feature vectors and sector feature vectors based on the statistical information of each ring and each sector.

[0016] Preferably, the construction of the multi-level global scene representation in step S4 specifically includes: S41: Construct a multi-attribute global descriptor for a circular sector partition: Project the non-ground point cloud onto polar coordinate space, within the polar radius range. With polar angle range The interior is divided into a radial ring and Each grid cell is divided into five angle sectors; each grid cell calculates height, density, span, contrast, and reliability features to form a five-channel multi-attribute joint encoding. S42: Generates aggregated values ​​of cell codes weighted by reliability characteristics within each radial loop. 3D ring eigenvector Generates the aggregated value of the unit code, weighted by reliability characteristics, within each sector from different angles. 3D sector feature vector ; Used for coarse-grained candidate recall Used for yaw angle deviation estimation; S43: Frame-level statistical aggregation descriptor: The arithmetic mean of the local feature descriptors of all keypoints in the current frame is taken element-wise along the keypoint dimension to obtain a frame-level vector with the same dimension as a single local feature descriptor. .

[0017] Preferably, step S5 includes: using the loop feature vector and the loop feature vectors of all frames in the historical frame database to perform fast distance calculation to filter out coarse-grained candidate frames; then using frame-level statistical aggregation descriptors to perform fine distance reordering on the coarse-grained candidate frames, and selecting candidate frames from them; for each candidate frame, using the local feature descriptors of key points to perform bidirectional nearest neighbor matching and ratio testing, and based on the matching point pairs, using rigid body transformation estimation with random sampling consistency and iterative nearest point refinement to obtain the relative pose transformation and interior point statistics of the candidate frame relative to the current frame, and determining whether it is a valid geometric loop through the candidate screening and scoring mechanism.

[0018] Preferably, the steps specifically include the following steps: S51: Coarse-grained candidate recall: The loop feature vector of the current frame... Calculate the Euclidean distance between the frame and the loop feature vectors of all historical frames in the database, and retain the previous frames in ascending order of distance. Select candidate frames and exclude those with a frame number difference less than the preset minimum interval. The nearest neighbor frames; S52: Fine-grained reordering: Frame-level statistical aggregation of descriptors for coarse-grained candidate frames. Recalculate the Euclidean distances and sort them, keeping the top results. Each candidate frame is entered into geometric verification, while the top-ranked frames from the coarse search are also retained. One candidate frame; S53: Keypoint Matching and Ratio Check: The local feature descriptor of each keypoint in the query frame is compared with the descriptors of all keypoints in the candidate frames using a nearest neighbor search. If the nearest distance... and the second closest distance The ratio is less than the preset ratio threshold. If the match is found, the match is accepted; otherwise, it is rejected. S54: Rigid Body Transformation Estimation: Randomly sample from matched pairs that pass the ratio test, and solve for the rigid body transformation using singular value decomposition. Inner point distance threshold Count the number of matched inner points ,through The next iteration retains The most frequent transformations; when or interior point ratio When the threshold is lower than the weak validation condition, an angle dimension folding back mechanism is triggered, and the matching and transformation estimation are re-executed using the folded descriptor. Furthermore, when the number of interior points or the interior point rate obtained from the standard geometric validation is lower than the preset weak validation threshold, the descriptor angle dimension folding back mechanism is triggered: after performing angle dimension folding operations on the local feature descriptors of both the query frame and the candidate frame according to formula (8), the key point matching described in step S53 and the rigid body transformation estimation described in step S54 are re-executed. The folded descriptor reduces the sensitivity to viewpoint rotation through symmetrical summation of the angle dimension, and can capture loops that the standard descriptor fails to match under large viewpoint rotation.

[0019] S55: Using the optimal transformation of S54 as the initial value, perform iterative nearest-point refinement using the correspondence between interior points until the translation and rotation changes are lower than the convergence threshold or the maximum number of iterations is reached. S56: Candidate Screening and Scoring: Passed Not less than the preset minimum number of interior points After joint screening where the inlier rate is not lower than the preset minimum and the translation amount does not exceed the preset maximum, the geometric confidence score of the candidates is calculated using a weighted scoring function that combines the number of inliers and the inlier ratio. .

[0020] Preferably, step S6 includes: using the loop feature vector and the loop feature vectors of all frames in the historical frame database to perform fast distance calculation to filter out coarse-grained candidate frames; then using frame-level statistical aggregation descriptors to perform fine distance reordering on the coarse-grained candidate frames, and selecting candidate frames from them; for each candidate frame, using the local feature descriptors of key points to perform bidirectional nearest neighbor matching and ratio testing, and based on the matching point pairs, using rigid body transformation estimation with random sampling consistency and iterative nearest point refinement to obtain the relative pose transformation and interior point statistics of the candidate frame relative to the current frame, and determining whether it is a valid geometric loop through the candidate screening and scoring mechanism.

[0021] Preferably, step S6 specifically includes the following steps: S61: Mining the Dual-Mode Optimal Path Dynamic Programming Trajectory: Defining Two Temporal Modes and Their Inter-Frame Index Constants: Forward Mode Constants To subtract the matching frame number from the query frame number, the corresponding path segment is retraced in the same direction; this is a constant in reverse mode. To query the frame sequence number and add the matching frame sequence number, corresponding to the same path segment traversed in the opposite direction; construct a dynamic programming state table to record the cumulative score, cumulative number of interior points, and length of the optimal time series chain ending at each candidate frame. The state transition must satisfy the constant deviation not exceeding the basic tolerance value. The constraints include the sum of the frame interval compensation values ​​and the fact that the frame interval does not exceed the preset maximum frame interval. The constraint that the transfer score is determined by the frame interval reward item The penalty term is calculated by subtracting the constant deviation penalty term, and the penalty coefficient is... For candidate chains that meet the minimum length, minimum average number of inliers, minimum cumulative score, and minimum average inlier rate, output a time-consistent trajectory and remove duplicates based on the median constant value. S62: Complete the homogeneous trajectory and extend the elastic prediction corridor: For each accepted temporally consistent trajectory, search for inliers within its query frame range with a search rate not lower than the first lenient threshold. Furthermore, frames whose constant deviation does not exceed the extended tolerance range are included in the trajectory; for trajectories that meet the preset intensity conditions, geometric verification is actively initiated within a preset neighborhood centered on the predicted matching frame to expand the candidates; and the number of nodes is not less than the preset minimum number of nodes. The trajectory is used to predict the expected matching frame position of each query frame in the extended region before and after by piecewise linear interpolation of existing nodes, within a preset search window width. Internal deviation penalty The optimal candidate is searched by the best score, and the extended trajectory is used to replace the original trajectory when the number of nodes after extension meets the condition. S63: Implement a neighborhood consensus voting mechanism: Candidates must satisfy either the standard path or the relaxed path. The standard path requires... and Each entry threshold shall not be lower than the preset standard threshold, with lenient path requirements. and Each candidate must meet or exceed a preset, lenient entry threshold; within the local neighborhood window of the query frame, the number of other candidates with a consistent constant relationship to the candidate is counted as the temporal support votes. , It must meet either the standard support threshold or the relaxed support threshold; finally, the geometric confidence score will be applied. Adding the weight of the supporting votes Multiply Subtract the ranking penalty factor. Multiply by the candidate's ranking number within the same frame to obtain the overall score. If the query frame does not yet have an optimal constraint, the current candidate is written directly; if an optimal constraint already exists and Exceeding the optimal score and preset replacement margin The sum of these constraints is used to replace the original constraint with the current candidate. S64: Output Local Smoothing Filter and Frame-by-Frame Optimal Constraint: For each pair of query frames with adjacent sequence numbers, check whether the jump amplitude of the matching frame sequence number in its optimal loop closure constraint exceeds the preset jump threshold. If it exceeds, then compare the loop closure constraints of the two frames. and The one with higher overall confidence is retained; the optimal closure constraint is output frame by frame.

[0022] The second aspect of this invention relates to a lidar loop closure detection system based on multi-level feature representation and spatiotemporal joint mining, used to implement the method of this invention, comprising: Point cloud preprocessing module: used to perform ground segmentation, cylindrical spatial region filtering, voxel downsampling and outlier removal on the raw 3D point cloud acquired by lidar; Keypoint detection module: used to perform dual saliency-driven keypoint detection on the preprocessed point cloud; Local Feature Description Module: Used to calculate a distance-angle bimodal local feature descriptor for each keypoint; Global scene representation module: used to construct multi-level global descriptors, including ring-sector partition multi-attribute global descriptors and frame-level statistical aggregation descriptors; Hierarchical retrieval and geometric verification module: used to perform coarse-to-fine candidate retrieval and geometric consistency verification; Spatiotemporal joint mining and output module: used to perform temporal consistency analysis and frame-by-frame optimal output on candidates for geometric verification; Database management module: Used to store and manage multi-level descriptor data for all historical frames.

[0023] This invention has the following innovative features: 1. Feature Domain Innovation: Keypoint Detection and Distance-Angle Dual-Modal Local Feature Representation Driven by Dual Saliency Addressing pain points: Traditional LiDAR 3D key point detection methods mostly rely on a single geometric metric (such as curvature, normal vector change, etc.), which is insufficient in distinguishing geometric structures of different scales and types in complex outdoor scenes, and the local descriptors have limited robustness to viewpoint rotation.

[0024] Core originality: (a) A dual significance metric of geometric offset significance and regional structural complexity is proposed. The former uses formula (1) to capture the degree of local centroid deviation of a point to locate corners and edges of geometric abrupt changes, while the latter uses the exponential decay model of formula (2) to assess the richness of regional geometric structure to suppress false alarms in open areas. The two are then fused using formula (4) after a nonlinear contrast enhancement transformation according to formula (3). The factor significantly amplified the distinction between the most salient structures and the background.

[0025] (b) Propose a distance-angle dual-modal local feature descriptor—constructed from the distance distribution and the angle distribution between the normal vectors. The two-dimensional histogram achieves equidistant sampling of the angle dimension by non-uniform cosine domain segmentation without explicit inverse cosine operation, and smooths local noise interference by neighborhood aggregation weighted fusion (attenuation weight 1 / (1+d)) of formula (7).

[0026] (c) Propose the descriptor angle dimension folding operation of formula (8) - by symmetric summation of the angle dimension, the descriptor is given partial invariance to in-plane rotation without increasing the descriptor dimension.

[0027] 2. Representation Domain Innovation: Multi-attribute Global Scene Representation and Hierarchical Progressive Retrieval in Ring-Sector Partitioning

[0028] Addressing pain points: Existing global descriptors typically compress point cloud information into a single type of feature or a single-scale dimensionality reduction vector, resulting in significant information loss; while multi-level retrieval methods often rely on a single global descriptor, making it difficult to achieve a good balance between retrieval efficiency and retrieval accuracy.

[0029] Core originality: (a) Proposes a multi-attribute global scene representation with ring and sector partitions: establish a partitioned grid of radial rings and angular sectors in polar coordinate bird's-eye view projection space. Each unit simultaneously encodes five complementary attributes: height, density, span, contrast and reliability, forming a five-channel multi-attribute joint encoding, which has a stronger scene discrimination capability compared with a single feature channel.

[0030] (b) Based on this multi-attribute representation, two dimensionality reduction retrieval keys, ring feature vector and sector feature vector, are derived simultaneously through formula (10). The former is used for fast coarse-grained candidate recall (O(N) complexity) in formula (12), and the latter is used for yaw angle deviation estimation, thus realizing the reuse of multiple retrieval functions under the same representation system.

[0031] (c) Construct a hierarchical progressive retrieval consisting of coarse recall of ring feature vectors (Formula 12), fine rearrangement of frame-level statistical aggregation descriptors (Formula 13), and coarse retrieval with channel retention. Coarse screening is completed with linear scanning with low computational overhead, and precise screening is completed with statistical aggregation descriptors with high discriminative power.

[0032] 3. Validation Domain Innovation: Adaptive Multi-Strategy Geometric Consistency Validation

[0033] Addressing pain points: Existing geometric verification methods typically employ fixed matching strategies and single verification paths, making it difficult to handle loop scenarios with varying levels of difficulty, such as large-view rotation, partial occlusion, and sparse point clouds.

[0034] Core originality: (a) The nearest neighbor ratio test mechanism of formula (14) is used to filter the ambiguity of descriptor matching - only the reliable matching pairs with uniqueness (nearest distance is significantly smaller than second nearest distance) in the descriptor space are retained, thereby reducing the interference of random false matching from the source.

[0035] (b) Establish a descriptor folding back verification path - when the standard descriptor matching fails to generate sufficient interior point support, automatically switch to the descriptor after folding the angle dimension of formula (8) for rematching and rigid body transformation estimation of formula (15), and use the tolerance of the folded descriptor to rotation to capture the large angle loops missed by the standard path.

[0036] (c) The random sampling consensus in-point voting mechanism of formula (15) is combined with iterative nearest point refinement. While ensuring the global optimal transformation search, the transformation estimation accuracy is improved through local iterative optimization. The weighted scoring function of formula (16) integrates the two dimensions of absolute number of in-points and relative proportion.

[0037] 4. Temporal Domain Innovation: Spatiotemporal Joint Multi-Level Consistency Mining

[0038] Addressing the pain points: Existing loop closure detection methods mostly treat each frame as an independent processing unit, ignoring the essential characteristic that real loop closures are presented as a continuous frame sequence sharing a consistent spatial constraint relationship in time; in particular, short-lived isolated loop closures (such as quickly passing through intersections, U-turns, etc.) are easily missed because they do not meet the continuous frame length requirements.

[0039] (a) Propose a dual-mode optimal path dynamic programming trajectory mining method using formulas (17) and (18) - Simultaneously model the inter-frame index constancy constraints under two physical scenarios: forward passage (qm constant) and reverse passage (q+m constant). Use the transition score (frame interval reward minus constant deviation penalty) of formula (19) for dynamic programming global optimization, and automatically discover continuous loop trajectories that satisfy long-term temporal consistency from the candidate matrix.

[0040] (b) In the trajectory enhancement system consisting of homologous trajectory completion and elastic prediction corridor extension, nodes that are missed due to conservative parameters within the trajectory coverage area are recovered with a relaxed threshold, and the linear prediction of formula (20) guides the search to expand the exploration space at both ends of the trajectory.

[0041] (c) The most innovative aspect is the independent neighborhood consensus voting rescue channel in formula (21)—this channel does not compete with trajectory mining, but instead targets isolated but geometrically reliable loops that trajectory mining abandons due to its conservatism, assigning temporal confidence by conducting constant consensus voting within the local time neighborhood. Its overall score = geometric score + support weight × number of votes ( (Typical value of 4.0) allows for a score improvement of +8.0 even with only two frames of neighborhood support, thus "rescuing" isolated loops with independent evidence of temporal consistency without relaxing the geometric verification threshold. This trajectory mining-neighborhood voting dual-channel parallel, competitive selection architecture fundamentally solves the dilemma of traditional methods "choosing one over the other" between continuous long loops and isolated short loops.

[0042] The beneficial effects of this invention are: 1. Balancing high precision and high recall: By screening geometric features with strong discriminative power through dual saliency keypoint detection (Equations 1-5), combined with distance-angle bimodal local descriptors (Equation 7) to provide rich local structural information, and multi-attribute global scene representation to provide complementary global perspectives, the system can maintain high precision (low false positive rate) while having a high recall rate for difficult loop scenarios such as reverse driving, large-view rotation, and partial occlusion.

[0043] 2. Optimal balance between retrieval efficiency and accuracy: The ring feature vector derived from the multi-attribute global representation of the ring sector partition (Equation 10) supports the O(N) linear scan coarse recall of Equation (12). The frame-level statistical aggregation descriptor (Equation 11) performs fine rearrangement with minimal computational overhead of Equation (13) on the basis of coarse recall, enabling the system to achieve real-time candidate retrieval in a database of tens of thousands of frames, while retaining channels through coarse retrieval. Avoid recall losses caused by misjudgment due to a single descriptor.

[0044] 3. Robust geometric verification: The nearest neighbor ratio test in formula (14) suppresses ambiguity from the matching source, the descriptor folding back-off mechanism in formula (8) provides an adaptive backup verification path for large-angle rotation scenarios, and the combination of random sampling consistency and iterative refinement in formulas (15) and (16) ensures the accuracy and robustness of the transformation estimation, enabling the system to reliably verify loop closure even without initial pose priors.

[0045] 4. Spatiotemporal Joint Utilization to Eliminate Isolated Frame Misjudgments: A multi-level temporal post-processing system—including dual-mode dynamic programming trajectory mining (17)-(19), homogeneous completion, elastic prediction (20), and neighborhood consensus voting (21)—fully utilizes the temporal structure of loop events from both continuous and isolated dimensions. Trajectory mining provides high-confidence loops with long-term temporal constraints, while neighborhood voting independently captures short-lived isolated effective loops. The two operate in parallel and compete for the best, fundamentally eliminating the uncertainty of single-frame geometric verification in the temporal dimension.

[0046] 5. Complete end-to-end solution: From raw 3D point cloud input to frame-by-frame optimal loop closure constraint output, this invention provides a complete end-to-end processing pipeline that requires no external positioning information or initial pose estimation and can be directly embedded into various LiDAR SLAM systems as a general loop closure detection module. Attached Figure Description

[0047] Figure 1 This is an overall flowchart of the method of the present invention, showing the progressive relationship of the six core steps from S1 to S6.

[0048] Figure 2 This is a schematic diagram illustrating the geometric meaning of double significance and the nonlinear enhancement effect in keypoint detection (corresponding to formulas 1-5).

[0049] Figure 3 This is a schematic diagram of the histogram structure and neighborhood aggregation process of the distance-angle bimodal local feature descriptor (corresponding to formula 6-8).

[0050] Figure 4 This is a schematic diagram of the structure of the global scene representation of the ring sector partition with multiple attributes (corresponding to formula 9-11).

[0051] Figure 5 This is a flowchart of hierarchical retrieval and geometric consistency verification (corresponding to formula 12-16).

[0052] Figure 6 This is a schematic diagram of time-series trajectory mining using dynamic programming for dual-mode optimal paths (corresponding to formulas 17-19).

[0053] Figure 7 This is a schematic diagram of the neighborhood consensus voting mechanism (corresponding to formula 21).

[0054] Figure 8 This is a schematic diagram of the loop closure detection effect of the method of the present invention on the KITTI08 dataset, showing the spatial distribution of the loop closure constraints detected on the complete vehicle driving trajectory.

[0055] Figure 9 This is a schematic diagram illustrating the loop closure detection performance of the BTC method on the KITTI08 dataset. Detailed Implementation

[0056] The technical solution of the present invention will now be described with reference to the accompanying drawings.

[0057] Example 1

[0058] This embodiment relates to a lidar loop closure detection method based on multi-level feature representation and spatiotemporal joint mining, including the following steps: S1: Preprocess the original 3D point cloud of the current frame acquired by the lidar, including ground point cloud segmentation and removal, region of interest filtering, voxel downsampling, and outlier removal, to obtain a structured sparse point cloud; the region of interest refers to a region centered on the lidar with a horizontal radius of... Vertical height range The cylindrical spatial range; S2: Perform geometric saliency dual-driven key point detection on the preprocessed point cloud: calculate the geometric offset saliency and regional structure complexity of each spatial point, perform nonlinear contrast enhancement on the two respectively and fuse them into joint saliency, and then obtain a set of three-dimensional key points with high geometric discriminative power through adaptive threshold screening and spatial nonmaximum suppression. S3: For each 3D keypoint, construct a distance-angle bimodal local feature descriptor: First, estimate the local surface normal vector of each point in the point cloud. Then, with the keypoint as the center, calculate the distance distribution and the angle distribution between the normal vector and the neighboring points to generate a 2D histogram as the neighborhood distribution histogram. Then, perform neighborhood aggregation and weighted fusion on the neighborhood distribution histogram of each keypoint, and enhance rotation invariance by folding the angle dimension to obtain the distance-angle bimodal local feature descriptor. S4: Constructing a multi-level global scene representation: On the one hand, projecting non-ground point clouds onto a polar coordinate bird's-eye view plane to construct a representation of the scene. a radial ring and A ring-sector partitioned grid is formed by sectors at various angles. Each grid cell calculates a multi-attribute joint encoding including height, density, span, contrast, and reliability features. Based on the statistical information of each ring and sector, ring feature vectors and sector feature vectors are generated respectively. On the other hand, the local feature descriptors of all key points in the current frame are averaged dimension by dimension to obtain frame-level statistical aggregated descriptors. S5: Perform hierarchical retrieval and geometric consistency verification: First, use the ring feature vector and the ring feature vectors of all frames in the historical frame database to perform fast distance calculation, and filter out the top... A coarse-grained candidate frame is generated; then, a fine-grained distance reordering is performed on the coarse-grained candidate frames using frame-level statistical aggregation descriptors, from which the top-ranked frames are selected. There are 10 candidate frames. For each candidate frame, bidirectional nearest neighbor matching and ratio test are performed using the distance-angle dual-modal local feature descriptor of key points. Based on the matching point pairs, the rigid body transformation estimation and iterative nearest point refinement through random sampling consistency are used to obtain the relative pose transformation and interior point statistics of the candidate frame relative to the current frame. The candidate screening and scoring mechanism is used to determine whether it is a valid geometric loop. S6: Execute spatiotemporal joint consistency mining and frame-by-frame optimal constraint output: First, organize all geometric loop candidates between frames into a candidate matrix. Use dual-mode optimal path dynamic programming to search for continuous temporally consistent trajectories that satisfy the inter-frame index constraining in the candidate matrix, and perform source trajectory completion and elastic prediction corridor extension for each trajectory. Then, for isolated candidates not included in any trajectory, evaluate them through a neighborhood consistency voting mechanism. Calculate the number of temporal consistency support votes obtained by each candidate in the local neighborhood, and weight the support votes to the candidate's geometric score to obtain a comprehensive score, which competes with the trajectory output for selection. Finally, after local smoothing filtering, output an optimal loop constraint for each frame query.

[0059] The preprocessing in step S1 specifically includes: S11: Ground segmentation based on polar coordinate grids—The original point cloud is mapped to a polar coordinate grid according to the geometric characteristics of the LiDAR scan. The radial dimension of the grid is divided into... Each radial interval is divided into angular dimensions. Each angle sector is divided into several angle sectors. Within each angle sector, line segments are fitted to points on the radial interval. The vertical distance from each point to the fitted line segment is calculated, and points with distances less than the ground distance threshold are selected. Points that are not ground points are marked as ground points and removed, while non-ground points are retained for subsequent processing.

[0060] S12: Cylindrical Region of Interest Filtering – Centered on the lidar, only retaining the horizontal projection radius within... Points within the range, while limiting the vertical height. between.

[0061] S13: Voxel hash grid downsampling—dividing the point cloud space within the region of interest into grids with sides of length 1. The three-dimensional mesh cells retain only one representative point per cell. O(1) deduplication is achieved through hash mapping, and the point cloud resolution parameters are adaptively calculated based on the voxel size. = This serves as an adaptive benchmark for subsequent search radii.

[0062] S14: Outlier Removal Based on Radius Search – For each point in the downsampled point cloud, use a radius search… Perform a neighborhood search; if the number of points in the neighborhood is less than the preset minimum number of neighbors... If so, mark the point as an outlier and remove it.

[0063] Step S2, the dual-driven keypoint detection based on geometric saliency, specifically includes: S21: Calculate the significance of geometric offset. For the i-th point in the point cloud... In the Centered on, with radius Search all neighboring points within the spherical neighborhood and calculate the spatial centroid of the neighboring points. Then the significance of geometric offset is defined as: Geometric offset significance: (1) This metric characterizes a point. The degree of deviation from its local geometric center—at corner and edge locations, the distribution of neighboring points is asymmetrical, and the centroid deviates significantly from the point itself. The value is relatively large; in flat regions, neighboring points are symmetrically distributed around the point, and the centroid is close to the point itself. The value is relatively small. Among them, the radius Determined adaptively based on point cloud resolution: , The preset geometric offset radius factor, The point cloud resolution parameters are calculated in step S13.

[0064] S22: Calculate the structural complexity of the region. Utilize pre-calculated gridded neighborhood density information, in a... Centered on, with radius The number and density distribution of statistically effectively occupied grid cells within a larger spherical neighborhood are analyzed, and the structural complexity of the region is calculated using an exponential decay model. Region structure complexity: (2) in The significance of the average geometric offset of each grid cell in the neighborhood of this point reflects the overall geometric complexity of the region; This represents the local point density of the grid cell containing that point. The physical meaning of this model is: in structurally complex areas (such as building facades and vegetation),... Gao Er Approaching 1; in open areas (such as open roads). low and Approaching 0. Radius , This is the preset region radius factor.

[0065] S23: Double significance nonlinear enhancement. (The remaining text appears to be incomplete and requires further context.) and Perform the following nonlinear transformation: First, search for all local maxima points of each element in the point cloud space; let the global maximum value be... The mean of the remaining local maxima is Then the significance values ​​of all points are enhanced by the following transformation: Nonlinear enhancement: (3) The property of this transformation is: if the global maximum value is much higher than other local extrema (i.e., there are a few anomalously significant salient structures), then A large value for the factor significantly amplifies the global significance contrast; however, if the local extrema are evenly distributed (i.e., the scene structure is uniform), the factor's value is limited, and its enhancement is also limited. This can be further analyzed by examining the values ​​of the factors. and The above transformation yields the enhanced geometric shift significance. and enhanced regional structural complexity .

[0066] S24: Joint Saliency Fusion and Keypoint Screening. Joint saliency is defined as the sum of two enhancement measures: Joint significance: (4) Use the global mean of joint significance as an adaptive threshold: Only keep Points with a radius of [missing information] are selected as keypoint candidates. Based on this, [missing information] points with a radius of [missing information] are selected as keypoint candidates. Spatial nonmaximum suppression is performed within the spherical neighborhood: if the joint significance of a candidate point is lower than any other candidate point in its neighborhood, that point is eliminated. The final set of retained keypoints is then determined. for: Key points set: (5)

[0067] The calculation of the distance-angle bimodal local feature descriptor in step S3 specifically includes: S31: Local surface normal vector estimation. For each point in the point cloud, the local surface normal vector is estimated using a vector centered at that point with a radius of... Within a spherical neighborhood, construct a 3×3 covariance matrix Σ of the neighborhood point coordinates: Covariance matrix: (6) in Given the coordinates of N neighboring points within the neighborhood, Let Σ be the centroid of the neighborhood. Perform eigenvalue decomposition on Σ, and take the eigenvector corresponding to the smallest eigenvalue as the local surface normal vector direction n at that point. Further, uniformly flip the normal vector direction to point towards the origin O of the lidar sensor coordinate system, thus ensuring... This eliminates the ambiguity of the normal vector direction.

[0068] S32: Neighborhood distribution histogram construction. For each key point, a histogram is constructed with that point as the center and a radius of... Search all neighboring points within the spherical neighborhood. For each pair of keypoints and their neighbors, calculate two statistics: Normalized distance , where d is the Euclidean distance between the two points. The interval [0, 1] of α is uniformly divided into... The distance interval falls within the first... k The conditions for each interval are: .

[0069] cosine value of the direction angle , where v is the direction vector from the keypoint to the neighboring point, and n is the local surface normal vector of the neighboring point. The value range [-1, 1] is divided into non-uniform intervals with equal angular spacing within the cosine domain. The angular intervals are divided by the following points: , Each corresponding angle in the field A range of radians. When When = 8, the dividing point is , , , 0, , , It achieves a uniform 22.5-degree interval division in the angular domain, while avoiding explicit inverse cosine calculation.

[0070] Count the neighboring points that fall into each distance interval-angle interval combination to form a... Two-dimensional histogram The neighborhood distribution histogram of the key point after L1 normalization is used.

[0071] S33: Neighborhood aggregation weighted fusion. For each keypoint i, its final distance-angle bimodal local feature descriptor... Histogram of distribution in its own neighborhood With Histogram of neighborhood distribution of all neighboring key points j within the range We obtain the weighted summation based on distance attenuation: Descriptor aggregation: (7) Merged descriptor Then, L1 normalization is performed. This aggregation operation is similar to smoothing the histogram within the neighborhood of local keypoints, enhancing the robustness of the descriptor to small shifts in keypoint positions, while preserving the distinctiveness of the local geometry of the keypoints themselves.

[0072] S34: Descriptor Angular Dimension Folding. To enhance the invariance of the descriptor to viewpoint rotation, an angular dimension folding operation is performed on the descriptor—for each distance interval α_k, the statistical values ​​of the two centrally symmetric intervals along the angular dimension are summed and merged: Angular dimension folding: (8) This operation will × Dimensional descriptor folded into × ( / 2) dimension, utilizing the cyclic symmetry of the angular dimension— and The absolute values ​​are equal, but the signs are opposite. The folded descriptor has a certain degree of tolerance for in-plane rotations of the scene, and is used in the folding back mechanism of subsequent geometric verification.

[0073] Step S4, the construction of the multi-level global scene representation, specifically includes: S41: Construct a multi-attribute global descriptor for annular sector partitioning. Project the non-ground point cloud of the current frame onto polar coordinate space according to the horizontal plane. The polar radius direction starts from the minimum effective radius. To the maximum effective radius Evenly divided into A radial ring, with polar angles uniformly divided between 0 and 2π. Each angle sector forms × The ring sector partitioned mesh. For each mesh cell... Calculate the following five attribute features: (1) Height characteristics : Take the maximum Z-coordinate value of all points within the cell, and use the global maximum height. Normalize.

[0074] (2) Density characteristics : The number of points within a cell Add 1 and take the natural logarithm, then normalize using the global maximum logarithmic density: .

[0075] (3) Span characteristics : Take the difference Δz between the maximum and minimum values ​​of the point cloud in the Z direction within the cell, and normalize it using the global maximum span.

[0076] (4) Contrast characteristics The following is a weighted combination of the gradient differences in height, density, and span between this unit and its four adjacent units (left, right, inner, and outer), as well as the deviation of this unit from the mean of all units in the same ring: Contrast characteristics: (9) (5) Reliability characteristics : Confidence weights calculated based on whether a cell is occupied (some fall within the cell) and the saliency distribution of the point cloud within that cell: .

[0077] S42: Ring eigenvectors and sector eigenvectors. Ring eigenvectors For one A dimensional vector, the k-th element of which is defined as the element with the reliability characteristic among all grid cells within the k-th radial loop. Weighted multi-attribute joint coding statistics: Ring eigenvectors: (10) Sector feature vector For one A dimensional vector, the m-th element of which is defined as the element with the reliability characteristic among all grid cells within the m-th angular sector. The weighted multi-attribute joint encoding statistics. The ring feature vector is used for coarse-grained fast candidate recall, and the sector feature vector is used to estimate the yaw angle deviation between two frames.

[0078] S43: Frame-level statistical aggregation descriptor. For all keypoints in the current frame, a distance-angle bimodal local feature descriptor { The arithmetic mean is taken element-wise along the keypoint dimension to obtain a frame-level vector with the same dimension as a single local feature descriptor. Frame-level statistical aggregation descriptor: (11)

[0079] The hierarchical retrieval and geometric consistency verification in step S5 specifically include: S51: Coarse-grained candidate recall. This involves retrieving the loop feature vector from the current frame. (q) and the ring feature vectors of all historical frames in the database Calculate the Euclidean distances one by one: Ring feature distance: (12) Sort by distance in ascending order, retaining the top few. The frame with the smallest distance is selected as a coarse-grained candidate frame. Meanwhile, for frames with a sequence number difference less than the preset minimum frame interval... Candidate frames are directly excluded to avoid incorrect matching of temporally adjacent frames. In addition, by performing cyclic cross-correlation analysis on the sector feature vector of the current frame and the sector feature vector of the candidate frame, the angle offset Δθ corresponding to the cross-correlation peak is found as a prior estimate of the yaw angle between the two frames.

[0080] S52: Fine-grained reordering. (For...) A coarse-grained candidate frame is used to aggregate descriptors using frame-level statistics. Recalculate the Euclidean distance to the current frame: Statistical descriptor distance: (13) Reorder by distance and retain the previous order. One candidate frame enters the geometric verification stage. At the same time, the top rankings from the initial coarse search phase are also retained. However, candidate frames that may not have passed the fine-grained reordering process. The final candidate set for geometric verification is .

[0081] S53: Keypoint matching and ratio test. For each candidate frame The local feature descriptor for each key point in the query frame q. Local feature descriptors of all key points in candidate frame c Perform L2 distance calculation. For each query keypoint, find the two closest and second-closest matching keypoints. If the following ratio test condition is met, the match is accepted: Ratio test: (14) in For the closest distance, The second closest distance, The preset ratio threshold is used. The theoretical basis of this ratio test mechanism is that for a correct match with "uniqueness" in the descriptive subspace, its nearest neighbor distance should be significantly smaller than its second nearest neighbor distance; while for an unreliable match located in a fuzzy region, its nearest neighbor and second nearest neighbor distances are close, and the ratio approaches 1.

[0082] S54: Rigid body transformation estimation. From the set M of all matching pairs that pass the ratio test, three pairs of matching points are randomly selected. The optimal rigid body transformation (R, t) satisfying the least squares principle is obtained through singular value decomposition (SVD). This transformation is applied to all matching point pairs, and the spatial distance after the transformation is statistically determined to be less than a preset interior point distance threshold. The number of matching point pairs is used as the number of interior points. : Interior point determination: (15) Repeat the above random sampling and verification process. Next, the rigid body transformation that retains the most interior points. As the optimal estimate, if the number of interior points of a certain transformation reaches a preset sufficient interior point threshold during the iteration process. If the iteration is terminated prematurely, then the iteration will be terminated early.

[0083] Furthermore, when the number of interior points or the interior point rate obtained from standard geometric verification is lower than a preset weak verification threshold, a descriptor angle dimension folding and fallback mechanism is triggered: after performing angle dimension folding operations on the local feature descriptors of both the query frame and the candidate frame according to formula (8), the key point matching described in step S53 and the rigid body transformation estimation described in step S54 are re-executed. The folded descriptor reduces the sensitivity to viewpoint rotation through symmetrical summation of the angle dimension, and can capture loops that the standard descriptor fails to match under large viewpoint rotation.

[0084] S55: Iterative nearest-point refinement. Using the optimal rigid body transformation obtained in step S54... Using the initial value, utilize all values ​​that satisfy the interior point distance threshold. The matching point pairs are used to construct a correspondence, and the two steps of finding the nearest corresponding point and updating the transformation are executed iteratively until the translation change is reached. and rotational change The convergence values ​​are below the preset convergence thresholds δ_t and δ_θ, respectively, or the maximum number of iterations is reached. So far, the refined relative pose transformation is obtained. .

[0085] S56: Candidate Screening and Scoring. For candidate frames that pass geometric verification, check whether they simultaneously meet the following conditions: (i) The number of interior points is not less than the preset minimum number of interior points. (ii) The interior rate is not lower than the preset minimum interior rate. (iii) The translation amount does not exceed the preset maximum translation amount. If all conditions are met, it is determined to be a valid geometric closure candidate, and its geometric confidence score is defined as follows: Geometric confidence score: (16) in The inlier ratio (the ratio of the number of inliers to the total number of matched pairs) is the inlier ratio. This scoring function combines information from two dimensions: the absolute number of inliers and their relative proportion—the number of inliers reflects the geometric support strength of the match, while the inlier ratio reflects the overall consistency of the match.

[0086] Step S6, spatiotemporal joint consistency mining and frame-by-frame optimal constraint output, specifically includes: S61: Mining the dynamic programming trajectory of the optimal path in two modes. All geometrically validated loop closure candidates are organized into a candidate matrix, where the rows are the query frame indices q, and the columns are the matching frame indices m. Two temporal modes and their corresponding inter-frame index constants are defined: Forward mode constants: (17) Reverse mode constants: (18) Forward mode (mode 0) corresponds to vehicles retracing the same path segment in the same direction—in consecutive frames, if q and m each increase by 1, then... Keep constant; reverse mode (mode 1) corresponds to vehicles traveling in the opposite direction through the same path segment—in consecutive frames, if q increases by 1 while m decreases by 1, then Keep it constant.

[0087] Define a dynamic programming state table `states[q][c][mode]`, representing the optimal time sequence chain ending at the `c`th candidate in the `q`th frame under the `mode` condition. State transitions must satisfy two conditions: Condition 1 (Consistency of constants): ,in The sequence number interval for query frames. Based on tolerance bias.

[0088] Condition 2 (Temporal continuity): ,in This is the preset maximum frame interval.

[0089] The transfer score is defined as: Transfer score: (19) in Frame interval bonus items (better b1 for 1 frame interval, b2 for 2 frames interval, and b2 for 3 frames interval). Frame reward b3), λ is the constant deviation penalty coefficient.

[0090] For each completed state chain (trajectory), graded acceptance is performed based on the average number of interior points: average number of interior points ≥ A strong trajectory requires at least Frame length; average number of internal points ≥ The intermediate trajectory requires at least Frame length; in other cases, at least Frame length (by setting a larger value) (The actual value is to disable weak tracks). In addition, the cumulative track score must be ≥ And the average inside point ratio must be ≥ After trajectory verification, deduplication is performed based on the median constant value to prevent the same loop event from being output repeatedly.

[0091] S62: Complete the source trajectory and extend the resilient prediction corridor. For each accepted temporally consistent trajectory, within its query frame range... The in-frame scan did not originally include the frames. If a frame has a candidate frame that satisfies: (i) the in-point rate is not lower than the leniency threshold. (ii) Deviation of constant quantity within the extended tolerance range If the interior points are within the specified range, they are added to the trajectory. For trajectories that meet the preset intensity conditions (average number of interior points reaches a certain threshold), the interior points are then added to the trajectory. And the number of nodes reaches ), further in ± centered on the predicted matching frame Within the frame neighborhood, the geometric verification described in step S5 is actively initiated, and the verified candidate dynamics are added to the candidate matrix and the trajectory is supplemented. The number of nodes is not less than... The trajectory is segmented into pieces using existing nodes as anchor points for linear interpolation: for each node before and after the trajectory... and Each query frame within the frame extension region The expected matching frame position is estimated by linear prediction of adjacent anchor points. Before and after the expected matching frame. Within the frame search window, candidates that meet the geometric verification conditions are searched, and the selection criterion is as follows: Corridor Optimization Scoring: (20) in This is the penalty coefficient for each frame deviating from the predicted matching frame position. If the number of nodes in the extended trajectory is not less than the original number of nodes minus... The extended trajectory then replaces the original trajectory.

[0092] S63: Neighborhood Consensus Voting Mechanism. This mechanism is a separate channel running parallel to step S61, trajectory mining, specifically for isolated but geometrically reliable loop candidates that were not included in any trajectory due to the conservatism of trajectory mining. It consists of four stages: (i) Entry Gating – Candidates must meet one of the standard path or the lenient path requirements. Standard Path: ≥ and ≥ Loose path: ≥ and ≥ .

[0093] (ii) Vote gating – before and after query frame q Within the frame neighborhood window, count the number of other candidates that have the same constant relationship with the candidate (satisfying formula (17) or (18), and whose constant deviation does not exceed the tolerance value), and use this count as the temporal support votes. The number of votes must meet the standard support threshold. Or relax support thresholds one.

[0094] (iii) Calculation of overall score: Overall rating: (twenty one) in The geometric confidence score is calculated using formula (16). The weighting factor for supporting votes (typically between 2.0 and 5.0). For the time-series support vote count, The ranking penalty factor (typically between 0.02 and 0.10) is used, and rank is the candidate's ranking number among all candidates in the same frame. The weight design of this scoring formula means that candidates that receive 2 or more votes of neighborhood support will receive a significant improvement in their overall score, thus qualifying them to compete with the trajectory channel output.

[0095] (iv) Competitive replacement – ​​If the query frame q does not yet have an optimal constraint, the current candidate is written directly. (Type marked as "vote rescue", added); if there are already optimal constraints and > [q] + (in If a preset replacement margin is used (typically between 1.0 and 3.0), the current candidate constraint will replace the original constraint; otherwise, the original constraint will be retained.

[0096] S64: Local smoothing filtering and frame-by-frame optimal constraint output. For each pair of adjacent query frames (q, q+1), check the jump magnitude of the matching frame number in their respective optimal closure constraints. - Has the preset transition threshold been exceeded? If the number exceeds the limit, then compare the number of interior points of the loop closure constraints in the two frames. And timing support votes The filter retains frames with higher overall confidence and discards those with significant jumps in closure constraints. This effectively eliminates isolated mismatches that may occur at trajectory boundaries or scene transitions. The optimal closure constraint is output frame by frame. For each query frame, the final determined optimal closure constraint is output. Each constraint contains the following fields: query frame number q, matching frame number m, and 9 elements of the rotation matrix {R} representing the relative pose transformation. 00 , R 01 , ..., R 22}, Translation vector has 3 components { , , }, internal points Interior point ratio Geometric confidence score The output file includes the confidence level and the constraint source type identifier (type). Type = 1 indicates the constraint originates from a time-series consistency trajectory path (TRACK), and Type = 2 indicates the constraint originates from a neighborhood consistency voting path (RESCUE). This output file can be directly used as the loop closure edge input for SLAM backend graph optimization.

[0097] Example 2

[0098] This embodiment provides a LiDAR loop closure detection method based on multi-level feature representation and spatiotemporal joint mining. It runs on an in-vehicle embedded computing platform or a mobile robot main control unit, receiving real-time 3D point cloud data streams from multi-line mechanical rotating or solid-state LiDAR sensors. The LiDAR typically operates at a frequency of 10 Hz, and each frame of the point cloud contains tens of thousands to hundreds of thousands of 3D spatial points. To verify the technical effectiveness of this embodiment, quantitative tests were performed on multiple sequences of the publicly available autonomous driving dataset KITTI. The implementation steps of the method are described in detail below with reference to the accompanying drawings.

[0099] Reference Figure 1 The overall process of the method of the present invention includes six core steps from S1 to S6, forming a complete processing pipeline from the original point cloud to the loop constraint output.

[0100] Step S1: Point cloud data preprocessing.

[0101] Referring to step S11, ground segmentation based on a polar coordinate grid is performed on the original point cloud of the current frame output by the lidar. A polar coordinate grid is constructed centered on the sensor origin: divided radially into 120 intervals and angularly into 360 sectors. Within each angular sector, least-squares line fitting is performed on the radially arranged points, and the perpendicular distance from each point to the fitted line segment is calculated. Points with distances less than a preset threshold are considered... Points with a distance of 0.15 meters are marked as ground points and discarded, while only non-ground points are retained for subsequent processing.

[0102] Next, the cylindrical region of interest filtering described in step S12 is performed: with the sensor as the origin, the region of interest within the horizontal projection plane is retained at a distance from the origin. = Points within a 30-meter radius, while restricting height coordinates to [ = -3.0 meters, =5.0 meters] between.

[0103] Then, perform the voxel hash grid downsampling described in step S13: divide the space into sections with side lengths of... A 3D cubic mesh with a diameter of 0.4 meters is used, where each mesh cell retains only the point closest to its center. O(1) time-efficient mesh cell indexing and deduplication are achieved using a 64-bit integer hash key, and point cloud resolution parameters are calculated. = 0.4 × 0.67 ≈ 0.27 meters.

[0104] Finally, outlier removal is performed as described in step S14: = 0.8 meters is the search radius, and the number of valid points in the neighborhood must be no less than = 10. After the above preprocessing, the number of points in each frame of the point cloud decreased from about 120,000 to about 15,000 to 50,000 valid points.

[0105] Step S2: Keypoint detection driven by both geometric saliency and other factors.

[0106] Reference Figure 2 and formulas (1)-(5). Calculate the significance of geometric shift according to formula (1). Search radius = 15 × ≈ 4.0 meters, calculate the neighborhood centroid and the normalized offset distance. Calculate the region structural complexity according to formula (2). Search radius = 40 × ≈ 10.8 meters, using spatial hashing to accelerate the pre-statistical neighborhood density of each cell in the grid, and then calculating using an exponential decay model.

[0107] According to formula (3), nonlinear enhancement is applied to the two types of significance respectively: global maximum value and the mean of the remaining local extrema The greater the difference, the more significant the enhancement, thus effectively amplifying the contrast between the prominent structure and the background. Joint significance is obtained by fusing according to formula (4). Take the global mean = mean(S) is an adaptive threshold, within the radius... =10 × Spatial nonmaximum suppression is performed within approximately 2.7 meters. After this step, the number of key points extracted from each frame of point cloud is usually 50 to 500 (defined by formula (5)). The key points are concentrated in areas with significant three-dimensional geometric structures, such as building corners, road edges, signs, and parked vehicles.

[0108] Step S3: Calculate the distance-angle bimodal local feature descriptor.

[0109] Reference Figure 3 and formulas (6)-(8). Normal vector estimation is performed according to formula (6): search radius. = 20× ≈ 5.4 meters, construct the covariance matrix and take the eigenvector corresponding to the smallest eigenvalue, and flip the vector to face the sensor origin.

[0110] Constructing a neighborhood distribution histogram: search radius = 10 × ≈ 2.7 meters. (Take...) = 4 distance intervals, = 8 angle intervals. The angle interval division points are based on... , , The values ​​0, -0.38268, -0.70711, and -0.92388 are set to achieve an approximately uniform 22.5-degree interval division in the angle domain. The neighbor points falling into 4 × 8 = 32 interval combinations are counted and L1 normalized.

[0111] Neighborhood aggregation and weighted fusion are performed according to formula (7): the histograms of each neighboring keypoint are weighted and summed with a distance decay weight of 1 / (1+d) and then L1 normalized. This aggregation operation is equivalent to smoothing filtering within the local keypoint neighborhood, which enhances the robustness of the descriptor to small shifts in keypoint position. Folded descriptors are generated according to formula (8): the angle dimension is symmetrically summed to obtain a 4 × 4 = 16-dimensional folded version, which is used for the folding backoff mechanism in subsequent geometric verification.

[0112] Step S4: Constructing a multi-level global scene representation.

[0113] Reference Figure 4 and formulas (9)-(11). Construct the multi-attribute global descriptor for the ring sector partition according to formula (9): take = 20 radial rings ( = 2.0 meters to = 80.0 meters), = 60 angular sectors (6 degrees per sector), totaling 1200 grid cells. Each cell calculates five-channel feature codes for height, density, span, contrast, and reliability.

[0114] Generate a 20-dimensional ring feature vector (for coarse-grained candidate recall) and a 60-dimensional sector feature vector (for yaw angle estimation) according to formula (10). Generate a 32-dimensional frame-level statistical aggregation descriptor (for fine-grained reordering) according to formula (11).

[0115] Step S5: Hierarchical retrieval and geometric consistency verification.

[0116] Reference Figure 5 and formulas (12)-(16). Coarse-grained candidate recall is performed according to formula (12): the Euclidean distance of the 20-dimensional ring feature vectors is calculated, and the top candidates are retained. = 75 candidate frames, excluding frames with a difference in frame number less than = 500 frames (corresponding to approximately 50 seconds of driving time) of neighboring frames.

[0117] Fine-grained reordering is performed according to formula (13): the distances of the 75 coarse candidates are recalculated and sorted using 32-dimensional frame-level statistical descriptors. Fine-grained reordering can be optionally disabled. = 0), while retaining the results before the coarse search. = 10 candidates directly proceed to geometric verification.

[0118] Perform the ratio test according to formula (14): = 0.8, effectively filtering out approximately 40% to 60% of unreliable matches. Rigid body transformation estimation is performed according to formula (15): 3 pairs of matching points are randomly sampled each time, and SVD is used to solve the problem, with the interior point distance threshold being... = 1.0 meter, maximum iterations = 50 times, interior points reached = 15, terminate early. If the number of inliers or the inlier rate is lower than the weak validation threshold, trigger the foldback of formula (8). Then perform iterative nearest-point refinement: convergence condition is < 0.1 meters and < 1.0 degree, maximum 20 iterations. Calculate the geometric confidence score according to formula (16) and filter ( =4, = 0.20, = 5.0 meters).

[0119] Step S6: Spatiotemporal joint consistency mining and frame-by-frame optimal constraint output.

[0120] Reference Figure 6 and formulas (17)-(19). Define the forward mode according to formulas (17) and (18). ) and reverse mode ( Two inter-frame index constants. The transfer score is calculated according to formula (19): inter-frame interval reward b1 = 0.30 (gap=1), b2 = 0.15 (gap=2), b3 = 0.05 (gap=3~5), constant deviation penalty coefficient λ = 0.22, and basic tolerance deviation... = 20 frames, maximum frame interval = 5 frames. Strong trajectory acceptance: =6, = 3 frames; Medium trajectory acceptance: = 4, = 5 frames; weak trajectory: = 50 frames (actually disabled). Cumulative score must be ≥ = 6.0, average inside point ratio must be ≥ = 0.12.

[0121] Perform homogeneous trajectory completion: lenient inlier rate threshold = 0.15, Extended tolerance bias = 4 frames. Strong trajectory conditions: average number of interior points ≥ 7 and number of nodes ≥ 5, prediction search range ± 3 frames. Perform flexible prediction corridor extension: minimum number of nodes. = 4, number of frames extended before and after = = 4, search half-window width = 12 frames, prediction deviation penalty = 0.03 (Formula 20). Tolerance for node loss after stretching. =2.

[0122] Reference Figure 7 and formula (21). Execute neighborhood consensus voting mechanism. Entry gating: standard path ( ≥4 and ≥ 0.10), loose path ( ≥ 3 and ≥ 0.08). Voting gate: Neighboring window ±5 frames, standard threshold. = 2 votes, lenient threshold = 5 votes. Overall score (Formula 21): Support weight = 4.0, ranking penalty = 0.05, replacement margin = 2.0.

[0123] Perform local smoothing filtering: jump threshold = 12 frames. Output the optimal constraints frame by frame, including rotation matrix, translation vector, interior point statistics and constraint source type identifier (type=1 for trajectory path, type=2 for voting path).

[0124] Referring to the test results in Table 1, this embodiment demonstrates high precision and high recall across different dataset sequences. Figure 8 On the complete driving trajectory of over 4000 frames in KITTI08, the detected loop closure constraints (represented by connecting lines) spatially closely matched the path segments actually traversed by the vehicle, verifying the spatial accuracy of loop closure detection. (Refer to...) Figure 9 The superiority of this embodiment is verified by comparing the detection results of the classic loop closure detection algorithm BTC method on the kitti08 dataset.

[0125] Table 1. Test results of the method of the present invention on various sequences of the KITTI dataset.

[0126] Reference Figure 8 On the complete driving trajectory of over 4000 frames in KITTI08, the detected loop closure constraints (represented by connecting lines) spatially closely matched the path segments actually traversed by the vehicle, verifying the spatial accuracy of loop closure detection. (Refer to...) Figure 9 The superiority of the proposed method is verified by comparing the detection results of the classic loop closure detection algorithm BTC on the kitti08 dataset.

[0127] Example 3

[0128] This embodiment provides a lidar loop closure detection system based on multi-level feature representation and spatiotemporal joint mining, used to implement the method described in Embodiment 1, and includes the following functional modules: Point cloud preprocessing module: This module performs ground segmentation, region of interest (ROI) filtering, voxel downsampling, and outlier removal on the raw 3D point cloud acquired by LiDAR, outputting a structured sparse point cloud. This module integrates a polar coordinate grid ground segmentation submodule, a cylindrical ROI filter, a voxel hash grid downsampling unit, and a radius search outlier filter.

[0129] Keypoint Detection Module: This module performs dual saliency-driven keypoint detection on the preprocessed point cloud. It includes a geometric offset saliency calculation submodule (implementing Equation 1), a region structure complexity calculation submodule (implementing Equation 2), a nonlinear contrast enhancement submodule (implementing Equation 3), and an adaptive threshold filtering and spatial nonmaximum suppression submodule (implementing Equations 4-5). This module utilizes spatial hashing to accelerate the grid and achieve fast neighborhood search at various radii.

[0130] Local Feature Description Module: Used to calculate distance-angle bimodal local feature descriptors for each key point, including normal vector estimation submodule (implementing formula 6), neighborhood distribution histogram construction submodule (2D histogram statistics of 4 distance × 8 angle), neighborhood aggregation weighted fusion submodule (implementing formula 7), and angle dimension folding submodule (implementing formula 8).

[0131] Global Scene Representation Module: Used to construct multi-level global descriptors, including a ring-sector partition multi-attribute global descriptor construction submodule (implementing formulas 9-10) and a frame-level statistical aggregation descriptor generation submodule (implementing formula 11).

[0132] Hierarchical retrieval and geometric verification module: used to perform coarse-to-fine candidate retrieval and geometric consistency verification, including the ring feature vector coarse retrieval submodule (implementing formula 12), frame-level statistical descriptor fine reordering submodule (implementing formula 13), key point ratio test matching submodule (implementing formula 14), rigid body transformation estimation submodule (implementing formulas 15-16), iterative refinement submodule, and folding back verification submodule.

[0133] Spatiotemporal joint mining and output module: used to perform temporal consistency analysis and frame-by-frame optimal output on candidates for geometric verification, including dual-mode dynamic programming trajectory mining submodule (implementing formulas 17-19), homogeneous trajectory completion submodule, elastic prediction corridor extension submodule (implementing formula 20), neighborhood consistency voting submodule (implementing formula 21), local smoothing filtering submodule and frame-by-frame optimal constraint output submodule.

[0134] Database management module: Used to store and manage multi-level descriptors (ring feature vectors, sector feature vectors, frame-level statistical aggregation descriptors, local feature descriptor matrices, key point location information, and preprocessed point cloud data) of all historical frames, supporting fast insertion of new frames and efficient querying of any historical frame.

[0135] The embodiments described in this specification are merely examples of implementations of the inventive concept. The scope of protection of this invention should not be considered as limited to the specific forms stated in the embodiments. The scope of protection of this invention also extends to equivalent technical means that can be conceived by those skilled in the art based on the inventive concept.

Claims

1. A lidar loop closure detection method based on multi-level feature representation and spatiotemporal joint mining, characterized in that, Includes the following steps: S1: Preprocess the original 3D point cloud of the current frame acquired by the lidar to obtain a structured sparse point cloud; S2: Perform geometric saliency dual-driven keypoint detection on the preprocessed point cloud to obtain a set of 3D keypoints with high geometric discriminative power; S3: For each 3D keypoint, construct a descriptor for the local features in a distance-angle dual-modality configuration; S4: Construct a multi-level global scene representation: Project the non-ground point cloud onto the polar coordinate plane to construct a ring-sector partitioned grid. Calculate the joint encoding of multiple attributes for each grid cell and derive the ring feature vector and sector feature vector. At the same time, take the dimensional average of the local feature descriptors of all key points in the current frame to obtain the frame-level statistical aggregated descriptor. S5: Perform hierarchical retrieval and geometric consistency verification, filter candidate frames using the loop feature vector and frame-level statistical aggregation descriptor, and perform matching and pose estimation based on the local feature descriptor to determine whether it is a valid geometric loop closure; S6: Perform spatiotemporal joint consistency mining and frame-by-frame optimal constraint output. Search for temporal consistency trajectories through dynamic programming, and evaluate isolated candidates through a neighborhood consistency voting mechanism. Finally, output an optimal loop constraint for each frame query.

2. The method as described in claim 1, characterized in that, Step S1, which involves preprocessing the original 3D point cloud of the current frame acquired by the lidar, includes: ground point cloud segmentation and removal, cylindrical spatial region filtering centered on the lidar, voxel downsampling, and outlier removal.

3. The method as described in claim 2, characterized in that, The preprocessing specifically includes the following steps: S11: Ground segmentation based on polar coordinate grid: The original point cloud is mapped to a polar coordinate grid according to the geometric characteristics of the LiDAR scan. Line segments are fitted to points in the radial interval within each angular sector, and the distance fitted line segments are less than a first preset threshold. The points were marked as ground points and then removed; S12: Filtered cylindrical region of interest: Centered on the lidar, only the horizontal projection radius within the cylindrical region is retained. Within the range and the vertical height is within the preset height range The point between; S13: Voxel hash grid downsampling: Divide the point cloud space within the region of interest into sections with sides equal to the voxel downsampling size. The three-dimensional mesh cells retain only one representative point per cell. Fast deduplication is achieved through hash mapping, and the point cloud resolution parameters are adaptively calculated based on the voxel size. ; S14: Outlier Removal Based on Radius Search: For each point in the downsampled point cloud, the outlier search radius is used. Perform a neighborhood search; if the number of points in the neighborhood is less than the minimum number of neighbors of the outlier, then... If so, mark the point as an outlier and remove it.

4. The method as described in claim 1, characterized in that, Step S2 includes: calculating the geometric offset significance and regional structural complexity of each spatial point, performing nonlinear contrast enhancement on both separately and then fusing them into joint significance, and then using adaptive thresholding and spatial nonmaximum suppression.

5. The method as described in claim 1, characterized in that, Step S3 includes estimating the local surface normal vectors of each point in the point cloud, then generating a neighborhood distribution histogram by statistically analyzing the distance distribution and the angle distribution between the normal vectors of the points in the neighborhood of each key point, and then performing neighborhood aggregation weighted fusion and angle dimension folding to enhance rotation invariance on the neighborhood distribution histogram of each key point to obtain the descriptor.

6. The method as described in claim 1, characterized in that, The ring-sector partitioned grid described in step S4 consists of multiple radial rings and multiple angular sectors. The multi-attribute joint encoding includes height features, density features, span features, contrast features, and reliability features, and generates ring feature vectors and sector feature vectors based on the statistical information of each ring and each sector.

7. The method as described in claim 6, characterized in that, Step S4 includes the following steps: S41: Construct a multi-attribute global descriptor for a circular sector partition: Project the non-ground point cloud onto polar coordinate space, within the polar radius range. With polar angle range The interior is divided into a radial ring and Each grid cell is divided into five angle sectors; each grid cell calculates height, density, span, contrast, and reliability features to form a five-channel multi-attribute joint encoding. S42: Generates aggregated values ​​of cell codes weighted by reliability characteristics within each radial loop. 3D ring eigenvector Generates the aggregated value of the unit code, weighted by reliability characteristics, within each sector from different angles. 3D sector feature vector ; Used for coarse-grained candidate recall Used for yaw angle deviation estimation; S43: Frame-level statistical aggregation descriptor: The arithmetic mean of the local feature descriptors of all keypoints in the current frame is taken element-wise along the keypoint dimension to obtain a frame-level vector with the same dimension as a single local feature descriptor. .

8. The method as described in claim 1, characterized in that, Step S5 includes: using the loop feature vector and the loop feature vectors of all frames in the historical frame database to perform fast distance calculation and filter out coarse-grained candidate frames; then using frame-level statistical aggregation descriptors to perform fine distance reordering on the coarse-grained candidate frames and select candidate frames from them; for each candidate frame, using the local feature descriptors of key points to perform bidirectional nearest neighbor matching and ratio testing, and based on the matching point pairs, using rigid body transformation estimation with random sampling consistency and iterative nearest point refinement to obtain the relative pose transformation and interior point statistics of the candidate frame relative to the current frame, and using a candidate screening and scoring mechanism to determine whether it is a valid geometric loop.

9. The method as described in claim 1, characterized in that: Step S6 includes: organizing all geometric loop closure candidates between frames into a candidate matrix; searching for continuous temporally consistent trajectories that satisfy the inter-frame index constancy constraint in the candidate matrix through dual-mode optimal path dynamic programming; and performing source trajectory completion and elastic prediction corridor extension on each trajectory; evaluating isolated candidates that are not included in any trajectory through a neighborhood consistency voting mechanism, calculating the number of temporal consistency support votes obtained by each candidate in the local neighborhood, weighting the support votes into the candidate's geometric score to obtain a comprehensive score, and competing with the trajectory output for the best result; and outputting an optimal loop closure constraint for each frame query after local smoothing filtering.

10. A lidar loop closure detection system based on multi-level feature representation and spatiotemporal joint mining, used to implement the method as described in any one of claims 1-9, characterized in that, include: Point cloud preprocessing module: used to perform ground segmentation, cylindrical spatial region filtering, voxel downsampling and outlier removal on the raw 3D point cloud acquired by lidar; Keypoint detection module: used to perform dual saliency-driven keypoint detection on the preprocessed point cloud; Local Feature Description Module: Used to calculate a distance-angle bimodal local feature descriptor for each keypoint; Global scene representation module: used to construct multi-level global descriptors, including ring-sector partition multi-attribute global descriptors and frame-level statistical aggregation descriptors; Hierarchical retrieval and geometric verification module: used to perform coarse-to-fine candidate retrieval and geometric consistency verification; Spatiotemporal joint mining and output module: used to perform temporal consistency analysis and frame-by-frame optimal output on candidates for geometric verification; Database management module: Used to store and manage multi-level descriptor data for all historical frames.

Images