SLAM loopback detection method and device
By extracting and filtering keyframes from point cloud data in the SLAM system and reconstructing them, combined with loop closure verification and optimization algorithms, the problem of low accuracy in SLAM loop closure detection was solved, and high-precision loop closure detection was achieved in complex environments.
Patent Information
- Application Number
- CN202511132603.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-08-13
- Publication Date
- 2025-12-16
AI Technical Summary
Existing SLAM technology has low accuracy in loop closure detection in complex environments, and suffers from inaccurate data and cumulative errors.
By acquiring point cloud data and driving data during the robot's movement, keyframe extraction and filtering reconstruction are performed. Loop closure verification is conducted using the bag-of-words model and the FLANN algorithm. Combined with global and local optimization algorithms, the accuracy of loop closure detection is improved.
It effectively solves the problem of accumulated errors in SLAM systems, improves the accuracy and reliability of loop closure detection, and the dynamic triggering mechanism ensures accurate loop closure detection even in complex environments.
Smart Images
Figure CN121147735A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of SLAM detection technology, and in particular to a SLAM loop closure detection method and apparatus. Background Technology
[0002] With the development of artificial intelligence and intelligent equipment technology, the demand for high-precision maps is growing stronger. Accurate positioning and map building are crucial foundations in fields such as robot navigation, autonomous driving, and indoor and outdoor surveying. However, due to limitations such as complex environments (e.g., underground parking lots, roads under bridges, indoor corridors), equipment costs, and electromagnetic interference, traditional GPS positioning and differential positioning technologies cannot be effectively applied in these special environments. Simultaneous Localization and Mapping (SLAM) technology, capable of autonomous positioning and map building using sensor data in unknown environments, has become a core technology for solving the positioning problems in these scenarios. However, SLAM technology faces the problem of accumulated errors in practical applications: as the robot's travel distance increases, positioning errors based on odometry and inertial measurement units gradually accumulate, leading to map distortion and positioning drift.
[0003] In existing technologies, loop closure detection provides global constraints for SLAM technology, with the aim of identifying whether the robot has returned to a previously visited position (i.e., "loop closure"). However, certain factors in current SLAM applications can cause the collected data to be inaccurate, and cumulative errors can occur during the loop closure process, resulting in low accuracy of loop closure detection.
[0004] Therefore, there is an urgent need to propose a SLAM loop closure detection method and device to solve the technical problems in the current SLAM application where certain factors can cause the collected data to be inaccurate, and the cumulative error during the loop closure process can lead to low accuracy in loop closure detection. Summary of the Invention
[0005] In view of this, it is necessary to provide a SLAM loop closure detection method and apparatus to solve the technical problems in the current SLAM application where certain factors cause the collected data to be inaccurate, and during the loop closure process, there will be cumulative errors, resulting in low loop closure detection accuracy.
[0006] To address the aforementioned problems, in a first aspect, the present invention provides a SLAM loop closure detection method, comprising: Acquire point cloud data and driving data detected by the robot during its operation; When the driving data and the point cloud data meet the preset triggering conditions, key frame extraction and filtering reconstruction are performed on the point cloud data to obtain a set of key frames; Based on the current keyframe in the keyframe set, the loopback target frame and loopback source frame are identified, and loopback verification is performed on the loopback target frame and the loopback source frame to obtain the verification result; the verification result includes valid loopback and invalid loopback; When the verification result is a valid loop closure, the key frames in the key frame set are globally optimized according to the preset loop closure constraints to obtain the first SLAM loop closure result. When the verification result is an invalid loop closure, the current key frame and adjacent key frames in the key frame set are locally optimized according to the preset loop closure constraint to obtain the second SLAM loop closure result.
[0007] In one possible implementation, the step of cropping and correcting the point cloud data to obtain the point cloud data includes: The point cloud data is filtered based on voxel grid filtering to obtain filtered point cloud data; The filtered point cloud data is denoised based on statistical outlier filtering to obtain denoised point cloud data. The RANSAC algorithm is used to fit the ground plane in the denoised point cloud data to obtain ground points, and the ground points are removed from the denoised point cloud data to obtain cropped point cloud data. Spatial geometric correction and region of interest extraction are performed on the cropped point cloud data to obtain updated point cloud data.
[0008] In one possible implementation, the step of performing spatial geometric correction and region of interest extraction on the cropped point cloud data to obtain updated point cloud data includes: The cropped point cloud data is timestamped to obtain aligned point cloud data; Motion compensation is performed on the aligned point cloud data based on linear interpolation to obtain compensated point cloud data. The local coordinates of the compensated point cloud data are unified based on the point cloud registration algorithm to obtain the corrected point cloud data. Based on the robot's motion direction and the sensor's field of view, the region of interest is extracted from the corrected point cloud data to obtain updated point cloud data.
[0009] In one possible implementation, the driving data includes travel distance and rotation angle; when the driving data and the point cloud data meet a preset trigger condition, keyframe extraction and filtering reconstruction are performed on the point cloud data to obtain a keyframe set, including: Based on the point cloud data, the amount of change in environmental features was confirmed; When the motion distance, the rotation angle, and the change in environmental features all meet the preset triggering conditions, keyframes are extracted from the point cloud data to obtain keyframes. When the keyframe is a visual SLAM, the pose of the keyframe is optimized based on the homography matrix and the essential matrix, and mismatches are eliminated by a model fitting algorithm to obtain the first keyframe. When the key frame is laser SLAM, the pose reliability of the key frame is evaluated based on the point cloud registration error, and optimized by local BA to obtain the second key frame. The first keyframe or the second keyframe is filtered according to the preset keyframe quality evaluation index to obtain the third keyframe. The third keyframe is added to the keyframe set to obtain the updated keyframe set.
[0010] In one possible implementation, the step of identifying the loopback target frame and the loopback source frame based on the current key frame in the keyframe set, and performing loopback verification on the loopback target frame and the loopback source frame to obtain the verification result includes: Based on the bag-of-words model, the key frames in the key frame set that meet the first preset condition are matched to obtain multiple first key frames, and the multiple first key frames are merged to obtain the loop-loop target frame. Based on the bag-of-words model, the keyframes in the keyframe set that meet the second preset condition with the current keyframe are matched to obtain multiple second keyframes, and the multiple second keyframes are merged to obtain the loopback source frame. Based on the FLANN algorithm, feature point matching is performed between the loop closure target frame and the loop closure source frame to obtain multiple pairs of feature points; The mean square error is obtained by filtering and calculating the multiple pairs of feature points based on the target loopback frame and the source loopback frame. When the mean square error is less than a preset threshold and the inspection time interval between the loop closure target frame and the loop closure source frame is greater than a preset time, the similarity between the visual features and laser point cloud features of the loop closure source frame and the loop closure target frame is calculated to obtain the loop closure confidence. When the confidence level of the loop closure is greater than the preset confidence threshold, the verification result is confirmed as a valid loop closure. When the confidence level of the loop closure is not greater than the preset confidence threshold, the verification result is confirmed as an invalid loop closure.
[0011] In one possible implementation, the step of filtering and calculating the multiple pairs of feature points based on the loopback target frame and the loopback source frame to obtain the mean square error includes: The pose transformation between the loop-loop target frame and the loop-loop source frame is calculated based on the PnP algorithm to obtain the transformation result; The three-dimensional reprojection error is calculated based on the transformation result to obtain the reprojection error; Based on the reprojection error, the multiple pairs of feature points are filtered to obtain multiple pairs of valid matching points; The mean square error is calculated for the multiple pairs of valid matching points.
[0012] In one possible implementation, the step of performing global optimization on keyframes in the keyframe set according to preset loop closure constraints to obtain a first SLAM loop closure result includes: When the verification result is a valid loop closure, a keyframe pose graph is constructed using the keyframe set. Using the preset loop closure constraint as the global edge, the key frames in the key frame pose graph are subjected to global nonlinear optimization through a preset nonlinear optimization library to obtain the first SLAM loop closure result.
[0013] In one possible implementation, the step of performing local optimization on the current keyframe and adjacent keyframes in the keyframe set according to the preset loop closure constraint to obtain the second SLAM loop closure result further includes: When the verification result is an invalid loop, the pose error is calculated for the current keyframe and the adjacent keyframes according to the preset loop constraint. The pose error is optimized based on local BA optimization to obtain the second SLAM loop closure result.
[0014] In one possible implementation, the step of extracting and filtering keyframes from the point cloud data and reconstructing the keyframe set when the driving data and the point cloud data meet a preset trigger condition further includes: When the key frame is not extracted from the point cloud data, virtual key frame reconstruction is performed based on the pose interpolation of adjacent key frames and historical point cloud map to obtain the fourth key frame. The fourth keyframe is added to the keyframe set to obtain the updated keyframe set.
[0015] Secondly, the present invention also provides a SLAM loop closure detection device, comprising: The point cloud acquisition module is used to acquire point cloud data and driving data detected by the robot during its movement. The keyframe extraction module is used to extract and filter the keyframes of the point cloud data when the driving data and the point cloud data meet the preset trigger conditions, so as to obtain a keyframe set. The loopback verification module is used to identify the loopback target frame and the loopback source frame based on the current key frame in the keyframe set, and to perform loopback verification on the loopback target frame and the loopback source frame to obtain the verification result; the verification result includes valid loopback and invalid loopback; A global optimization module is used to perform global optimization on key frames in the key frame set according to preset loop closure constraints when the verification result is the valid loop closure, so as to obtain the first SLAM loop closure result. The local optimization module is used to perform local optimization on the current key frame and adjacent key frames in the key frame set according to the preset loop closure constraint when the verification result is the invalid loop closure, so as to obtain the second SLAM loop closure result.
[0016] The beneficial effects of this invention are: acquiring point cloud data and driving data detected by the robot during its movement; when the driving data and point cloud data meet preset trigger conditions, keyframe extraction and filtering reconstruction are performed on the point cloud data to obtain a keyframe set, thereby enabling filtering and reconstruction of the extracted keyframes and improving the accuracy of the collected data; furthermore, based on the current keyframe in the keyframe set, the loop closure target frame and loop closure source frame are identified, and loop closure verification is performed on the loop closure target frame and the loop closure source frame to obtain verification results; the verification results include valid loop closures and invalid loop closures; when the verification result is a valid loop closure, global optimization is performed on the keyframes in the keyframe set according to preset loop closure constraints to obtain the first SLAM loop closure result. When the verification result is an invalid loop closure, the current keyframe and adjacent keyframes in the keyframe set are locally optimized according to the preset loop closure constraints to obtain the second SLAM loop closure result. When a loop closure is detected through a dynamic triggering mechanism, verification optimization is performed using the determined loop closure target frame and loop closure source frame. If it is a valid loop closure, it means that the robot has returned to the starting area, and the keyframe set can be globally optimized to effectively solve the problem of odometry cumulative error. If it is an invalid loop closure, it means that there may be a mismatch problem caused by environmental interference, and the current keyframe and adjacent keyframes can be locally optimized. Then the robot continues the subsequent driving process, thereby improving the accuracy and reliability of loop closure detection. Attached Figure Description
[0017] Figure 1 This is a schematic flowchart of an embodiment of the SLAM loop closure detection method provided by the present invention; Figure 2 For the present invention Figure 1 A schematic diagram of an embodiment following step S101; Figure 3 For the present invention Figure 1 A schematic diagram of an embodiment of step S102; Figure 4 For the present invention Figure 1 A schematic flowchart of an embodiment of step S103; Figure 5 This is a schematic diagram of an embodiment of the SLAM loop closure detection device provided by the present invention. Detailed Implementation
[0018] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.
[0019] like Figure 1 As shown, a specific embodiment of the present invention discloses a SLAM loop closure detection method, comprising: S101. Acquire point cloud data and driving data detected by the robot during its driving process.
[0020] The SLAM loop closure detection method provided in this application can be applied to a SLAM loop closure detection system. The SLAM loop closure detection system can be a software system running on a terminal device. The terminal device can be a server, tablet computer, augmented reality (AR) / virtual reality (VR) device, laptop computer, ultra-mobile personal computer (UMPC), netbook, personal digital assistant (PDA), mobile phone, etc. This application does not impose any restrictions on the specific type of terminal device.
[0021] Point cloud data can be data detected by multiple sensors (vision + LiDAR).
[0022] To illustrate, taking LiDAR SLAM as an example, when the robot enters the underground parking lot, the LiDAR collects point cloud data at a frequency of 10Hz, and the IMU simultaneously outputs angular velocity and acceleration (i.e., driving data); then the point cloud data can be cropped and corrected to obtain the final point cloud data.
[0023] S102. When the driving data and point cloud data meet the preset trigger conditions, the point cloud data is extracted and filtered to reconstruct key frames to obtain a set of key frames.
[0024] Among them, keyframe extraction and filtering reconstruction refers to selecting representative and high-confidence frames from the target point cloud. For example, the pose of visual keyframes can be optimized by homography matrix, or the reliability of laser keyframes can be evaluated based on point cloud registration error, and then low-quality frames can be filtered out by quality evaluation indicators.
[0025] Specifically, after collecting point cloud data and driving data of the robot during its current driving process, the driving data can be judged to determine whether it meets preset trigger conditions. The preset trigger conditions can be set according to actual conditions, and this embodiment of the invention does not impose any limitations on them. When the driving data and point cloud data meet the preset trigger conditions, keyframe extraction and filtering reconstruction can be performed on the point cloud data to obtain keyframes of the robot during its current driving process. These keyframes can then be added to the keyframe set until overlapping keyframes are found.
[0026] S103. Based on the current keyframe in the keyframe set, identify the loopback target frame and the loopback source frame, and perform loopback verification on the loopback target frame and the loopback source frame to obtain the verification results; the verification results include valid loopback and invalid loopback.
[0027] In SLAM (Simultaneous Localization and Mapping) systems, the Loop Closure Target Frame and Loop Closure Source Frame are crucial concepts, primarily used for detecting and handling loop closures. Loop closure verification involves confirming the correlation between the current frame and historical frames through feature matching and geometric consistency checks. For example, a bag-of-words model can be used for initial matching of candidate frames, followed by precise feature point matching using the FLANN algorithm, and finally, reprojection error analysis can be used to filter out valid matching pairs.
[0028] Specifically, when the current keyframe overlaps with or duplicates the keyframes previously detected by the robot, the loop closure target frame can be determined by the keyframe that overlaps with the current keyframe, and the loop closure source frame can be determined by the current keyframe. Then, the loop closure target frame and the loop closure source frame are used to perform loop closure verification, thereby obtaining the verification result. The verification result can include valid loop closure and invalid loop closure.
[0029] S104. When the verification result is a valid loop closure, the key frames in the key frame set are globally optimized according to the preset loop closure constraints to obtain the first SLAM loop closure result.
[0030] Specifically, different optimizations can be performed depending on the situation. When the verification result is a valid loop closure, global optimization, or pose optimization, can be performed on all keyframes in the keyframe set. The pose graph of the keyframes can be adjusted using loop closure constraints. For example, valid loop closures can be used as global edge inputs to a nonlinear optimization library for graph optimization to correct accumulated errors and obtain the first SLAM loop closure result.
[0031] S105. When the verification result is an invalid loop closure, perform local optimization on the current key frame and adjacent key frames in the key frame set according to the preset loop closure constraints to obtain the second SLAM loop closure result.
[0032] When the verification result is an invalid loop closure, the current keyframe and adjacent keyframes can be locally optimized according to the preset loop closure constraints to obtain the second SLAM loop closure result.
[0033] Specifically, during robot movement, LiDAR or depth cameras continuously acquire raw point cloud data. Voxel filtering and statistical outlier removal reduce data volume and noise. Subsequently, a plane fitting algorithm separates ground points from non-ground points, preserving effective structural information such as walls and obstacles. After spatial geometric correction and motion compensation, the region of interest (ROI) point cloud along the robot's movement direction is extracted as target data. When the robot's movement distance reaches a set threshold, the rotation angle exceeds a critical value, or environmental features change significantly, the system triggers a keyframe extraction process. For visual SLAM keyframes, pose is optimized using the essential matrix and mismatches are eliminated; for LiDAR SLAM keyframes, pose reliability is assessed based on point cloud registration errors, and accuracy is improved through local optimization. Filtered keyframes are added to a set for subsequent loop closure detection. In the loop closure verification stage, similar keyframes are retrieved using a bag-of-words model to confirm the target and source frames for loop closure. ORB features are extracted for rapid matching, the PnP algorithm is used to calculate pose transformation and verify reprojection errors, and the validity of the loop closure is determined by combining time intervals and confidence levels. Different optimizations are performed based on the loop closure verification results. For valid loop closures, global optimization is performed, and for invalid loop closures, local optimization is performed. The corrected SLAM results are then output.
[0034] Compared with existing technologies, this embodiment provides the following: It acquires point cloud data and driving data detected by the robot during its movement; when the driving data and point cloud data meet preset trigger conditions, it extracts and filters keyframes from the point cloud data to obtain a keyframe set, thereby improving the accuracy of the collected data by filtering and reconstructing the extracted keyframes; furthermore, based on the current keyframe in the keyframe set, it identifies the loop closure target frame and the loop closure source frame, and performs loop closure verification on the target frame and the source frame to obtain verification results; the verification results include valid loop closures and invalid loop closures; when the verification result is a valid loop closure, it performs global optimization on the keyframes in the keyframe set according to preset loop closure constraints to obtain the first SLAM loop closure. Results: When the verification result is an invalid loop closure, local optimization is performed on the current keyframe and adjacent keyframes in the keyframe set according to the preset loop closure constraints to obtain the second SLAM loop closure result. Through the dynamic triggering mechanism, when a loop closure is detected, verification optimization is performed using the determined loop closure target frame and loop closure source frame. If it is a valid loop closure, it means that the robot has returned to the starting area, and global optimization can be performed on the keyframe set to effectively solve the problem of odometry cumulative error. If it is an invalid loop closure, it means that there may be a mismatch problem caused by environmental interference, and local optimization can be performed on the current keyframe and adjacent keyframes. Then the robot continues the subsequent driving process, thereby improving the accuracy and reliability of loop closure detection.
[0035] In some embodiments of the present invention, such as Figure 2 As shown, after step S101, the following steps are also included: S201. Filter the point cloud data based on voxel grid filtering to obtain filtered point cloud data.
[0036] Voxel grid filtering refers to dividing a 3D space into equal-volume cubic units and downsampling the point cloud within each unit. This can be implemented using the VoxelGrid class in the PCL library, reducing point cloud density and computational cost. For example, voxel grid filtering can reduce the point cloud density from 2500 points / m² to 1000 points / m².
[0037] S202. Denoise the filtered point cloud data based on statistical outlier filtering to obtain denoised point cloud data.
[0038] Statistical outlier removal refers to removing noise points by calculating the statistical characteristics of the distance distribution within the neighborhood of a point cloud. Specifically, it can be implemented using the StatisticalOutlierRemoval class in the PCL library to eliminate noise points. For example, statistical outlier removal can be used to eliminate noise points that are more than three standard deviations away from the target value.
[0039] S203. Fit the ground plane in the denoised point cloud data based on the RANSAC algorithm to obtain ground points, and remove the ground points from the denoised point cloud data to obtain cropped point cloud data.
[0040] The RANSAC algorithm refers to fitting a ground plane model using a random sampling consensus method. Specifically, it can be implemented using the SACSegmentation module in the PCL library to identify and remove ground points to retain effective environmental features. For example, it removes about 90% of ground points, focusing on vertical features such as walls and columns.
[0041] S204. Perform spatial geometric correction and region of interest extraction on the cropped point cloud data to obtain updated point cloud data.
[0042] Spatial geometric correction refers to eliminating distortions in point cloud data caused by robot motion through coordinate transformation. This can be achieved using the ICP or NDT algorithm to unify the local coordinate system of point clouds at different times. Region of interest extraction refers to filtering effective point cloud regions based on the robot's motion direction and the sensor's field of view. This can be achieved by setting spatial bounding boxes or angle thresholds to focus on effective data containing environmental features.
[0043] Specifically, the point cloud data is first downsampled using voxel grid filtering to reduce data volume while preserving environmental structural features. Next, statistical outlier filtering removes isolated points caused by sensor noise, improving point cloud quality. Then, the RANSAC algorithm is used to fit a ground plane model, identifying and removing ground points to avoid interference with subsequent feature matching. After ground point removal, spatial geometric correction is used to eliminate point cloud distortion caused by robot motion; for example, point cloud registration algorithms are used to unify multiple frames of point clouds to the same coordinate system. Finally, regions of interest are extracted based on the robot's motion direction and the sensor's field of view; for example, point cloud data within a fan-shaped area in front of the robot is retained, resulting in updated point cloud data that can be used for loop closure detection.
[0044] Through the above technical solution, this application can effectively remove noise, ground interference and motion distortion in point cloud data, improve the feature quality of point cloud data, thereby providing high-precision input data for subsequent loop closure detection and reducing the probability of mismatch due to data errors.
[0045] In some embodiments of the present invention, step S204 includes: The cropped point cloud data is timestamped to obtain aligned point cloud data; Motion compensation is performed on aligned point cloud data based on linear interpolation to obtain compensated point cloud data. Local coordinate unification is performed on the compensated point cloud data based on the point cloud registration algorithm to obtain the corrected point cloud data. Based on the robot's motion direction and the sensor's field of view, the region of interest is extracted from the corrected point cloud data to obtain updated point cloud data.
[0046] Timestamp alignment refers to synchronizing point cloud data with sensor acquisition time. This can be achieved using timestamp interpolation algorithms or hardware clock synchronization mechanisms to ensure a unified time reference for data acquired by different sensors. Linear interpolation uses pose information from adjacent time points to perform motion compensation on point cloud data. This can be done using uniform motion models or acceleration models to calculate interpolation and eliminate point cloud distortions generated during robot movement. Point cloud registration algorithms are methods to transform point cloud data acquired from different viewpoints to the same coordinate system. This can be done using iterative nearest-point algorithms or normal distribution transformation algorithms, achieving local coordinate system unification through feature matching. Region of interest extraction involves selecting effective regions based on the robot's motion trajectory and sensor field of view. This can be done using geometric boundary constraints or probability density distribution models to retain effective point cloud data related to the navigation path.
[0047] Specifically, to address motion distortion from LiDAR or vision sensors, the timestamp alignment stage utilizes IMU (Inertial Measurement Unit) data for timestamp alignment, eliminating errors caused by sensor data acquisition delays. In the motion compensation stage, linear interpolation is used to compensate for point cloud motion, thus eliminating point cloud stretching and deformation caused by changes in motion speed. In the local coordinate unification stage, a point cloud registration algorithm based on Normal Distribution Transform (NDT) is used to register the motion-compensated point cloud data with historical maps, establishing a unified local coordinate system. Finally, the effective detection range of the sensors is determined based on the robot's current motion direction and the sensor's field of view. Point cloud data is dynamically cropped, focusing on key environmental features in front and to the sides, reducing the processing of invalid data.
[0048] Through the above technical solutions, this application can significantly improve the spatiotemporal consistency of point cloud data and eliminate feature matching errors caused by device clock deviations and motion distortions. By dynamically adjusting the region of interest, it effectively reduces the interference of invalid point cloud data on subsequent keyframe extraction, thereby improving the accuracy and computational efficiency of loop closure detection.
[0049] In some embodiments of the present invention, the driving data includes the distance traveled and the angle of rotation; such as Figure 3 As shown, step S102 includes: S301. Based on the point cloud data, confirm the changes in environmental features.
[0050] Among them, the change in environmental features can be expressed as the change in the number of feature points in the point cloud data compared to the previous moment.
[0051] S302. When the changes in motion distance, rotation angle, and environmental features all meet the preset triggering conditions, key frames are extracted from the point cloud data to obtain key frames.
[0052] Among these, motion distance refers to the linear displacement of the robot, which can be calculated using an odometry or inertial measurement unit, and is used to determine whether the spatial interval conditions for keyframe generation have been met. Rotation angle refers to the angle at which the robot rotates around an axis, which can be measured using a gyroscope or visual odometry, and is used to determine whether the environmental perception data has changed significantly due to turning. Environmental feature change refers to a quantitative indicator of changes in dynamic objects or structures in the scene, which can be calculated using point cloud density differences or feature point matching rates, and is used to assess whether the current environment necessitates keyframe generation.
[0053] Specifically, by combining three thresholds—motion distance (e.g., every 0.5 meters moved), rotation angle (e.g., every 5° rotated), and environmental feature change (e.g., the number of feature points changes by more than 20%)—keyframe creation is adaptively triggered to avoid redundant frame storage.
[0054] S303. When the keyframe is visual SLAM, the pose of the keyframe is optimized based on the homography matrix and the essential matrix, and mismatches are eliminated through the model fitting algorithm to obtain the first keyframe.
[0055] The homography matrix describes the projection transformation relationship between two image planes. It can be solved using feature point matching and the least squares method, and is used to optimize the pose consistency of visual keyframes. The essential matrix describes the epigeometric constraints between two images. It can be calculated using a five-point algorithm and is used to verify the accuracy of the geometric relationships in visual keyframes. The model fitting algorithm refers to a robust parameter estimation method, specifically the RANSAC algorithm, used to remove outlier matching points in visual keyframes.
[0056] Specifically, for visual SLAM, camera pose optimization based on the homography matrix and the essential matrix is used, combined with a model fitting algorithm (RANSAC) to remove mismatched points and obtain the first keyframe.
[0057] S304. When the key frame is laser SLAM, the pose reliability of the key frame is evaluated based on the point cloud registration error, and optimized by local BA to obtain the second key frame.
[0058] Point cloud registration error refers to the residual metric after laser point cloud alignment, which can be calculated iteratively using the ICP algorithm and is used to evaluate the pose reliability of laser keyframes. Local beamforming (BA) refers to local bundle adjustment optimization, which can be implemented using a nonlinear least squares method and is used to optimize the local shape of laser keyframes. Figure 1 To the point of being responsive.
[0059] Specifically, for laser SLAM, the reliability of keyframe pose is evaluated using point cloud registration error, and pose parameters are optimized through local BA (Bundle Adjustment).
[0060] S305. Filter the first keyframe or the second keyframe according to the preset keyframe quality evaluation index to obtain the third keyframe.
[0061] Among them, the preset keyframe quality evaluation index refers to the keyframe screening criteria, which may include the number of feature points (≥200), matching success rate (≥70%), and pose error (≤0.1 m / 1°), and is used to mark or delete low-quality keyframes.
[0062] S306. Add the third keyframe to the keyframe set to obtain the updated keyframe set.
[0063] Specifically, during robot movement, a keyframe extraction mechanism is triggered when the detected movement distance exceeds a set threshold (e.g., 5 meters), the rotation angle exceeds a set angle (e.g., 30 degrees), and the change in environmental features exceeds a set proportion (e.g., 40%). For visual SLAM systems, pose estimation is optimized by calculating the homography matrix between adjacent frames, spatial geometric relationships are verified using the essential matrix, and then the RANSAC algorithm is used to remove mismatched feature point pairs, forming the optimized first keyframe. For laser SLAM systems, the matching degree between the current frame and the local map is evaluated by calculating the point cloud registration error, and the point cloud alignment accuracy is optimized using local BA to generate the second keyframe. Further, keyframes are filtered based on quality indicators such as whether the number of feature points is below a set lower limit (e.g., less than 50 points) or whether the scene information entropy is below a set value (e.g., less than 1.2 bits), retaining the third keyframe with sufficient environmental information, and finally updating the keyframe set.
[0064] Through the above technical solution, this application can dynamically adapt to different sensor types and environmental conditions, and can still generate highly reliable keyframes under changes in illumination or interference from dynamic objects. By employing differentiated optimization algorithms, it effectively reduces visual mismatch rates and laser point cloud registration errors, improving the overall quality of the keyframe set and providing an accurate data foundation for subsequent loop closure detection, thereby mitigating the problem of misjudgment of loop closures caused by sensor noise or environmental interference.
[0065] In some embodiments of the present invention, step S102 further includes: When no keyframe is extracted from the point cloud data, virtual keyframe reconstruction is performed based on the pose interpolation of adjacent keyframes and the historical point cloud map to obtain the fourth keyframe. Add the fourth keyframe to the keyframe set to obtain the updated keyframe set.
[0066] Among these, pose interpolation of adjacent keyframes refers to estimating the robot's pose at the current moment using the pose information of adjacent keyframes through an interpolation algorithm. Specifically, linear interpolation or spline interpolation methods can be used to achieve this, providing continuous motion trajectory estimation when keyframes are missing. Virtual keyframe reconstruction using historical point cloud maps involves combining the environmental features already constructed in the historical point cloud map with point cloud fusion or projection transformation to generate virtual keyframes. This can be achieved using point cloud registration algorithms or 3D reconstruction models, compensating for keyframe extraction failures due to insufficient environmental features.
[0067] Specifically, when point cloud data fails to extract valid keyframes due to dynamic environmental changes, sensor noise, or insufficient features, the robot's pose at the current moment is estimated using an interpolation algorithm based on the timestamps and pose information of adjacent keyframes. For example, if the poses of adjacent keyframes are P1 and P2, with a time interval of Δt, and the current moment is located at a position with a time ratio of α between the two, then the interpolated pose can be expressed as P = αP1 + (1-α)P2. Furthermore, by combining the environmental structure information stored in the historical point cloud map, the sensor observation data at the current moment is registered with the historical map to generate a virtual keyframe (i.e., the fourth keyframe) containing local environmental features. After this virtual keyframe is added to the keyframe set, the continuity of map construction can be maintained, avoiding positioning drift caused by missing keyframes.
[0068] For example, when the robot moves 0.5 meters, rotates 8°, or the surrounding features change by 30%, keyframe extraction is triggered. The FPFH features of the point cloud are extracted, and the feature distance between the current frame and the previous keyframe is calculated. If the change is less than 10%, keyframe creation is abandoned to avoid redundancy. The keyframes are then optimized for pose: using the point cloud registration results of adjacent frames, 6DoF pose parameters are optimized through local BA to ensure translation error < 0.05 meters and rotation error < 0.5°. If feature matching fails for three consecutive frames due to a corridor corner, a keyframe completion mechanism is triggered: based on the poses (x1, y1, θ1) and (x2, y2, θ2) of the previous two frames, virtual keyframe poses ((x1+x2) / 2, (y1+y2) / 2, (θ1+θ2) / 2) are interpolated, and environmental features at the corresponding location are extracted from the historical point cloud map as virtual frame content.
[0069] In some specific implementations, the interpolation algorithm can use quaternion spherical linear interpolation to process the rotation component in order to maintain the smoothness of the attitude interpolation; the point cloud data of the virtual keyframe can be transformed from the three-dimensional point cloud in the historical map to the coordinate system of the current estimated pose through back projection to form virtual observation data.
[0070] Through the above technical solutions, this application can maintain the integrity of the keyframe set in complex environments, effectively suppress the localization drift of the SLAM system caused by the absence of keyframes, and improve the robustness of loop closure detection and the accuracy of map construction.
[0071] In some embodiments of the present invention, such as Figure 4 As shown, step S103 includes: S401. Based on the bag-of-words model, match the key frames in the key frame set that meet the first preset conditions to obtain multiple first key frames, and merge the multiple first key frames to obtain the loop-loop target frame.
[0072] Among them, the bag-of-words model refers to a fast matching method based on visual vocabulary statistics. Specifically, it can use an offline trained visual vocabulary to perform feature clustering and encoding on keyframes, which is used to quickly filter out candidate loop closure frames similar to the current keyframe from a large number of historical keyframes.
[0073] Specifically, an octree spatial index structure is used to predict potential loop closure regions based on the current keyframe pose, thus narrowing the search range; Fast Point Feature Histograms (FPFH) features of the point cloud are extracted to construct a Bag of Words (BoW) model, and similar keyframes are quickly matched using TF-IDF weights.
[0074] Specifically, after matching similar keyframes using the bag-of-words model, keyframes are extracted based on a first preset condition. For example, matching similar keyframes to 8 keyframes less than 10 meters away from the point 30 seconds prior to registration (no more than 10; to avoid increased computation, keyframes farther away are removed and sorted by distance). These are then merged into a combination of loopback target frames. S402. Based on the bag-of-words model, match the key frames in the key frame set that meet the second preset conditions with the current key frame to obtain multiple second key frames, and merge the multiple second key frames to obtain the loopback source frame.
[0075] Specifically, the second preset condition can be keyframes with a duration of no more than 1 minute and a number of no more than 10. Keyframes with a duration of no more than 1 minute and a number of no more than 10 can be found in the keyframe set using the bag-of-words model and then merged as a combination of loopback source frames.
[0076] S403. Based on the FLANN algorithm, feature point matching is performed on the loop closure target frame and the loop closure source frame to obtain multiple pairs of feature points.
[0077] The FLANN algorithm is a feature matching method based on fast approximate nearest neighbor search. Specifically, it can use KD trees or hierarchical clustering structures to accelerate the feature point matching process and establish preliminary correspondences among multiple pairs of feature points.
[0078] Specifically, FPFH features of the point clouds of the loop-loop source frame and the loop-loop target frame are extracted to obtain 82 pairs of feature points.
[0079] S404. Based on the target frame and source frame of the loop closure, multiple pairs of feature points are filtered and calculated to obtain the mean square error.
[0080] The mean square error refers to the matching error calculated through geometric consistency verification. Specifically, it can be obtained by the mean square of the three-dimensional reprojection error and is used to evaluate the reliability of feature point matching.
[0081] S405. When the mean square error is less than the preset threshold and the inspection time interval between the loop closure target frame and the loop closure source frame is greater than the preset time, the similarity between the loop closure source frame and the loop closure target frame is calculated based on the visual features and laser point cloud features of the loop closure source frame to obtain the loop closure confidence.
[0082] The check interval refers to the time difference between the current keyframe and the candidate loop closure frame. It can be set to a fixed duration, such as 2 minutes, to avoid false matches caused by repeatedly detecting the same region within a short period. Loop closure confidence is a quantitative indicator that combines visual and laser feature similarity. Specifically, it can be calculated by weighted fusion of visual feature matching scores and point cloud overlap, used to ultimately determine whether a loop closure is valid. For example, if the mean squared error (MSE) of point cloud registration is 0.03 meters, which is less than the threshold of 0.1 meters, and the check interval is 3 minutes (>2 minutes), it satisfies the time continuity condition.
[0083] S406. When the confidence level of the loop closure is greater than the preset confidence threshold, the verification result is confirmed as a valid loop closure.
[0084] For example, if the confidence level of a loop closure is calculated to be 0.90, which is greater than the preset confidence threshold of 0.8, it is determined to be a valid loop closure, thus improving the anti-interference capability.
[0085] S407. When the confidence level of the loop closure is not greater than the preset confidence threshold, the verification result is confirmed as an invalid loop closure.
[0086] Specifically, in the loop closure verification process, candidate loop closure frames are first quickly matched using a bag-of-words model to reduce computation. Then, ORB features of the current keyframe and candidate frames are extracted, and the FLANN algorithm is used for coarse feature point matching to generate multiple pairs of feature points. Next, geometric verification is used to filter valid matching points, and the mean squared error is calculated to eliminate obvious mismatches. When the mean squared error meets preset conditions and the time interval is long enough, visual features and laser point cloud features are further fused to calculate similarity and generate loop closure confidence. If the confidence exceeds a threshold, it is determined to be a valid loop closure; otherwise, it is marked as an invalid loop closure. For example, when calculating the loop closure confidence, the visual feature matching score can be calculated using the similarity weights of the bag-of-words model, while the point cloud overlap can be determined by the proportion of overlapping regions after registration using the ICP algorithm.
[0087] Through the above technical solutions, this application can significantly improve the accuracy and robustness of loop closure detection, avoid misjudgments caused by changes in lighting, dynamic objects or sensor noise, and reduce redundant calculations by time interval constraints, thereby improving system operating efficiency.
[0088] In some embodiments of the present invention, step S404 includes: The pose transformation between the loop-closing target frame and the loop-closing source frame is calculated based on the PnP algorithm to obtain the transformation result.
[0089] Among them, the PnP algorithm refers to the algorithm that solves the camera pose by the correspondence between three-dimensional spatial points and two-dimensional image points. Specifically, it can be implemented by EPnP or UPnP algorithms, and is used to calculate the relative pose transformation between the current keyframe and the candidate loop closure frame.
[0090] The reprojection error is calculated based on the transformation results.
[0091] Among them, the 3D reprojection error refers to the positional deviation between the 3D point projected onto the 2D image plane and the observed 2D point. Specifically, it can be achieved by calculating the sum of squared Euclidean distances, which is used to evaluate the geometric consistency of feature point matching.
[0092] Multiple pairs of feature points are filtered based on the reprojection error to obtain multiple pairs of valid matching points.
[0093] Among them, a valid matching point refers to a pair of feature points that meet the geometric constraints after being filtered by reprojection error. Specifically, false matching points can be eliminated by setting an error threshold. For example, points with a reprojection error greater than 3 pixels are considered invalid matches.
[0094] The mean square error is calculated by performing calculations on multiple pairs of valid matching points.
[0095] In this embodiment of the invention, the pose transformation between matching frames is calculated using the Perspective-n-Point (PnP) algorithm, the consistency of the 3D coordinates of feature points is verified using triangulation, and the point cloud registration error is calculated using the Iterative Closest Point (ICP) algorithm. Specifically, firstly, the pose transformation matrix between the current keyframe and the candidate loopback frames is solved using the PnP algorithm. This matrix includes rotation and translation parameters. Then, the 3D feature points in the candidate loopback frames are projected onto the 2D image plane of the current keyframe according to the transformation matrix, and the Euclidean distance between the projected points and the actual observation points is calculated as the reprojection error. Further, feature point pairs are filtered by setting an error threshold (e.g., 2.5 pixels), and matching points with errors less than the threshold are retained as valid matching points. Finally, the reprojection errors of all valid matching points are squared and averaged to obtain the mean square error, which serves as the matching quality evaluation index.
[0096] Through the above technical solutions, this application can significantly improve the accuracy of loop closure detection, for example, reducing the false match rate by about 40% in dynamic environments. Simultaneously, geometric consistency verification enhances the system's robustness to sensor noise. This solution effectively solves the problem of accumulated errors due to environmental interference, providing a reliable data foundation for subsequent pose optimization, thereby improving the overall localization and mapping accuracy of the SLAM system.
[0097] In some embodiments of the present invention, step S104 includes: Construct a keyframe pose graph using a set of keyframes; Using preset loop closure constraints as global edges, global nonlinear optimization is performed on the keyframes in the keyframe pose graph through a preset nonlinear optimization library to obtain the first SLAM loop closure result.
[0098] The keyframe pose graph refers to the graph structure composed of keyframes and their relative pose relationships. Specifically, it can be implemented using nodes to represent keyframe poses and edges to represent the constraints between poses. In the scheme, it is used to establish the topology required for global optimization. Global edges refer to pose constraints confirmed by loop closure detection across time or space. Specifically, they can be represented using edge types in graph optimization. In the scheme, they provide strong cross-regional constraints for global optimization. The pre-defined nonlinear optimization library refers to the mathematical tools used to solve nonlinear least squares problems. Specifically, it can be implemented using open-source libraries such as g2o or Ceres Solver. In the scheme, it is used to achieve optimal pose solutions under multiple constraints.
[0099] Specifically, when loop closure verification confirms a valid correlation between the current keyframe and historical keyframes, the system models the poses and constraints of all keyframes as a graph structure, namely the keyframe pose graph. Each keyframe is treated as a node in the graph, its odometry constraints with adjacent keyframes are treated as local edges, and loop closure constraints are treated as global edges connecting discontinuous nodes. A pre-defined nonlinear optimization library simultaneously adjusts the pose parameters of all keyframes by minimizing the sum of squared residuals of all edges. For example, in a scenario containing three loop closure constraints, the optimization process will simultaneously correct the poses of all keyframes in these three loop closure regions and their associated paths, ensuring geometric consistency between the local trajectory and the global map. For example, add loop closure constraint edges with ID=100 and ID=15 to the Pose Graph. The default equation for the loop closure constraint is: T_100 = T_15 × T_corr, where T_corr is the detected pose transformation. Use the g2o optimization library for global optimization, with the pose of all keyframes as the optimization variable and the optimization objective being to minimize the sum of squared errors of all edges. After optimization, the cumulative translation error of the global map is reduced from 0.25 meters to 0.1 meters, and the rotation error is reduced from 3° to 0.8°. Obtain the SLAM loop closure results, feed the optimized pose back to the SLAM system, update the global map, and complete the error correction.
[0100] Through the above technical solution, this application can effectively eliminate the cumulative error caused by sensor noise or dynamic environment, and improve the performance of sensors in large-scale scenarios. Figure 1 Consistency. In complex environments with multiple effective closure constraints, the global optimization process can avoid suboptimal solutions caused by local optimization, ensuring high-precision matching between the robot trajectory and the real physical space.
[0101] In some embodiments of the present invention, step S105 includes: The pose error is calculated by performing calculations on the current keyframe and adjacent keyframes based on preset closure constraints. The pose error is optimized based on local BA optimization to obtain the second SLAM loop closure result.
[0102] Invalid loop closures refer to verification results where the loop closure confidence level fails to reach a pre-set confidence threshold. This can be determined through visual feature similarity calculations and laser point cloud matching evaluations, used to eliminate erroneous loop closure assumptions caused by mismatches or dynamic environmental interference. Pose error refers to the pose estimation deviation between the current keyframe and adjacent keyframes. This can be calculated using odometry integration error or sensor measurement noise models, used to quantify the correction amount for local optimization. Local BA optimization refers to a bundle adjustment algorithm based on a subset of local keyframes. Specifically, a sliding window mechanism can be used to jointly optimize the pose and 3D points of selected keyframes, used to suppress error accumulation in the case of invalid loop closures.
[0103] Specifically, when the loop closure verification module determines that the similarity between the current keyframe and the candidate loop closure frames is insufficient, the system inputs the pose data of the current keyframe and its adjacent keyframes into the pose error calculation unit. First, based on the covariance matrix in the preset loop closure constraints, error modeling is performed on the relative pose of the current keyframe and its adjacent keyframes. For example, the pose transformation relationship is represented using Lie algebras, and its residual terms are calculated. Then, a local BA optimization algorithm is used to iteratively optimize the pose parameters of the selected keyframe and the associated 3D point coordinates within a sliding window. For example, the Levenberg-Marquardt algorithm is used to minimize the weighted sum of squares of the reprojection error and the odometry constraint. During this process, the preset loop closure constraints serve as optimization boundary conditions, limiting the optimization direction to avoid erroneous corrections introduced by invalid loops.
[0104] Through the above technical solution, this application can effectively suppress the linear growth of positioning error in scenarios where loop closure detection fails. For example, when sudden changes in illumination or interference from temporary obstacles lead to insufficient loop closure confidence, the positioning error can be maintained through local optimization. Figure 1 Consistency. At the same time, this method avoids global map distortion caused by forcibly adding incorrect closure constraints, improving the system's robustness in complex environments.
[0105] This invention provides a closed-loop error control system by tightly integrating data flow and control flow. Through continuous data processing, feature extraction, loop closure detection, and error correction, the system can maintain high-precision positioning and map building in complex environments.
[0106] To better implement the SLAM loop closure detection method in the embodiments of the present invention, the embodiments of the present invention also provide a SLAM loop closure detection device, such as... Figure 5 As shown, the SLAM loop closure detection device 500 includes: The point cloud acquisition module 501 is used to acquire point cloud data and driving data detected by the robot during its driving process; The keyframe extraction module 502 is used to extract and filter keyframes from the point cloud data when the driving data and point cloud data meet the preset trigger conditions, so as to obtain a set of keyframes. The loop closure verification module 503 is used to identify the loop closure target frame and the loop closure source frame based on the current key frame in the key frame set, and to perform loop closure verification on the loop closure target frame and the loop closure source frame to obtain the verification result; the verification result includes valid loop closure and invalid loop closure; The global optimization module 504 is used to perform global optimization on the key frames in the key frame set according to the preset loop closure constraints when the verification result is a valid loop closure, so as to obtain the first SLAM loop closure result. The local optimization module 505 is used to perform local optimization on the current key frame and adjacent key frames in the key frame set according to the preset loop closure constraints when the verification result is an invalid loop closure, so as to obtain the second SLAM loop closure result.
[0107] The SLAM loop closure detection device 500 provided in the above embodiments can realize the technical solutions described in the above SLAM loop closure detection method embodiments. The specific implementation principles of each module or unit can be found in the corresponding content in the above SLAM loop closure detection method embodiments, and will not be repeated here.
[0108] The SLAM loop closure detection method and apparatus provided by the present invention have been described in detail above. Specific examples have been used to illustrate the principle and implementation of the present invention. The description of the above embodiments is only for the purpose of helping to understand the method and core idea of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation and application scope based on the idea of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.
Claims
1. A SLAM loop closure detection method, characterized in that, include: Acquire point cloud data and driving data detected by the robot during its movement; When the driving data and the point cloud data meet the preset triggering conditions, key frame extraction and filtering reconstruction are performed on the point cloud data to obtain a set of key frames; Based on the current keyframe in the keyframe set, the loopback target frame and loopback source frame are identified, and loopback verification is performed on the loopback target frame and the loopback source frame to obtain the verification result; the verification result includes valid loopback and invalid loopback; When the verification result is a valid loop closure, the key frames in the key frame set are globally optimized according to the preset loop closure constraints to obtain the first SLAM loop closure result. When the verification result is an invalid loop closure, the current key frame and adjacent key frames in the key frame set are locally optimized according to the preset loop closure constraint to obtain the second SLAM loop closure result.
2. The SLAM loop closure detection method according to claim 1, characterized in that, After cropping and correcting the point cloud data to obtain the point cloud data, the process further includes: The point cloud data is filtered based on voxel grid filtering to obtain filtered point cloud data; The filtered point cloud data is denoised based on statistical outlier filtering to obtain denoised point cloud data. The RANSAC algorithm is used to fit the ground plane in the denoised point cloud data to obtain ground points, and the ground points are removed from the denoised point cloud data to obtain cropped point cloud data. Spatial geometric correction and region of interest extraction are performed on the cropped point cloud data to obtain updated point cloud data.
3. The SLAM loop closure detection method according to claim 2, characterized in that, The step of performing spatial geometric correction and region of interest extraction on the cropped point cloud data to obtain updated point cloud data includes: The cropped point cloud data is timestamped to obtain aligned point cloud data; Motion compensation is performed on the aligned point cloud data based on linear interpolation to obtain compensated point cloud data. The local coordinates of the compensated point cloud data are unified based on the point cloud registration algorithm to obtain the corrected point cloud data. Based on the robot's motion direction and the sensor's field of view, the region of interest is extracted from the corrected point cloud data to obtain updated point cloud data.
4. The SLAM loop closure detection method according to claim 1, characterized in that, The driving data includes travel distance and rotation angle; when the driving data and the point cloud data meet preset trigger conditions, keyframe extraction and filtering reconstruction are performed on the point cloud data to obtain a keyframe set, including: Based on the point cloud data, the amount of change in environmental features was confirmed; When the motion distance, the rotation angle, and the change in environmental features all meet the preset triggering conditions, keyframes are extracted from the point cloud data to obtain keyframes. When the keyframe is a visual SLAM, the pose of the keyframe is optimized based on the homography matrix and the essential matrix, and mismatches are eliminated by a model fitting algorithm to obtain the first keyframe. When the key frame is laser SLAM, the pose reliability of the key frame is evaluated based on the point cloud registration error, and optimized by local BA to obtain the second key frame. The first keyframe or the second keyframe is filtered according to the preset keyframe quality evaluation index to obtain the third keyframe; The third keyframe is added to the keyframe set to obtain the updated keyframe set.
5. The SLAM loop closure detection method according to claim 1, characterized in that, The step of identifying the loopback target frame and loopback source frame based on the current key frame in the key frame set, and performing loopback verification on the loopback target frame and the loopback source frame to obtain the verification result includes: Based on the bag-of-words model, the key frames in the key frame set that meet the first preset condition are matched to obtain multiple first key frames, and the multiple first key frames are merged to obtain the loop-loop target frame. Based on the bag-of-words model, the keyframes in the keyframe set that meet the second preset condition with the current keyframe are matched to obtain multiple second keyframes, and the multiple second keyframes are merged to obtain the loopback source frame. Based on the FLANN algorithm, feature point matching is performed between the loop closure target frame and the loop closure source frame to obtain multiple pairs of feature points; The mean square error is obtained by filtering and calculating the multiple pairs of feature points based on the target loopback frame and the source loopback frame. When the mean square error is less than a preset threshold and the inspection time interval between the loop closure target frame and the loop closure source frame is greater than a preset time, the similarity between the visual features and laser point cloud features of the loop closure source frame and the loop closure target frame is calculated to obtain the loop closure confidence. When the confidence level of the loop closure is greater than the preset confidence threshold, the verification result is confirmed as a valid loop closure. When the confidence level of the loop closure is not greater than the preset confidence threshold, the verification result is confirmed as an invalid loop closure.
6. The SLAM loop closure detection method according to claim 5, characterized in that, The step of filtering and calculating the mean square error based on the target loopback frame and the source loopback frame to obtain the mean square error includes: The pose transformation between the loop-loop target frame and the loop-loop source frame is calculated based on the PnP algorithm to obtain the transformation result; The three-dimensional reprojection error is calculated based on the transformation result to obtain the reprojection error; Based on the reprojection error, the multiple pairs of feature points are filtered to obtain multiple pairs of valid matching points; The mean square error is calculated for the multiple pairs of valid matching points.
7. The SLAM loop closure detection method according to claim 1, characterized in that, The step of performing global optimization on keyframes in the keyframe set according to preset loop closure constraints to obtain the first SLAM loop closure result includes: Construct a keyframe pose graph using the set of keyframes; Using the preset loop closure constraint as the global edge, the key frames in the key frame pose graph are subjected to global nonlinear optimization through a preset nonlinear optimization library to obtain the first SLAM loop closure result.
8. The SLAM loop closure detection method according to claim 1, characterized in that, The step of performing local optimization on the current keyframe and adjacent keyframes in the keyframe set according to the preset loop closure constraint to obtain the second SLAM loop closure result further includes: The pose error is calculated based on the preset loop closure constraint for the current keyframe and the adjacent keyframes. The pose error is optimized based on local BA optimization to obtain the second SLAM loop closure result.
9. The SLAM loop closure detection method according to claim 4, characterized in that, When the driving data and the point cloud data meet the preset trigger conditions, the point cloud data is subjected to keyframe extraction and filtering reconstruction to obtain a keyframe set, which further includes: When the key frame is not extracted from the point cloud data, virtual key frame reconstruction is performed based on the pose interpolation of adjacent key frames and historical point cloud map to obtain the fourth key frame. The fourth keyframe is added to the keyframe set to obtain the updated keyframe set.
10. A SLAM loop closure detection device, characterized in that, include: The point cloud acquisition module is used to acquire point cloud data and driving data detected by the robot during its movement. The keyframe extraction module is used to extract and filter the keyframes of the point cloud data when the driving data and the point cloud data meet the preset trigger conditions, so as to obtain a keyframe set. The loopback verification module is used to identify the loopback target frame and the loopback source frame based on the current key frame in the keyframe set, and to perform loopback verification on the loopback target frame and the loopback source frame to obtain the verification result; the verification result includes valid loopback and invalid loopback; A global optimization module is used to perform global optimization on key frames in the key frame set according to preset loop closure constraints when the verification result is the valid loop closure, so as to obtain the first SLAM loop closure result. The local optimization module is used to perform local optimization on the current key frame and adjacent key frames in the key frame set according to the preset loop closure constraint when the verification result is the invalid loop closure, so as to obtain the second SLAM loop closure result.