Loop closure detection methods, apparatus, computer equipment and storage media

Abstract

This application provides a loop closure detection method, apparatus, computer device, and storage medium, comprising: determining the bag-of-words vector and global feature information corresponding to the current image data; and determining the target point cloud data matching the current image data; using the feature data of the current image data to determine whether there are loop closure candidate data pairs matching the current image data among multiple historical data pairs; if so, determining the environmental complexity index and the number of aggregated frames matching it; constructing a historical point cloud sub-map using the number of aggregated first point cloud data, and constructing a current point cloud sub-map using the number of aggregated second point cloud data; the first point cloud data includes key point cloud data and previous first historical point cloud data; the second point cloud data includes target point cloud data and previous second historical point cloud data; performing registration based on the historical point cloud sub-map and the current point cloud sub-map, and determining to execute the loop closure processing flow when the registration result indicates successful registration.

Description

Technical Field

[0001] This application relates to the field of synchronous positioning and mapping technology, and more specifically, to a loop closure detection method, apparatus, computer equipment, and storage medium. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) systems for robots are now widely used in fields such as autonomous driving, unmanned systems, and robotics. Loop closure detection, as one of the key modules, is a core technology for achieving globally consistent and high-precision map construction. Its basic goal is to identify whether the robot has revisited a previously visited location, thereby correcting accumulated errors and improving localization and mapping accuracy.

[0003] Early research relied heavily on Kalman filtering or odometry-based consistency assessments for loop closure detection, but these methods struggled to address error accumulation over long periods. As research progressed, appearance-based loop closure detection gradually became mainstream. Typical methods utilize visual or laser data to construct feature descriptions for scene matching, thus providing constraints for backend graph optimization. However, these loop closure detection methods still face numerous challenges, such as variations in lighting, viewing angle differences, interference from dynamic objects, and repetitive environmental structures, all of which significantly affect matching accuracy. Therefore, achieving accurate loop closure detection techniques across different scenarios remains a significant challenge. Summary of the Invention

[0004] In view of this, this application provides a loop closure detection method, apparatus, computer equipment, and storage medium.

[0005] In a first aspect, embodiments of this application provide a loop closure detection method, including: Acquire real-time image data and current frame point cloud data collected by the robot during its movement; Determine the bag-of-visual-words vector and global feature information corresponding to the current image data, and store the global feature information in the global feature database and the bag-of-visual-words vector in the bag-of-visual-words database; When it is determined that the current frame point cloud data belongs to a key frame, the current frame point cloud data is updated to the key frame point cloud list, and the target point cloud data matching the current image data is determined from the key frame point cloud list, wherein the target point cloud data and the current image data constitute the current data pair; Using the feature data of the current image data, determine whether there are loop closure candidate data pairs that match the current image data among multiple historical data pairs; wherein the feature data includes at least one of the following: visual bag-of-words vector, global feature information; If it exists, then based on the current image data and / or the target point cloud data, determine the environmental complexity index, and determine the number of aggregated frames that match the environmental complexity index; Using the aggregated frame of several first point cloud data, a historical point cloud sub-map is constructed, and using the aggregated frame of several second point cloud data, a current point cloud sub-map is constructed; wherein the first point cloud data includes the key point cloud data in the loopback candidate data pair, and the first historical point cloud data before the key point cloud data; the second point cloud data includes the target point cloud data and the second historical point cloud data before the target point cloud data; Based on the historical point cloud sub-map and the current point cloud sub-map, registration is performed to determine the registration result; when the registration result indicates successful registration, the loopback process is executed.

[0006] In one optional implementation, determining the visual bag-of-words vector and global feature information corresponding to the current image data includes: The data format of the current image data is converted to grayscale format, and the converted current image data is subjected to distortion correction to generate processed image data; Based on the processed image data, pyramid images of various sizes and scales are generated; Fast FAST corner feature detection and descriptor calculation are performed on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner. Based on the descriptor features corresponding to each corner point and the set visual dictionary, a bag-of-words vector corresponding to the current image data is generated; The corner points included in the current image data are filtered, and global feature information corresponding to the current image data is generated based on the descriptor features corresponding to the filtered corner points.

[0007] In one optional implementation, the step of performing fast FAST corner feature detection and descriptor calculation on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner includes: For each image in the pyramid image, traverse each pixel in the image and treat each pixel as a pixel to be processed, then perform the following steps: A sampling neighborhood is constructed centered on the pixel to be processed. The pixel information of candidate pixels at multiple target orientations on the boundary of the sampling neighborhood, with the pixel to be processed as the reference, is determined, and the first pixel deviation is determined between them and the pixel information of the pixel to be processed. The number of first pixels whose absolute value of the first pixel deviation is greater than the first pixel threshold is also determined. If the number of the first pixel points is greater than the set first number threshold, then it is determined whether the multiple candidate pixel points on the boundary of the sampling neighborhood meet the following screening conditions: the pixel information of at least n consecutive candidate pixel points is greater than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold; or, the pixel information of at least n consecutive candidate pixel points is less than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold. If the conditions are met, then the pixel to be processed is determined to be a candidate corner point; After traversing all pixels in the image, non-maximum suppression processing is performed on multiple candidate corner points included in the image to obtain the corner points included in the image, and a descriptor feature corresponding to each corner point is generated. The corner points and descriptive features corresponding to each image in the pyramid image are determined as the corner points and descriptive features corresponding to each corner point included in the current image data.

[0008] In one optional implementation, determining whether there exists a loop closure candidate data pair matching the current image data among multiple historical data pairs using the feature data of the current image data includes: Based on the visual bag-of-words vector of the current image data and the visual bag-of-words vector of the historical image data in the historical data pairs stored in the visual bag-of-words database, it is determined whether there are candidate historical image data with a bag-of-words matching score greater than the set first matching threshold. If they exist, for each candidate historical image data, a feature matching score is determined based on the global feature information of the current image data and the global feature information of the candidate historical image data stored in the global feature database. If the feature matching score is greater than the set second matching threshold, then the time difference between the timestamp of the target point cloud data associated with the current image data and the timestamp of the historical point cloud data associated with the candidate historical image data is determined. If the time difference is greater than the set time difference threshold, then the historical data pair including the candidate historical image data is determined to be a loopback candidate data pair.

[0009] In one optional implementation, the registration based on the historical point cloud sub-map and the current point cloud sub-map, and the determination of the registration result, includes: The historical point cloud sub-map and the current point cloud sub-map are registered to determine the transformation matrix, inlier rate, and registration score. Based on the transformation matrix, the historical point cloud sub-map is transformed to generate a processed historical point cloud sub-map. Based on the processed historical point cloud sub-map and the current point cloud sub-map, the overlap rate and normal consistency index are determined; The fusion confidence score is determined based on the inlier rate, the registration score, the overlap rate, and the normal consistency index. When the fusion confidence score indicates high confidence, the registration result is determined to be successful. When the fusion confidence score indicates a medium confidence level, the registration result is determined to be pending confirmation. When the fusion confidence score indicates low confidence, the registration result is determined to be a registration failure.

[0010] In an optional implementation, after the registration result indicates successful registration, the method further includes: Continue acquiring image and point cloud data collected by the robot; Using the collected image data and point cloud data, determine whether the frame data meets the following verification conditions: the detected region is the same region as the historical region indicated by the loop closure candidate data pair, and the determined registration result is high confidence or medium confidence. If multiple consecutive frames of data meet the verification conditions, the verification result of the loopback candidate data pair is determined to be a successful verification. The process of determining and executing the loopback procedure includes: After the review is approved, the loopback process will be executed.

[0011] In an optional implementation, before registration based on the historical point cloud sub-map and the current point cloud sub-map, the method further includes: Determine the first global descriptor corresponding to the historical point cloud sub-map, and the second global descriptor corresponding to the current point cloud sub-map; Determine the descriptor distance based on the first global descriptor and the second global descriptor; The registration process based on the historical point cloud sub-map and the current point cloud sub-map, and the determination of the registration result, includes: When the distance between the descriptors is less than the set distance threshold, registration is performed based on the historical point cloud sub-map and the current point cloud sub-map to determine the registration result.

[0012] Secondly, embodiments of this application also provide a loop closure detection device, comprising: The acquisition module is used to acquire the current image data and current frame point cloud data collected in real time during the robot's movement. The first determining module is used to determine the visual bag-of-words vector and global feature information corresponding to the current image data, and store the global feature information in the global feature database and the visual bag-of-words vector in the visual bag-of-words database. The second determining module is used to update the current frame point cloud data to the key frame point cloud list when it is determined that the current frame point cloud data belongs to a key frame, and to determine the target point cloud data that matches the current image data from the key frame point cloud list, wherein the target point cloud data and the current image data constitute a current data pair; The third determining module is used to determine whether there is a loop closure candidate data pair matching the current image data among multiple historical data pairs using the feature data of the current image data; wherein the feature data includes at least one of the following: visual bag-of-words vector, global feature information; The fourth determining module is used to determine an environmental complexity index based on the current image data and / or the target point cloud data, if such an index exists, and to determine the number of aggregated frames that match the environmental complexity index. A construction module is used to construct a historical point cloud sub-map using the aggregated frame's first point cloud data, and to construct a current point cloud sub-map using the aggregated frame's second point cloud data; wherein the first point cloud data includes key point cloud data in the loopback candidate data pair, and first historical point cloud data preceding the key point cloud data; the second point cloud data includes the target point cloud data and second historical point cloud data preceding the target point cloud data; The fifth determining module is used to perform registration based on the historical point cloud sub-map and the current point cloud sub-map, determine the registration result, and determine to execute the loopback processing flow when the registration result indicates successful registration.

[0013] This application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described loop closure detection method.

[0014] This application provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the above-described loop closure detection method.

[0015] The loop closure detection method, apparatus, computer equipment, and storage medium provided in this application determine the bag-of-visual-words vector and global feature information corresponding to the current image data. This allows for the use of feature data from the current image data, such as global feature information and / or the bag-of-visual-words vector, to determine whether there are candidate loop closure data pairs matching the current image data among multiple historical data pairs, thus achieving initial screening for loop closure detection. When a candidate loop closure data pair is determined, an environmental complexity index is used to determine the number of point cloud data frames used to construct the point cloud sub-map. For example, in structurally clear areas, a smaller number of aggregated frames can be determined to construct the point cloud sub-map using less point cloud data, improving construction efficiency. Conversely, in geometrically degraded areas, a larger number of aggregated frames can be determined to construct the point cloud sub-map using more point cloud data, enhancing robustness and ensuring the accuracy of the sub-map, thus achieving adaptive adjustment of the sub-map scale.

[0016] Furthermore, this application replaces single-frame point cloud registration in related technologies with sub-maps, utilizing multi-frame geometric redundancy to obtain a more stable relative transformation. Since sub-maps increase point cloud density through multi-frame overlay and filter dynamic noise, this application uses sub-maps for registration, resulting in more reliable registration results and improved loop closure detection accuracy. Attached Figure Description

[0017] The accompanying drawings, which are included to provide a further understanding of this specification and form part of this specification, illustrate exemplary embodiments and are used to explain this specification, but do not constitute an undue limitation thereof. In the drawings: Figure 1 This is a flowchart illustrating a loop closure detection method according to an exemplary embodiment of this application; Figure 2 This is a flowchart illustrating a loop closure detection method in another exemplary embodiment of this application; Figure 3 This is a schematic diagram of a loop closure detection device shown in an exemplary embodiment of this application; Figure 4 This is a schematic diagram of the structure of a computer device provided in this application. Detailed Implementation

[0018] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application as detailed in the appended claims.

[0019] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The singular forms “a,” “the,” and “the” used in this application and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0020] It should be understood that although the terms first, second, third, etc., may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "when," "when," or "in response to determination."

[0021] To facilitate understanding of this embodiment, a detailed description of the loop closure detection method disclosed in this application embodiment is provided first. The execution subject of the loop closure detection method provided in this application embodiment is generally a computer device with certain computing capabilities. This computer device includes, for example, a terminal device, a server, or other processing devices. The terminal device can be user equipment (UE), a mobile device, a computing device, an embedded device, etc. In some possible implementations, the loop closure detection method can be implemented by the processor calling computer-readable instructions stored in memory.

[0022] See Figure 1 The diagram shows a flowchart of a loop closure detection method provided in an embodiment of this application. The method includes steps S101 to S107, wherein: S101. Acquire the current image data and current frame point cloud data collected in real time during the robot's movement; S102. Determine the bag-of-visual-words vector and global feature information corresponding to the current image data, and store the global feature information in the global feature database and the bag-of-visual-words vector in the bag-of-visual-words database; S103. When it is determined that the current frame point cloud data belongs to a key frame, the current frame point cloud data is updated to the key frame point cloud list, and the target point cloud data matching the current image data is determined from the key frame point cloud list, wherein the target point cloud data and the current image data constitute the current data pair. S104. Using the feature data of the current image data, determine whether there is a loop closure candidate data pair that matches the current image data among multiple historical data pairs; wherein the feature data includes at least one of the following: visual bag-of-words vector, global feature information; S105. If it exists, then based on the current image data and / or the target point cloud data, determine the environmental complexity index and determine the number of aggregated frames that match the environmental complexity index. S106. Construct a historical point cloud sub-map using the aggregated frame's first point cloud data, and construct a current point cloud sub-map using the aggregated frame's second point cloud data; wherein the first point cloud data includes key point cloud data in the loopback candidate data pair, and first historical point cloud data preceding the key point cloud data; the second point cloud data includes the target point cloud data and second historical point cloud data preceding the target point cloud data; S107. Based on the historical point cloud sub-map and the current point cloud sub-map, perform registration and determine the registration result; when the registration result indicates successful registration, determine to execute the loopback processing flow.

[0023] This application determines the bag-of-words vector and global feature information corresponding to the current image data. By utilizing the feature data of the current image data, such as global feature information and / or the bag-of-words vector, it identifies whether there are loop closure candidate data pairs matching the current image data among multiple historical data pairs, thus achieving initial screening for loop closure detection. When a loop closure candidate data pair is identified, an environmental complexity index is determined to decide the number of point cloud data frames used to construct the point cloud sub-map. For example, in structurally clear areas, a smaller number of aggregated frames can be determined to construct the point cloud sub-map using less point cloud data, improving construction efficiency. Conversely, in geometrically degraded areas, a larger number of aggregated frames can be determined to construct the point cloud sub-map using more point cloud data, enhancing robustness and ensuring the accuracy of the sub-map, thus achieving adaptive adjustment of the sub-map scale.

[0024] Furthermore, this application replaces single-frame point cloud registration in related technologies with sub-maps, utilizing multi-frame geometric redundancy to obtain a more stable relative transformation. Since sub-maps increase point cloud density through multi-frame overlay and filter dynamic noise, this application uses sub-maps for registration, resulting in more reliable registration results and improved loop closure detection accuracy.

[0025] The specific implementation process of S101-S107 is explained below.

[0026] For the S101, a camera and LiDAR can be mounted on the robot to synchronously collect environmental data. The camera and LiDAR can continuously collect environmental data according to their respective acquisition frequencies. The data collected by the camera can be color image data (such as RGB images), while the data collected by the LiDAR can be 3D point cloud data.

[0027] Regarding S102, after acquiring the current image data, feature extraction can be performed on the current image data to determine the corresponding bag-of-visual-word vector and global feature information. For example, the current image data can be detected to determine if it includes corner points and their corresponding descriptors; then, the corner point descriptors are quantized into the visual dictionary to obtain the bag-of-visual-word vector corresponding to the current image, and the bag-of-visual-word vector is stored in a maintained bag-of-visual-word database. Alternatively, the descriptors of multiple corner points can be used as global feature information for the current image data, and this global feature information is stored in a maintained global feature database. The bag-of-visual-word database stores the bag-of-visual-word vectors corresponding to each image data acquired by the robot during its movement, and the global feature database stores the global feature information corresponding to each image data acquired by the robot during its movement.

[0028] Optionally, determining the visual bag-of-words vector and global feature information corresponding to the current image data includes: Step a1: Convert the data format of the current image data to grayscale format, and perform distortion correction processing on the converted current image data to generate processed image data; Step a2: Generate pyramid images of various sizes and proportions based on the processed image data; Step a3: Perform fast FAST corner feature detection and descriptor calculation on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner. Step a4: Generate a bag-of-visuals vector corresponding to the current image data based on the descriptor features corresponding to each corner point and the set visual dictionary; Step a5: Filter the corner points included in the current image data, and generate global feature information corresponding to the current image data based on the descriptor features corresponding to the filtered corner points.

[0029] In practice, the current image data can be converted from a color image to a grayscale image first, and then distortion removal processing can be performed on the converted grayscale image. For example, distortion removal processing can be performed using a pre-calibrated camera intrinsic parameter matrix and distortion coefficients, and camera calibration tools (such as OpenCV's undistort function) can be used to perform distortion removal processing on the image to obtain the processed image data.

[0030] The processed image data can then be downsampled to construct pyramid images of various sizes and proportions. For example, a pyramid image can include an image of the original size, an image of half the original size, an image of one-quarter of the original size, and so on.

[0031] For each image in the pyramid image, FAST corner feature detection and descriptor calculation are performed to obtain the corresponding corner points and descriptor features. Then, based on the corner points and descriptor features included in each image of the pyramid image, the corner points and descriptor features of each corner point are synthesized to obtain the corner points corresponding to the current image data.

[0032] Next, the descriptive features corresponding to each corner point are quantized into the set visual dictionary to generate a bag-of-visual-words vector corresponding to the current image data. This bag-of-visual-words vector is then associated with the identifier of the current image data and stored in the bag-of-visual-words database. Furthermore, the corner points included in the current image data can be filtered, such as removing corner points located at image edges or weak corner points with response values ​​below a set threshold. Based on the descriptive features of the filtered corner points, global feature information corresponding to the current image data is generated and stored in a global feature database. This global feature database can be indexed by the image identifier ID and stores detailed information about all high-quality feature points (i.e., corner points) in each image data, such as 2D pixel coordinates, image pyramid level (e.g., scale), principal direction, and descriptive features.

[0033] The above method generates visual bag-of-words vectors and global feature information from the current image data, providing data support for the initial visual screening of subsequent loop closure candidates.

[0034] Optionally, in step a3, fast FAST corner feature detection and descriptor calculation are performed on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner, including: For each image in the pyramid image, traverse each pixel in the image and treat each pixel as a pixel to be processed, then perform the following steps: Step a31: Construct a sampling neighborhood centered on the pixel to be processed, determine the first pixel deviation between the pixel information of candidate pixels at multiple target orientations on the boundary of the sampling neighborhood with the pixel to be processed as the reference and the pixel information of the pixel to be processed; and determine the number of first pixels whose absolute value of the first pixel deviation is greater than the first pixel threshold. Step a32: If the number of the first pixel points is greater than the set first number threshold, then determine whether the multiple candidate pixel points on the boundary of the sampling neighborhood meet the following screening conditions: the pixel information of at least n consecutive candidate pixel points is greater than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold; or, the pixel information of at least n consecutive candidate pixel points is less than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold. Step a33: If the condition is met, then the pixel to be processed is determined to be a candidate corner point; Step a34: After traversing all pixels in the image, perform non-maximum suppression processing on the multiple candidate corner points included in the image to obtain the corner points included in the image, and generate descriptor features corresponding to each corner point; Step a35: Determine the corner points and descriptive features corresponding to each image in the pyramid image as the corner points and descriptive features corresponding to each corner point included in the current image data.

[0035] For each image in the pyramid image, each pixel in the image is traversed and taken as the pixel to be processed. The pixel to be processed is determined by the four-pixel fast screening method and the full neighborhood continuous arc detection method to determine whether the pixel to be processed belongs to the candidate corner point. Then, the detected candidate corner points are subjected to non-maximum suppression processing to obtain the corner points included in the image more accurately, that is, to obtain a reasonably distributed set of corner feature points and corresponding descriptors.

[0036] During implementation, for each pixel to be processed, a sampling neighborhood is constructed centered on that pixel, for example, a circular sampling neighborhood with a radius of 3. The boundary of this circular sampling neighborhood can include 16 circumferential pixels. To reduce unnecessary calculations and improve processing efficiency, a four-pixel fast filtering can be performed first. For example, four pixels symmetrically positioned vertically, horizontally, and vertically around the center position (i.e., the location of the pixel to be processed) are selected on the circumference; these are candidate pixels located at multiple target orientations corresponding to the pixel to be processed. The first pixel deviation between the pixel information of the multiple candidate pixels and the pixel information of the center position is calculated. The number of pixels whose absolute value of the first pixel deviation is greater than a first pixel threshold is then determined.

[0037] If the number of first pixels is greater than the first threshold, it means that the number of pixels that are significantly brighter or darker than the center pixel meets the requirement, and the subsequent judgment process is executed; otherwise, if the number of first pixels is less than or equal to the set first threshold, it means that the pixel to be processed cannot be a stable corner point, the pixel to be processed is filtered out, and the process continues to traverse the next pixel in the image.

[0038] When the number of first pixels exceeds a set first threshold, a full-neighborhood continuous arc detection method is further executed. This involves determining whether multiple candidate pixels on the boundary of the sampling neighborhood meet the following screening conditions: At least n consecutive candidate pixels have pixel information greater than the pixel information of the pixel to be processed, and the second pixel deviation between pixel information is greater than a set second pixel threshold. This indicates that compared to the center pixel, at least n consecutive pixels on the neighborhood boundary have consistently brighter pixel information. Alternatively, at least n consecutive candidate pixels have pixel information less than the pixel information of the pixel to be processed, and the second pixel deviation between pixel information is greater than a set second pixel threshold. This indicates that compared to the center pixel, at least n consecutive pixels on the neighborhood boundary have consistently darker pixel information. Here, n can be set according to requirements such as neighborhood size. For example, when the sampling neighborhood is a circle with a radius of 3, n can be set to 12.

[0039] If any of the above filtering conditions are met, the pixel to be processed is determined to be a candidate corner point; otherwise, it is determined not to be a candidate corner point. The process can then continue to traverse the next pixel until all pixels in the image have been traversed.

[0040] After traversing all pixels in the image, non-maximum suppression is performed on multiple candidate corner points in the image. For example, within a local window, the corner point response is compared based on its strength (e.g., the sum of the absolute values ​​of the gray differences between the neighborhood and the center can be determined), and only the point with the largest response is retained, suppressing dense and repeated corner points. This results in a reasonably distributed FAST feature point set that is convenient for subsequent matching and tracking, which is the corner point included in the image. Furthermore, descriptive features corresponding to each corner point can be generated.

[0041] Steps a31-a35 yield the corner points and descriptive features of each image in the pyramid image. Finally, the corner points and descriptive features corresponding to each image in the pyramid image are determined as the corner points and corresponding descriptive features of the current image data.

[0042] Through the above process, this application can more accurately determine the corner points and descriptor features included in the current image data, so as to more accurately determine the visual bag-of-words vector and global feature information.

[0043] For S103, it can be determined whether the current frame point cloud data belongs to a keyframe. Since the robot is in motion during the continuous sampling process of the LiDAR, the collected point cloud data has motion distortion, which needs to be compensated. Therefore, motion compensation can be performed by the SLAM front-end odometry to obtain the radar pose at the corresponding moment; then it is determined whether the relative pose deviation between the radar pose and the radar pose of the previous keyframe is greater than the set relative pose threshold. If so, it is determined that the current frame point cloud data belongs to a keyframe; otherwise, it is determined that the current frame point cloud data does not belong to a keyframe.

[0044] If the current frame's point cloud data is determined to belong to a keyframe, then the current frame's point cloud data is updated in the keyframe point cloud list. If it does not belong to a keyframe, then it does not need to be added to the keyframe point cloud list, and the acquisition of the next frame's point cloud data continues.

[0045] Next, based on the timestamp corresponding to the current image data, the most recent radar keyframe that meets the time threshold requirement is determined from the keyframe point cloud list. This keyframe is used as the target point cloud data for matching the current image data, and the target point cloud data and the current image data are combined to form the current data pair. If no matching keyframe exists in the current image data, the next image data can be acquired.

[0046] For S104, the visual bag-of-words vector and / or global feature information of the current image data can be used to determine whether there are loop closure candidate data pairs matching the current image data among multiple historical data pairs, in order to initially determine whether to enter the loop closure confirmation process. For example, the visual bag-of-words vector of the current image data and the visual bag-of-words vectors of the historical image data in each historical data pair can be used to calculate the matching score. Historical data pairs with matching scores exceeding the score threshold are identified as loop closure candidate data pairs matching the current image data; if no historical data pairs with matching scores exceeding the score threshold exist, it is determined that there are no loop closure candidate data pairs matching the current image data. Similarly, the feature matching degree can be calculated using global feature information to determine loop closure candidate data pairs based on the feature matching degree.

[0047] Optionally, determining whether there exists a loopback candidate data pair matching the current image data among multiple historical data pairs using the feature data of the current image data includes: Step b1: Based on the visual bag-of-words vector of the current image data and the visual bag-of-words vector of the historical image data in the historical data pairs stored in the visual bag-of-words database, determine whether there are candidate historical image data with a bag-of-words matching score greater than the set first matching threshold. Step b2: If it exists, for each candidate historical image data, determine the feature matching score based on the global feature information of the current image data and the global feature information of the candidate historical image data stored in the global feature database; Step b3: If the feature matching score is greater than the set second matching threshold, then determine the time difference between the timestamp of the target point cloud data associated with the current image data and the timestamp of the historical point cloud data associated with the candidate historical image data. Step b4: If the time difference is greater than the set time difference threshold, then the historical data pair including the candidate historical image data is determined to be a loopback candidate data pair.

[0048] In practice, a three-level screening process can be achieved through the specific sequential sequence of "word bag initial screening → feature point matching → time difference filtering" to accurately select loop-loop candidate data pairs from historical data pairs.

[0049] First, the initial screening is a bag-of-words (BOD) screening. Using the visual bag-of-words vector corresponding to the current image data, historical data pairs whose bag-of-words matching scores meet a threshold are selected. Specifically, the BOD matching scores are calculated between the visual bag-of-words vector corresponding to the current image data and the visual bag-of-words vectors of each historical image data stored in the BOD database. It is then determined whether there are candidate historical image data pairs with a bag-of-words matching score greater than the set first matching threshold. If such pairs exist, the second-level screening continues; otherwise, the process of acquiring the next image data and point cloud data continues.

[0050] The second level of selection is feature matching, which involves calculating the matching score of feature points between two image frames when the bag-of-words matching requirement is met. In practice, for candidate historical image data whose bag-of-words matching score meets the requirement, global feature information of the candidate historical image data is obtained from the global feature database, and the feature matching score between the global feature information of the current image data and the global feature information of the candidate historical image data is calculated. For example, the dimensions of the global feature information can be unified, and the similarity distance can be calculated, with the similarity distance determined as the feature matching score.

[0051] Alternatively, for the candidate historical image data that has passed the initial screening, the feature points included in it can be retrieved from the global feature database, and the nearest neighbor distance ratio test, random sampling consistency (RANSAC) geometric verification, inlier count verification, etc. can be performed to determine whether it meets the feature matching requirements.

[0052] Specifically, the system retrieves feature point coordinates and descriptors from the global feature database for the current image data (current frame) and candidate historical image data (candidate historical frames). First, a nearest neighbor ratio test is used to obtain candidate matching point pairs: for a feature point in the current frame, the two feature points with the closest Hamming distance are found among all feature points in the candidate historical frames, and distances d1 and d2 are calculated. If d1 / d2 is less than a set threshold, the matching pair is accepted; otherwise, it is rejected. Then, the RANSAC algorithm, combined with geometric models such as the fundamental matrix or homography matrix, is used to calculate the reprojection error, and a threshold is set based on the chi-square test to filter inliers and outliers. Finally, the feature matching score is determined based on the number of inliers. For example, if the number of inliers is greater than a set second matching threshold (e.g., the number of inliers is greater than 30, or the proportion of inliers to the total number of matching point pairs is greater than 40%), the second level of filtering is passed, and the third level of filtering continues.

[0053] The third level of filtering is time difference filtering, which determines whether the difference in timestamps between two keyframes in the images meets the requirements. If it does, loop closure candidate keyframe pairs are obtained. Specifically, the time difference between the timestamp of the target point cloud data associated with the current image data and the timestamp of the historical point cloud data associated with the candidate historical image data is calculated. If the time difference is greater than the set time difference threshold, it is determined that the visual features of the candidate historical image data meet the initial conditions for loop closure, and the historical data pair including the candidate historical image data is determined as a loop closure candidate data pair.

[0054] This application achieves multi-level cascaded gating through the above process, effectively reducing false positives and false negatives for loop-loop candidate data pairs. Specifically, this application connects visual bag-of-words initial screening, feature point matching, and temporal difference filtering in a specific order to form a three-level gating mechanism. This allows visual coarse screening to save computational resources, feature matching to fine-tune the screening to ensure accuracy, and temporal difference filtering to eliminate short-term false positives. The logic is rigorous and efficient, achieving relatively efficient and accurate screening of loop-loop candidate data pairs.

[0055] For S105, when loop closure candidate data pairs exist, an environmental complexity index is determined based on the current image data and / or target point cloud data, and the number of aggregated frames matching the environmental complexity index is determined. For example, evaluation information such as gradient statistics, corner density and spatial distribution, and descriptor repetition of the current image data can be determined. Gradient statistics include image gradient, gradient mean, and gradient variance. Based on the determined evaluation information, the environmental complexity is determined. For example, if the evaluation information indicates that the current image data has many corners that are evenly distributed, moderate gradients, and no large amount of repetitive textures, it indicates sufficient visual constraints, i.e., high environmental complexity, belonging to a structurally clear region, and the number of aggregated frames matching can be small. Conversely, if the evaluation information indicates that the current image data has few corners or corners concentrated in a small area, and a large amount of repetitive textures, it indicates insufficient visual constraints and a risk of degradation, i.e., low environmental complexity, belonging to a geometrically degraded region, and the number of aggregated frames matching can be large.

[0056] For example, for target point cloud data, multiple point clouds can be sampled. The local point cloud covariance matrix is ​​calculated using K-nearest neighbors for the sampled point clouds, and eigenvalues ​​are obtained through eigenvalue decomposition. Geometric indices, such as flatness, linearity, and scattering, are then calculated based on these three eigenvalues. This yields the average flatness, average linearity, and average scattering of the target point cloud data. Furthermore, the complexity of the environment can be assessed based on these average flatness, average linearity, and average scattering. For instance, if the average scattering is high and none of the three eigenvalues ​​significantly approaches 0, it indicates a clear geometric structure; if the flatness or linearity is too high and the smallest eigenvalue is close to 0, it indicates a geometrically degraded region.

[0057] For example, the environmental complexity can be determined by combining the evaluation information of the target point cloud data and the evaluation information of the current image data.

[0058] During implementation, the mapping relationship between environmental complexity and the number of aggregated frames can also be maintained so that the matching number of aggregated frames can be determined based on the environmental complexity.

[0059] For S106 and S107, when there are multiple loop closure candidate data pairs, all loop closure candidate data pairs can be traversed, and subsequent point cloud sub-map registration processes can be performed for each loop closure candidate data pair.

[0060] A point cloud sub-map can be constructed using aggregated frame data. For example, if the number of aggregated frames is 5, a historical point cloud sub-map can be constructed using the key point cloud data in the loopback candidate data pair and the first four frames of historical point cloud data (keyframes) preceding the key point cloud data; and a current point cloud sub-map can be constructed using the target point cloud data and the second four frames of historical point cloud data (keyframes) preceding the target point cloud data.

[0061] The historical point cloud sub-map and the current point cloud sub-map are registered to determine the registration result. When the registration result indicates successful registration, the loop closure process is executed. The loop closure process includes: First, determining the loop closure relative transformation matrix. Specifically, based on the valid confirmation of the loop closure result, the relative pose transformation relationship between the two sub-maps is calculated by matching the feature information and point cloud information of the current point cloud sub-map and the historical point cloud sub-map. The loop closure relative transformation matrix containing translation and rotation parameters is accurately obtained, providing a precise pose transformation reference for subsequent sub-map alignment and fusion.

[0062] The second step involves injecting the backend pose graph optimization module for global pose correction. The previously obtained loop-loop relative transformation constraint relationship is integrated into the SLAM backend pose graph optimization module. Using the loop-loop constraint as the core correction condition, the pose drift error accumulated during map construction is globally optimized and calibrated. This corrects the global pose parameters of each sub-map node, eliminates the local pose deviation caused by the independent construction of a single sub-map, and achieves the global optimal solution for the pose of all sub-maps.

[0063] The third step is to complete the historical sub-map transformation and alignment based on the optimized pose. Relying on the accurate global pose output from the backend optimization, and combined with the loop relative transformation matrix, the overall coordinate transformation and pose calibration of the historical sub-maps to be merged are performed, so that the current sub-map and the matched historical sub-maps are unified under the same global coordinate system, eliminating the coordinate offset and pose misalignment problems between multiple sub-maps, and achieving accurate alignment of the spatial positions of the two sets of sub-maps.

[0064] Step 4: Perform point cloud fusion processing for overlapping areas. For the spatially overlapping areas of the two sets of sub-maps after alignment, redundant, duplicate, and biased point clouds within the overlapping range are filtered out. Through point cloud deduplication, noise filtering, weighted fusion, and coordinate mean correction, the point cloud data in the overlapping areas is integrated and reconstructed. This preserves effective topographic and environmental feature point cloud information, eliminates duplicate and redundant data, and optimizes the accuracy and completeness of the point clouds in the overlapping areas.

[0065] Step 5: Iteratively update the globally consistent map. Update the merged local sub-maps into the global map system, replacing the original old sub-map data that has discrepancies or overlaps, and complete the iterative update of the global map.

[0066] The above process, through closed-loop pose optimization and local fusion mechanism, completely eliminates the geometric ambiguity and coordinate inconsistency problems in the overlapping areas of sub-maps, effectively suppresses the cumulative error of map construction, and significantly improves the spatial continuity, geometric consistency and overall accuracy of the global map.

[0067] Optionally, the registration based on the historical point cloud sub-map and the current point cloud sub-map, and the determination of the registration result, includes: Step c1: Register the historical point cloud sub-map and the current point cloud sub-map to determine the transformation matrix, inlier rate, and registration score; Step c2: Based on the transformation matrix, perform transformation processing on the historical point cloud sub-map to generate the processed historical point cloud sub-map; Step c3: Determine the overlap rate and normal consistency index based on the processed historical point cloud sub-map and the current point cloud sub-map; Step c4: Determine the fusion confidence score based on the inlier rate, the registration score, the overlap rate, and the normal consistency index; Step c5: When the fusion confidence score indicates high confidence, the registration result is determined to be successful; when the fusion confidence score indicates medium confidence, the registration result is determined to be pending confirmation; when the fusion confidence score indicates low confidence, the registration result is determined to be unsuccessful.

[0068] This application performs high-precision registration calculations based on the constructed historical point cloud sub-maps and the current point cloud sub-maps, and combines multi-dimensional evaluation indicators to complete the hierarchical determination of the registration results.

[0069] In step c1, a bi-sub-map registration operation is performed, and basic registration parameters are output. Point cloud feature matching and spatial registration calculations are performed on the historical point cloud sub-map and the current point cloud sub-map to obtain the spatial transformation matrix between the sub-maps. Simultaneously, the inlier rate and registration score corresponding to the registration process are statistically analyzed, serving as basic quantitative evaluation parameters for the degree of spatial matching between the sub-maps. The transformation matrix characterizes the coordinate translation and attitude rotation correspondence between the two sets of sub-maps, while the inlier rate and registration score are used to initially measure the registration matching accuracy and effectiveness.

[0070] In step c2, the spatial transformation of the historical point cloud sub-map is completed based on the transformation matrix. According to the transformation matrix obtained in step c1, global coordinate transformation and attitude correction are performed on the historical point cloud sub-map, and the historical point cloud sub-map is uniformly mapped to the coordinate system of the current point cloud sub-map to obtain the processed historical point cloud sub-map with coordinate alignment. This eliminates the coordinate system deviation between the two sets of sub-maps and provides a unified spatial reference for subsequent evaluation of overlapping features and normal features.

[0071] In step c3, a multidimensional consistency evaluation index is constructed. Based on the processed historical point cloud sub-maps after coordinate system alignment and the current point cloud sub-maps, a quantitative evaluation of spatial overlap characteristics and normal consistency is conducted. On the one hand, by comparing the spatial coverage of the two sets of sub-maps, the sub-map overlap rate is calculated, representing the degree of spatial overlap between the two sets of sub-maps; on the other hand, by extracting the normal vector features of the point clouds in the corresponding areas of the two sets of sub-maps, the normal consistency index is calculated, representing the matching consistency of local environmental structure and terrain surface.

[0072] In step c4, a fusion confidence score is calculated by integrating multi-dimensional indicators. The aforementioned inlier rate, registration score, sub-map overlap rate, and normal consistency index are integrated. Using a preset weighting rule and quantitative scoring model, the multi-dimensional registration evaluation parameters are fused and calculated, outputting a fusion confidence score that comprehensively characterizes the overall matching reliability of the dual-point cloud sub-maps, thus achieving a quantitative and unified evaluation of the registration effect.

[0073] In step c5, the registration result is graded based on the fusion confidence score. Three confidence threshold ranges (high, medium, and low) are preset. The calculated fusion confidence score is compared with these preset thresholds to grade the registration result. When the fusion confidence score falls within the high confidence threshold range, the registration validity of the two sets of sub-maps meets the preset accuracy requirements, and the registration result is determined to be successful. When the fusion confidence score falls within the medium confidence threshold range, it indicates uncertainty in the sub-map matching effect and insufficient matching reliability; the registration result is determined to be pending confirmation and requires further verification. When the fusion confidence score falls within the low confidence threshold range, it indicates a large matching deviation between the two sets of sub-maps and extremely low matching validity; the registration result is determined to be failed.

[0074] If the registration result is successful, loop closure is confirmed; if the registration result is unsuccessful, subsequent loop closure processing is not executed. If the registration result is pending confirmation, the current data pair is added to the pending confirmation queue so that loop closure verification can be performed again based on subsequently acquired image and point cloud data. For example, if subsequent consecutive frames show high or medium confidence, the loop closure processing is confirmed; if subsequent detection determines low confidence, the loop closure processing is not executed.

[0075] Optionally, after the registration result indicates successful registration, but before determining to perform loop closure processing, the method further includes: continuing to acquire image data and point cloud data collected by the robot; using the acquired image data and point cloud data, determining whether the frame data meets the following verification conditions: the detected region is the same region as the historical region indicated by the loop closure candidate data pair, and the determined registration result is high confidence or medium confidence; if multiple consecutive frames of data meet the verification conditions, then the verification result of the loop closure candidate data pair is determined to be verified as passed.

[0076] This application adds a multi-frame continuous data verification step after the sub-map registration result is determined to be successful and before the formal execution of the loopback processing flow. This is to avoid false loopback problems caused by single-frame registration mismatch, environmental noise, and random interference, and to improve the reliability and robustness of loopback triggering.

[0077] In practice, after obtaining a high-confidence registration result and initially determining the existence of valid loop closure candidate relationships, image data and point cloud data during the robot's movement are continuously collected. Following the aforementioned S102-S107 process, the validity of the current sensor data is verified frame by frame. This step pre-sets dual verification conditions: the environmental area detected by the sensor data of the current frame is the same physical area as the historical loop closure area corresponding to the loop closure candidate data pair, and the sub-map registration result corresponding to the current frame is a high-confidence or medium-confidence valid registration state.

[0078] The image data and point cloud data acquired in real time for multiple consecutive frames are checked frame by frame. If the data in multiple consecutive frames consistently meet the above verification conditions, it is determined that the current loop closure matching relationship is not an accidental mismatch, thus eliminating the randomness error of single registration. Finally, the verification result of the loop closure candidate data pair is determined to be that the verification is passed.

[0079] Only after the loop closure candidate data passes the verification can the subsequent complete loop closure processing flow be formally triggered and executed, including operations such as pose optimization, sub-map alignment, and point cloud fusion. If multiple consecutive frames of data fail to meet the verification conditions, or if the inter-frame matching status is unstable, or if the registration confidence level is insufficient, the current loop closure candidate verification is deemed unsuccessful, the current loop closure matching result is considered invalid, and there is no need to initiate the subsequent loop closure processing flow; the current loop closure candidate result is directly discarded to avoid the negative impact of invalid loop closure correction on the global map accuracy.

[0080] This application can effectively distinguish between perceived confusion and actual revisits through cross-frame timing consistency verification, thereby improving the accuracy of loopback processing.

[0081] Optionally, before registering based on the historical point cloud sub-map and the current point cloud sub-map, the method further includes: determining a first global descriptor corresponding to the historical point cloud sub-map and a second global descriptor corresponding to the current point cloud sub-map; and determining the descriptor distance based on the first global descriptor and the second global descriptor.

[0082] The step of registering based on the historical point cloud sub-map and the current point cloud sub-map to determine the registration result includes: when the descriptor distance is less than a set distance threshold, registering based on the historical point cloud sub-map and the current point cloud sub-map to determine the registration result.

[0083] Before performing precise registration between historical point cloud sub-maps and current point cloud sub-maps, this application sets up a pre-screening step based on global descriptors. Invalid registration combinations are eliminated by judging global feature similarity, which reduces the computational overhead of subsequent high-precision registration and improves the overall algorithm efficiency.

[0084] In practice, firstly, global feature descriptors corresponding to two sets of sub-maps to be matched are extracted and determined. Specifically, the first global descriptor corresponding to the historical point cloud sub-map is obtained, and the second global descriptor corresponding to the current point cloud sub-map is extracted. These two types of global descriptors are used to represent the overall environmental structure and global feature attributes of the corresponding sub-maps, respectively, thereby achieving a quantitative representation of the overall features of the sub-maps.

[0085] Secondly, similarity is calculated based on the first and second global descriptors to obtain the descriptor distance between the two sets of global descriptors. This descriptor distance can quantitatively reflect the global feature similarity between the historical point cloud sub-map and the current point cloud sub-map. The smaller the descriptor distance, the higher the overall environmental feature similarity between the two sets of sub-maps, and the greater the possibility of effective loop closure matching.

[0086] Finally, registration admission is determined based on descriptor distance. When the calculated descriptor distance is less than a preset distance threshold, the global feature similarity between the two sets of sub-maps is determined to meet the registration admission condition. The aforementioned precise registration operation between the historical point cloud sub-map and the current point cloud sub-map can then be performed, and the final registration result is output based on multi-dimensional evaluation indicators. If the descriptor distance is greater than or equal to the preset distance threshold, the global feature difference between the two sets of sub-maps is determined to be too large, and there is no effective matching basis. The subsequent precise registration process is skipped to avoid invalid registration calculations.

[0087] See Figure 2 As shown, combined with Figure 2 The loop closure detection method proposed in this application is illustrated by example. Specifically, after the robot runs, it performs S1: multi-source data acquisition, that is, acquiring the current image data and the current frame point cloud data.

[0088] S2: Visual Feature Extraction. Specifically, the current image data is converted to grayscale and distortion-corrected to obtain processed image data. A pyramid image is then generated based on the processed image data. Corner detection is performed on the pyramid image, and descriptors for each corner are determined. Based on the descriptors of each corner, a bag-of-words vector and global feature information are generated. The bag-of-words vector is stored in the bag-of-words database, and the global feature information is stored in the global feature database.

[0089] S3: Point cloud preprocessing. First, motion compensation is performed on the point cloud data, and based on the determined point cloud pose and the historical pose of the previous keyframe, it is determined whether it belongs to a keyframe; if it does, the current frame point cloud data is updated to the keyframe point cloud list.

[0090] S4: Time-domain correlation of radar vision. Based on the image timestamp, determine the target point cloud data that matches the current image data from the list of keyframe point clouds.

[0091] S5: Visual three-level cascaded filtering. The three-level filtering includes: Level 1: Bag-of-words initial screening; Level 2: Feature point matching; Level 3: Temporal difference filtering. After filtering through the three levels, loop closure candidate data pairs are output. The filtering process can be referred to the previous description.

[0092] S6: Adaptive mapping based on environmental complexity. Specifically, candidate loop closure pairs can be traversed in order of score (e.g., matching score). Environmental complexity is evaluated to determine the number of aggregation frames. Sub-maps are constructed using the point cloud data of the aggregation frames, i.e., the current point cloud sub-map and historical point cloud sub-maps are constructed.

[0093] S7: Point Cloud Registration and Fusion Confidence Assessment. Optionally, initial screening can be performed based on the sub-map level global descriptor. After passing the initial screening, ICP fine registration is performed, outputting the transformation matrix, inlier rate, and registration score. Post-processing is then performed to calculate the overlap rate and normal consistency. Finally, the fusion confidence score is determined based on the inlier rate, registration score, overlap rate, and normal consistency index. A three-level classification is applied based on the fusion confidence score: high confidence scores are subject to loopback confirmation, medium confidence scores are added to the confirmation queue, and low confidence scores are discarded.

[0094] S8: Timing consistency cross-frame verification. Two verification conditions are set: verification condition 1: multiple consecutive frames point to the same historical region; verification condition 2: registration quality consistently meets the standard. If multiple subsequent consecutive frames meet the above conditions, the verification passes.

[0095] S9: Execute the loopback process.

[0096] Therefore, this application has at least the following characteristics: 1. It dynamically adjusts the spatiotemporal range of the sub-map according to environmental complexity, aggregating structurally clear areas in fewer frames and geometrically degraded areas in more frames, thus achieving dynamic adaptive adjustment of the sub-map scale. 2. It has a fusion confidence level confirmation mechanism, that is, it performs level confirmation by comprehensively considering the inlier rate, registration score, normal consistency and overlap rate, so as to accurately determine whether to execute the loopback processing flow. 3. It sets up a temporal consistency cross-frame verification logic, that is, it verifies whether subsequent consecutive frames can still successfully match the same area in the past. 4. Local sub-map fusion after loopback, specifically, after confirming the loopback and optimizing the pose, this application performs local fusion reconstruction on the overlapping areas. 5. It sets up a sub-map-level global descriptor coarse search, so as to quickly narrow down the candidate range before fine registration, while taking into account the real-time performance of large-scale scenes.

[0097] Corresponding to the embodiments of the aforementioned loop closure detection method, this application also provides embodiments of a loop closure detection device. Figure 3 A schematic diagram of the loop closure detection device provided in this application specifically includes: The acquisition module 301 is used to acquire the current image data and current frame point cloud data collected in real time during the robot's movement. The first determining module 302 is used to determine the visual bag vector and global feature information corresponding to the current image data, and store the global feature information in the global feature database and the visual bag vector in the visual bag database. The second determining module 303 is used to update the current frame point cloud data to the key frame point cloud list when it is determined that the current frame point cloud data belongs to a key frame, and to determine the target point cloud data that matches the current image data from the key frame point cloud list, wherein the target point cloud data and the current image data constitute a current data pair. The third determining module 304 is used to determine whether there is a loop closure candidate data pair matching the current image data among multiple historical data pairs using the feature data of the current image data; wherein the feature data includes at least one of the following: visual bag-of-words vector, global feature information; The fourth determining module 305 is used to determine an environmental complexity index based on the current image data and / or the target point cloud data if the environment complexity index exists, and to determine the number of aggregated frames that match the environmental complexity index. The construction module 306 is used to construct a historical point cloud sub-map using the aggregated frame of several first point cloud data, and to construct a current point cloud sub-map using the aggregated frame of several second point cloud data; wherein the first point cloud data includes key point cloud data in the loopback candidate data pair, and first historical point cloud data before the key point cloud data; the second point cloud data includes the target point cloud data and second historical point cloud data before the target point cloud data; The fifth determining module 307 is used to perform registration based on the historical point cloud sub-map and the current point cloud sub-map, determine the registration result, and determine to execute the loopback processing flow when the registration result indicates successful registration.

[0098] In an optional implementation, the first determining module, when determining the visual bag-of-words vector and global feature information corresponding to the current image data, is used to: The data format of the current image data is converted to grayscale format, and the converted current image data is subjected to distortion correction to generate processed image data; Based on the processed image data, pyramid images of various sizes and scales are generated; Fast FAST corner feature detection and descriptor calculation are performed on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner. Based on the descriptor features corresponding to each corner point and the set visual dictionary, a bag-of-words vector corresponding to the current image data is generated; The corner points included in the current image data are filtered, and global feature information corresponding to the current image data is generated based on the descriptor features corresponding to the filtered corner points.

[0099] In an optional implementation, when the first determining module performs fast FAST corner feature detection and descriptor calculation on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner, it is used to: For each image in the pyramid image, traverse each pixel in the image and treat each pixel as a pixel to be processed, then perform the following steps: A sampling neighborhood is constructed centered on the pixel to be processed. The pixel information of candidate pixels at multiple target orientations on the boundary of the sampling neighborhood, with the pixel to be processed as the reference, is determined, and the first pixel deviation is determined between them and the pixel information of the pixel to be processed. The number of first pixels whose absolute value of the first pixel deviation is greater than the first pixel threshold is also determined. If the number of the first pixel points is greater than the set first number threshold, then it is determined whether the multiple candidate pixel points on the boundary of the sampling neighborhood meet the following screening conditions: the pixel information of at least n consecutive candidate pixel points is greater than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold; or, the pixel information of at least n consecutive candidate pixel points is less than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold. If the conditions are met, then the pixel to be processed is determined to be a candidate corner point; After traversing all pixels in the image, non-maximum suppression processing is performed on multiple candidate corner points included in the image to obtain the corner points included in the image, and a descriptor feature corresponding to each corner point is generated. The corner points and descriptive features corresponding to each image in the pyramid image are determined as the corner points and descriptive features corresponding to each corner point included in the current image data.

[0100] In an optional implementation, when the third determining module uses the feature data of the current image data to determine whether there is a loop closure candidate data pair matching the current image data among multiple historical data pairs, it is used to: Based on the visual bag-of-words vector of the current image data and the visual bag-of-words vector of the historical image data in the historical data pairs stored in the visual bag-of-words database, it is determined whether there are candidate historical image data with a bag-of-words matching score greater than the set first matching threshold. If they exist, for each candidate historical image data, a feature matching score is determined based on the global feature information of the current image data and the global feature information of the candidate historical image data stored in the global feature database. If the feature matching score is greater than the set second matching threshold, then the time difference between the timestamp of the target point cloud data associated with the current image data and the timestamp of the historical point cloud data associated with the candidate historical image data is determined. If the time difference is greater than the set time difference threshold, then the historical data pair including the candidate historical image data is determined to be a loopback candidate data pair.

[0101] In an optional implementation, when the fifth determining module performs registration based on the historical point cloud sub-map and the current point cloud sub-map and determines the registration result, it is used to: The historical point cloud sub-map and the current point cloud sub-map are registered to determine the transformation matrix, inlier rate, and registration score. Based on the transformation matrix, the historical point cloud sub-map is transformed to generate a processed historical point cloud sub-map. Based on the processed historical point cloud sub-map and the current point cloud sub-map, the overlap rate and normal consistency index are determined; The fusion confidence score is determined based on the inlier rate, the registration score, the overlap rate, and the normal consistency index. When the fusion confidence score indicates high confidence, the registration result is determined to be successful. When the fusion confidence score indicates a medium confidence level, the registration result is determined to be pending confirmation. When the fusion confidence score indicates low confidence, the registration result is determined to be a registration failure.

[0102] In an optional implementation, after the registration result indicates successful registration, the fifth determining module is further configured to: Continue acquiring image and point cloud data collected by the robot; Using the collected image data and point cloud data, determine whether the frame data meets the following verification conditions: the detected region is the same region as the historical region indicated by the loop closure candidate data pair, and the determined registration result is high confidence or medium confidence. If multiple consecutive frames of data meet the verification conditions, the verification result of the loopback candidate data pair is determined to be a successful verification. The process of determining to execute the loopback process includes: after the review is approved, determining to execute the loopback process.

[0103] In an optional implementation, before registration is performed based on the historical point cloud sub-map and the current point cloud sub-map, the fifth determining module is further configured to: Determine the first global descriptor corresponding to the historical point cloud sub-map, and the second global descriptor corresponding to the current point cloud sub-map; Determine the descriptor distance based on the first global descriptor and the second global descriptor; When the fifth determining module performs registration based on the historical point cloud sub-map and the current point cloud sub-map and determines the registration result, it is used for: When the distance between the descriptors is less than the set distance threshold, registration is performed based on the historical point cloud sub-map and the current point cloud sub-map to determine the registration result.

[0104] The specific implementation process of the functions and roles of each unit in the above device can be found in the implementation process of the corresponding steps in the above method, and will not be repeated here.

[0105] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this application according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0106] This application also provides a computer-readable storage medium storing a computer program that can be used to execute the loop closure detection method described in the above embodiments.

[0107] This application also provides a computer device, see [link to relevant documentation] Figure 4 The diagram shown illustrates the structure of the computer device provided in this application. At the hardware level, the computer device includes a processor, an internal bus, a network interface, memory, and non-volatile memory, and may also include other hardware required for various operations. The processor reads the corresponding computer program from the non-volatile memory into memory and then runs it to implement the loopback detection method described in the above embodiments. Of course, besides software implementation, this specification does not exclude other implementation methods, such as logic devices or a combination of hardware and software, etc. That is to say, the execution entity of the following processing flow is not limited to individual logic units, but can also be hardware or logic devices.

[0108] Those skilled in the art will understand that embodiments of this application can be provided as methods, apparatus, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs) containing computer-usable program code. The form of a computer program product implemented on ROM, optical memory, etc.

[0109] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0110] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0111] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the functions specified in one or more boxes. In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0112] Memory may include non-persistent storage in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0113] Computer-readable media, including both permanent and non-permanent, removable and non-removable media, can store information using any method or technology. Information can be computer-readable instructions, data structures, program modules, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, and optical disc read-only memory (CD). ROM, digital multifunction optical disc (DVD) or other optical storage, magnetic cassette, magnetic tape, magnetic disk storage or other magnetic storage devices or any other non-transfer media, may be used to store information that can be accessed by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0114] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0115] This specification can be described in the general context of computer-executable instructions that are executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc., that perform a specific task or implement a specific abstract data type. This specification can also be practiced in distributed computing environments, where tasks are performed by remote processing devices connected via a communication network. In distributed computing environments, program modules can reside in local and remote computer storage media, including storage devices.

[0116] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to interchangeably. Each embodiment focuses on describing the differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments.

[0117] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A loopback detection method, characterized in that, The method includes: Acquire real-time image data and current frame point cloud data collected by the robot during its movement; Determine the bag-of-visual-words vector and global feature information corresponding to the current image data, and store the global feature information in the global feature database and the bag-of-visual-words vector in the bag-of-visual-words database; When it is determined that the current frame point cloud data belongs to a key frame, the current frame point cloud data is updated to the key frame point cloud list, and the target point cloud data matching the current image data is determined from the key frame point cloud list, wherein the target point cloud data and the current image data constitute the current data pair; Using the feature data of the current image data, determine whether there are loop closure candidate data pairs that match the current image data among multiple historical data pairs; wherein the feature data includes at least one of the following: visual bag-of-words vector, global feature information; If it exists, then based on the current image data and / or the target point cloud data, determine the environmental complexity index, and determine the number of aggregated frames that match the environmental complexity index; Using the aggregated frame of several first point cloud data, a historical point cloud sub-map is constructed, and using the aggregated frame of several second point cloud data, a current point cloud sub-map is constructed; wherein the first point cloud data includes the key point cloud data in the loopback candidate data pair, and the first historical point cloud data before the key point cloud data; the second point cloud data includes the target point cloud data and the second historical point cloud data before the target point cloud data; Based on the historical point cloud sub-map and the current point cloud sub-map, registration is performed to determine the registration result; when the registration result indicates successful registration, the loopback process is executed.

2. The method according to claim 1, characterized in that, Determining the visual bag-of-words vector and global feature information corresponding to the current image data includes: The data format of the current image data is converted to grayscale format, and the converted current image data is subjected to distortion correction to generate processed image data; Based on the processed image data, pyramid images of various sizes and scales are generated; Fast FAST corner feature detection and descriptor calculation are performed on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner. Based on the descriptor features corresponding to each corner point and the set visual dictionary, a bag-of-words vector corresponding to the current image data is generated; The corner points included in the current image data are filtered, and global feature information corresponding to the current image data is generated based on the descriptor features corresponding to the filtered corner points.

3. The method according to claim 2, characterized in that, The step of performing fast FAST corner feature detection and descriptor calculation on each image in the pyramid image to determine the corners included in the current image data and the descriptor features corresponding to each corner includes: For each image in the pyramid image, traverse each pixel in the image and treat each pixel as a pixel to be processed, then perform the following steps: A sampling neighborhood is constructed centered on the pixel to be processed. The pixel information of candidate pixels at multiple target orientations on the boundary of the sampling neighborhood, with the pixel to be processed as the reference, is determined, and the first pixel deviation is determined between them and the pixel information of the pixel to be processed. The number of first pixels whose absolute value of the first pixel deviation is greater than the first pixel threshold is also determined. If the number of the first pixel points is greater than the set first number threshold, then it is determined whether the multiple candidate pixel points on the boundary of the sampling neighborhood meet the following screening conditions: the pixel information of at least n consecutive candidate pixel points is greater than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold; or, the pixel information of at least n consecutive candidate pixel points is less than the pixel information of the pixel to be processed, and the second pixel deviation between the pixel information is greater than the set second pixel threshold. If the conditions are met, then the pixel to be processed is determined to be a candidate corner point; After traversing all pixels in the image, non-maximum suppression processing is performed on multiple candidate corner points included in the image to obtain the corner points included in the image, and a descriptor feature corresponding to each corner point is generated. The corner points and descriptive features corresponding to each image in the pyramid image are determined as the corner points and descriptive features corresponding to each corner point included in the current image data.

4. The method according to claim 1, characterized in that, The step of using the feature data of the current image data to determine whether there is a loop closure candidate data pair matching the current image data among multiple historical data pairs includes: Based on the visual bag-of-words vector of the current image data and the visual bag-of-words vector of the historical image data in the historical data pairs stored in the visual bag-of-words database, it is determined whether there are candidate historical image data with a bag-of-words matching score greater than the set first matching threshold. If they exist, for each candidate historical image data, a feature matching score is determined based on the global feature information of the current image data and the global feature information of the candidate historical image data stored in the global feature database. If the feature matching score is greater than the set second matching threshold, then the time difference between the timestamp of the target point cloud data associated with the current image data and the timestamp of the historical point cloud data associated with the candidate historical image data is determined. If the time difference is greater than the set time difference threshold, then the historical data pair including the candidate historical image data is determined to be a loopback candidate data pair.

5. The method according to claim 1, characterized in that, The registration process based on the historical point cloud sub-map and the current point cloud sub-map, and the determination of the registration result, includes: The historical point cloud sub-map and the current point cloud sub-map are registered to determine the transformation matrix, inlier rate, and registration score. Based on the transformation matrix, the historical point cloud sub-map is transformed to generate a processed historical point cloud sub-map. Based on the processed historical point cloud sub-map and the current point cloud sub-map, the overlap rate and normal consistency index are determined; The fusion confidence score is determined based on the inlier rate, the registration score, the overlap rate, and the normal consistency index. When the fusion confidence score indicates high confidence, the registration result is determined to be successful. When the fusion confidence score indicates a medium confidence level, the registration result is determined to be pending confirmation. When the fusion confidence score indicates low confidence, the registration result is determined to be a registration failure.

6. The method according to claim 5, characterized in that, After the registration result indicates successful registration, the method further includes: Continue acquiring image and point cloud data collected by the robot; Using the collected image data and point cloud data, determine whether the frame data meets the following verification conditions: the detected region is the same region as the historical region indicated by the loop closure candidate data pair, and the determined registration result is high confidence or medium confidence. If multiple consecutive frames of data meet the verification conditions, the verification result of the loopback candidate data pair is determined to be a successful verification. The process of determining and executing the loopback procedure includes: After the review is approved, the loopback process will be executed.

7. The method according to claim 1, characterized in that, Before registration based on the historical point cloud sub-map and the current point cloud sub-map, the process also includes: Determine the first global descriptor corresponding to the historical point cloud sub-map, and the second global descriptor corresponding to the current point cloud sub-map; Determine the descriptor distance based on the first global descriptor and the second global descriptor; The registration process based on the historical point cloud sub-map and the current point cloud sub-map, and the determination of the registration result, includes: When the distance between the descriptors is less than the set distance threshold, registration is performed based on the historical point cloud sub-map and the current point cloud sub-map to determine the registration result.

8. A loop closure detection device, characterized in that, The device includes: The acquisition module is used to acquire the current image data and current frame point cloud data collected in real time during the robot's movement. The first determining module is used to determine the visual bag-of-words vector and global feature information corresponding to the current image data, and store the global feature information in the global feature database and the visual bag-of-words vector in the visual bag-of-words database. The second determining module is used to update the current frame point cloud data to the key frame point cloud list when it is determined that the current frame point cloud data belongs to a key frame, and to determine the target point cloud data that matches the current image data from the key frame point cloud list, wherein the target point cloud data and the current image data constitute a current data pair; The third determining module is used to determine whether there is a loop closure candidate data pair matching the current image data among multiple historical data pairs using the feature data of the current image data; wherein the feature data includes at least one of the following: visual bag-of-words vector, global feature information; The fourth determining module is used to determine an environmental complexity index based on the current image data and / or the target point cloud data, if such an index exists, and to determine the number of aggregated frames that match the environmental complexity index. A construction module is used to construct a historical point cloud sub-map using the aggregated frame's first point cloud data, and to construct a current point cloud sub-map using the aggregated frame's second point cloud data; wherein the first point cloud data includes key point cloud data in the loopback candidate data pair, and first historical point cloud data preceding the key point cloud data; the second point cloud data includes the target point cloud data and second historical point cloud data preceding the target point cloud data; The fifth determining module is used to perform registration based on the historical point cloud sub-map and the current point cloud sub-map, determine the registration result, and determine to execute the loopback processing flow when the registration result indicates successful registration.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by a processor, it implements the steps of the method according to any one of claims 1-7.

10. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, The processor performs the steps of the method according to any one of claims 1-7.

Images