Loopback detection method, device and equipment and readable storage medium
By filtering key point clouds of key frames and using 3D ray tracing to identify spatial visibility relationships, the problem of low accuracy in loop closure detection in special scenarios is solved, achieving more efficient and accurate loop closure detection.
Patent Information
- Application Number
- CN202511059103.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-30
- Publication Date
- 2025-10-31
AI Technical Summary
Existing loop closure detection technologies have low accuracy in special scenarios such as adjacent or reverse tunnels, and are prone to generating incorrect loop closure matching pairs.
By filtering the key point clouds of key frames, the spatial visibility relationship between the current frame and key frames is identified using 3D ray tracing. The existence of loop closure pairs is determined by combining the number of spatial points and the visibility relationship.
It improves the accuracy and efficiency of loop closure detection, avoids including similar environmental scenarios in loop closures, and ensures the reliability of detection.
Smart Images
Figure CN120876592A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of three-dimensional positioning technology, and more specifically, to a loop closure detection method, apparatus, device, and readable storage medium. Background Technology
[0002] Laser or visual odometry inevitably introduces errors during operation. As time progresses or mileage increases, these errors accumulate, potentially leading to ghosting of point clouds or even significant positional shifts when a vehicle returns to a historical scene. Loop closure detection technology effectively eliminates these accumulated errors by matching the current scene with historical scenes and establishing constraints between poses.
[0003] Existing laser-based loop closure detection methods typically characterize the scene by extracting environmental features and generating descriptors, then detect loop closures by comparing these descriptors. However, in special scenarios such as adjacent tunnels or tunnels with opposing traffic, the high degree of overlap in the laser-acquired data features due to the highly similar environments leads to extremely similar descriptors, which can easily result in erroneous loop closure matches and affect detection accuracy. Summary of the Invention
[0004] In view of this, this application provides a loop closure detection method, apparatus, device, and readable storage medium to address the shortcomings of existing loop closure detection technologies in terms of low detection accuracy in special scenarios.
[0005] To achieve the above objectives, the following solution is proposed:
[0006] A loop closure detection method, comprising:
[0007] Get the current pose of the current frame;
[0008] From each historical frame, key frames that have the possibility of looping with the current frame are selected, and the key point cloud of the key frames is obtained.
[0009] Based on the key point cloud and the current pose, determine the number of spatial points used to characterize the spatial relationship between the current pose and the key point cloud, and determine whether the number of spatial points exceeds a preset threshold.
[0010] A 3D ray tracing method is used to identify whether the position of the current frame matches the spatial visibility relationship of the key frame;
[0011] If the number of frames exceeds the limit and a match is successful, then the current frame and the keyframe are determined to form a loopback pair.
[0012] If the current frame does not exceed the limit and / or the matching fails, it is determined that the current frame and the key frame cannot form a loopback pair.
[0013] Optionally, determining the number of spatial points used to characterize the spatial relationship between the current pose and the keypoint cloud based on the keypoint cloud includes:
[0014] Dynamic objects are removed from the key point cloud and the historical point cloud of N frames before and after the key frame to obtain multi-frame point cloud data.
[0015] The point cloud data from each frame is integrated to form a point cloud sub-image;
[0016] Centered on the current pose, the point cloud sub-image is divided into equal-angle segments to obtain multiple fan-shaped regions with different corresponding angle ranges. All target fan-shaped regions that match the current pose are identified from each fan-shaped region. The number of the highest and lowest spatial points in all target fan-shaped regions is counted as the number of spatial points.
[0017] Optionally, identifying all target sector regions matching the current pose from each sector region includes:
[0018] Each spatial point in the point cloud sub-image is sequentially taken as a target spatial point, and the yaw angle between the target spatial point and the current pose is calculated.
[0019] The sector region whose corresponding angle range matches the yaw angle is taken as the target sector region.
[0020] Optionally, the step of using 3D ray tracing to identify whether the position of the current frame matches the spatial line-of-sight relationship of the keyframe includes:
[0021] Dynamic objects are removed from the keyframe point cloud and the historical point cloud of N frames before and after the keyframe to obtain multi-frame first point cloud data.
[0022] The first point cloud data of each frame and the current pose are fused to form a first point cloud set;
[0023] Dynamic objects are removed from the current frame point cloud and the historical point clouds of M frames before and after the current frame to obtain multi-frame second point cloud data, where M is less than N;
[0024] Select contour points from the spatial points of the first point cloud set to represent the contour of the point cloud.
[0025] The second point cloud data of each frame, the current pose, and each contour point are fused to form a second point cloud set; multiple voxel meshes of the same size are constructed in the second point cloud set;
[0026] Identify the current spatial point contained in each voxel grid, determine whether the corresponding voxel grid corresponds to an obstacle, and if so, mark the corresponding voxel grid as an obstacle grid;
[0027] Using a three-dimensional ray tracing method, it is determined whether the probability of passing through the obstructing mesh when traveling to each contour point in the current posture exceeds a probability threshold.
[0028] If not, then the match is successful;
[0029] If so, the match fails.
[0030] Optionally, identifying the current spatial point contained in each voxel grid and determining whether the corresponding voxel grid corresponds to an obstacle includes:
[0031] Based on the three-dimensional coordinates of each current spatial point, determine the index triplet corresponding to each current spatial point;
[0032] The voxel grid corresponding to each current spatial point is determined based on the index triplet of each current spatial point;
[0033] When the cumulative number of current spatial points in each voxel grid exceeds the preset maximum number, the corresponding obstacle in the voxel grid is determined.
[0034] Optionally, the step of using 3D ray tracing to determine whether the probability of passing through obstructing meshes when traveling to each contour point in the current pose exceeds a probability threshold includes:
[0035] Select contour points from the spatial points of the first point cloud set to represent the contour of the point cloud.
[0036] Construct a ray pointing from the current pose in the second point cloud set to each contour point;
[0037] Determine the number of contour points through which the corresponding ray passes in the obstructing mesh;
[0038] Determine the total number of each contour point;
[0039] The probability is obtained by calculating the ratio between the number of contour points and the total number.
[0040] Determine whether the probability exceeds the probability threshold.
[0041] Optionally, the step of filtering key frames from various historical frames that have a possibility of looping back with the current frame includes:
[0042] The nearest neighbor search method is used to select historical frames from the historical frames whose frame interval with the current frame exceeds the frame interval threshold as the key frames.
[0043] A loop closure detection device, comprising:
[0044] The acquisition module is used to acquire the current pose of the current frame;
[0045] The filtering module is used to filter key frames from various historical frames that have the possibility of looping back with the current frame, and to obtain the key point cloud of the key frames.
[0046] The judgment module is used to determine the number of spatial points that characterize the spatial relationship between the current pose and the key point cloud based on the key point cloud and the current pose, and to determine whether the number of spatial points exceeds a preset number threshold.
[0047] The recognition module is used to identify whether the position of the current frame matches the spatial visibility relationship of the key frame using a three-dimensional ray tracing method.
[0048] The determination module is used to determine that if the current frame and the key frame form a loopback pair if the number of frames exceeds the limit and the match is successful, and to determine that the current frame and the key frame cannot form a loopback pair if the number of frames exceeds the limit and / or the match fails.
[0049] A loop closure detection device includes a memory and a processor;
[0050] The memory is used to store programs;
[0051] The processor is used to execute the program to implement the various steps of the loop closure detection method described above.
[0052] A readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the various steps of the loop closure detection method described above.
[0053] As can be seen from the above technical solutions, the loop closure detection method provided in this application can filter keyframes that may form a loop closure pair with the current frame from multiple historical frames, narrowing the range of historical frames that need to be compared and improving the loop closure detection effect. Since the key point cloud of the key frame contains the three-dimensional coordinates of multiple spatial points and the current pose can indicate the spatial position and attitude information, the number of spatial points reflecting the correlation between the current frame and the key frame in spatial position can be calculated by combining the key point cloud and the current pose. By comparing with a preset number threshold, it can be determined whether the current frame and the key frame have sufficient correlation in spatial position, providing a quantitative judgment basis for determining the loop closure pair. At the same time, this application can use three-dimensional ray tracing to identify whether the position of the current frame matches the spatial visibility relationship of the key frame. The position of the current frame can reflect the spatial relationship of the current frame, while the spatial visibility relationship of the key frame can characterize the obstacle distribution characteristics of the environment corresponding to the key frame, such as the obstacle position and the mutual occlusion relationship of obstacles. For normal loop closure scenes, the current frame position and the key frame position are generally close to each other and can observe the same objects. Therefore, by analyzing the matching of the current frame position and the spatial visibility relationship between the keyframe and the current frame, the interaction logic between the current frame position and the obstacles in the keyframe environment can be evaluated, verifying whether the current frame and the keyframe conform to the spatial constraints of the same environment. This avoids including scenes with similar environments (adjacent or opposing tunnels) in loops. If the spatial position correlation is sufficient and the spatial visibility relationship matches, it can be determined that the keyframe and the current frame form a loop pair; otherwise, it is determined that the keyframe and the current frame cannot form a loop pair. Thus, this application can comprehensively consider the consistency of spatial overlap and spatial visibility relationship for loop detection, improving the accuracy and efficiency of loop detection. Attached Figure Description
[0054] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of this application. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.
[0055] Figure 1 This is a flowchart of a loop closure detection method disclosed in an embodiment of this application;
[0056] Figure 2 A schematic diagram of point cloud sub-image partitioning provided in an embodiment of this application;
[0057] Figure 3 This is a structural block diagram of a loop closure detection device disclosed in an embodiment of this application;
[0058] Figure 4This is a hardware structure block diagram of a loop closure detection device disclosed in an embodiment of this application. Detailed Implementation
[0059] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.
[0060] This application provides a loop closure detection method, which can be applied to various driving systems or mobile robot systems, as well as various computer terminals or smart terminals. The executing entity can be the processor or server of the computer terminal or smart terminal.
[0061] Next, combine Figure 1 The loop closure detection method of this application is described in detail, including the following steps:
[0062] Step S1: Obtain the current pose of the current frame.
[0063] Specifically, the current frame can be collected when the sensor detects that the motion trajectory meets the preset trigger conditions.
[0064] The sensors can be installed on mobile entities such as mobile robots, lidar, or cameras.
[0065] The trigger conditions can be set according to the accuracy of the actual motion trajectory coverage.
[0066] Generally, the triggering conditions can be that the turning angle of the running trajectory exceeds 0.5° or the movement distance exceeds 1 meter.
[0067] The current pose can include the position and orientation of the moving subject.
[0068] Step S2: From each historical frame, filter out key frames that have a possibility of looping with the current frame, and obtain the key point cloud of the key frame.
[0069] Specifically, it is possible to retrieve each historical frame of the moving subject.
[0070] To improve screening efficiency, historical frames can be initially screened based on time or frame intervals. Frames with an interval exceeding the frame number threshold from the current frame can be selected as candidate keyframes. Frames with too short intervals can be filtered out to avoid redundant calculations. This ensures that the selected keyframes maintain a sufficient frame interval to reduce mutual interference and ultimately obtain one or more keyframes with loopback potential.
[0071] Key point clouds can contain the three-dimensional coordinates of multiple spatial points.
[0072] Step S3: Based on the key point cloud and the current pose, determine the number of spatial points used to characterize the spatial relationship between the current pose and the key point cloud, and determine whether the number of spatial points exceeds a preset threshold.
[0073] Specifically, the coordinate system of the current pose can be unified with the coordinate system of the key point cloud to ensure that the two are analyzed under the same spatial reference.
[0074] Centered on the current pose, the spatial region corresponding to the keyframe is divided into regions, and the contour points in the keypoint cloud are retrieved in each region.
[0075] The retrieved contour points are spatial points that characterize the spatial relationship between the current pose and the keypoint cloud. Counting these points yields the number of spatial points used for judgment. This number is then compared to a preset threshold. If the threshold is exceeded, it indicates a high degree of spatial overlap and correlation between the current pose and the keypoint cloud, suggesting a high probability that the current frame and its corresponding keyframe form a loop closure pair. If the threshold is not exceeded, it indicates a weak spatial correlation, making a loop closure pair less likely. This process quantifies the degree of spatial correlation, providing crucial spatial features for subsequent loop closure pair determination, effectively filtering candidate frames with high loop closure potential, and reducing the computational load of subsequent processing.
[0076] The quantity threshold can be determined based on the number of spatial points of multiple loop pairs.
[0077] Step S4: Use three-dimensional ray tracing to identify whether the position of the current frame matches the spatial visibility relationship of the key frame.
[0078] Specifically, there are several ways to use 3D ray tracing to identify whether the position of the current frame matches the spatial visibility relationship of the key frame.
[0079] For example, based on the historical point cloud or environment map corresponding to the keyframe, the current pose of the current frame can be integrated into the historical point cloud or environment map of the keyframe, and 3D ray tracing can be performed with the position of the current frame as the origin to determine the visibility of the current position in the historical point cloud of the keyframe; if the visibility indicates that the current frame is visible in the historical point cloud corresponding to the keyframe, the match is successful; otherwise, the match fails.
[0080] For example, using the current frame position as the origin, 3D ray tracing can be performed based on the currently perceived environmental data to generate the visibility relationship features corresponding to the current position, such as the interaction logic of visible obstacles and the ray occlusion mode. Using the keyframe position as the origin, 3D ray tracing can be performed based on the historically perceived environmental data to generate the visibility relationship features corresponding to the key position, such as the interaction logic of visible obstacles and the ray occlusion mode. Subsequently, by comparing the consistency of the intersection point distribution of rays and obstacles, the overlap of the spatial visible range, and the matching degree of the occlusion order in the two types of visibility relationship features, it is determined whether the spatial visibility relationship between the current frame and the keyframe matches. If the core features match, it is determined that the visibility relationship between the two is successfully matched.
[0081] Step S5: If the number of frames exceeds the limit and the match is successful, then the current frame and the key frame are determined to form a loopback pair; if the number of frames exceeds the limit and / or the match fails, then the current frame and the key frame cannot form a loopback pair.
[0082] Specifically, when the number of contour points exceeds the number threshold and the current frame position matches the spatial visibility relationship with the key frame, it indicates that the environment of the current frame is consistent with the environment of the key frame, and it is determined that the current frame and the key frame can form a loop pair.
[0083] When the number of spatial points does not exceed the threshold and / or the current frame position does not match the spatial visibility relationship with the key frame, it indicates that the current frame and the key frame are not in the same space. In this case, the current frame and the key frame cannot form a loop pair.
[0084] As can be seen from the above technical solutions, the loop closure detection method provided in this application can filter keyframes that may form a loop closure pair with the current frame from multiple historical frames, narrowing the range of historical frames that need to be compared and improving the loop closure detection effect. Since the key point cloud of the key frame contains the three-dimensional coordinates of multiple spatial points and the current pose can indicate the spatial position and attitude information, the number of spatial points reflecting the correlation between the current frame and the key frame in spatial position can be calculated by combining the key point cloud and the current pose. By comparing with a preset number threshold, it can be determined whether the current frame and the key frame have sufficient correlation in spatial position, providing a quantitative judgment basis for determining the loop closure pair. At the same time, this application can use three-dimensional ray tracing to identify whether the position of the current frame matches the spatial visibility relationship of the key frame. The position of the current frame can reflect the spatial relationship of the current frame, while the spatial visibility relationship of the key frame can characterize the obstacle distribution characteristics of the environment corresponding to the key frame, such as the obstacle position and the mutual occlusion relationship of obstacles. For normal loop closure scenes, the current frame position and the key frame position are generally close to each other and can observe the same objects. Therefore, by analyzing the matching of the current frame position and the spatial visibility relationship between the keyframe and the current frame, the interaction logic between the current frame position and the obstacles in the keyframe environment can be evaluated, verifying whether the current frame and the keyframe conform to the spatial constraints of the same environment. This avoids including scenes with similar environments (adjacent or opposing tunnels) in loops. When the spatial position correlation is sufficient and the spatial visibility relationship matches, it can be determined that the keyframe and the current frame form a loop pair; otherwise, it is determined that the keyframe and the current frame cannot form a loop pair. Thus, this application can comprehensively consider the consistency of spatial overlap and spatial visibility relationship for loop detection, improving the accuracy and efficiency of loop detection.
[0085] In some embodiments of this application, the process of selecting key frames from various historical frames that have a possibility of loopback with the current frame in step S2 is described in detail, and the steps are as follows:
[0086] S20. Using the nearest neighbor search method, select historical frames from each historical frame whose frame number interval with the current frame exceeds the frame number interval threshold as the key frame.
[0087] Specifically, each historical frame has a unique corresponding historical index value, and each historical index value is used to indicate the frame number of the corresponding historical frame.
[0088] Similarly, the current frame also has a corresponding current index value.
[0089] To reduce the number of loops, if the difference between the keyframe index of the current frame and the previous loop is less than the first threshold, the current frame is removed.
[0090] From each historical frame, select historical frames whose difference from the current index value exceeds the frame interval threshold as candidate keyframes.
[0091] To reduce the detection frequency, candidate keyframes whose historical index value difference with other candidate keyframes is less than the second threshold are removed from the remaining candidate keyframes.
[0092] The final candidate keyframes are the keyframes.
[0093] Next, we will use a specific example to illustrate this.
[0094] If the current index value of the current frame is A, and the difference between A and A-1 is less than 5, then delete A;
[0095] Historical frames with a difference of more than 150 from A are selected as candidate keyframes.
[0096] Select candidate keyframes from the remaining candidate keyframes whose index difference with other candidate keyframes is less than 40 and delete them.
[0097] The remaining candidate keyframes are those that have a possibility of looping back with the current frame.
[0098] As can be seen from the above technical solution, this embodiment provides an optional method for filtering keyframes. Through the above method, the loop closure detection frequency can be further reduced and the loop closure detection process can be accelerated.
[0099] In some embodiments of this application, the process of determining the number of spatial points used to characterize the spatial relationship between the current pose and the key point cloud in step S3 is described in detail, and the steps are as follows:
[0100] S30. Remove dynamic objects from the key point cloud and the historical point cloud of N frames before and after the key frame to obtain multi-frame point cloud data.
[0101] Specifically, it is possible to obtain N historical frames before and after the keyframe;
[0102] Factor graphs can be used to optimize the acquired historical frames, keyframes, and current frames.
[0103] The centerpoint model can be used to dynamically remove objects from optimized historical frames and keyframes to obtain multi-frame point cloud data.
[0104] Among them, the CenterPoint model is a 3D target detection and tracking algorithm framework for laser point clouds. It achieves 3D target detection and tracking by detecting the target center point and regressing the target's size, orientation, and velocity.
[0105] S31. Integrate the point cloud data from each frame to form a point cloud sub-map.
[0106] Specifically, based on attitude estimation technology, the spatial position and attitude information of each frame of point cloud data in the global coordinate system can be determined, eliminating positional deviations between frames caused by sensor movement or dynamic changes in the scene.
[0107] Point cloud registration algorithms can be used to align multiple frames of point cloud data to ensure seamless fusion of adjacent frames in overlapping areas. Combined with optimization processes such as deduplication and filtering, calibrated single-frame point cloud data can be integrated to construct a point cloud sub-map that reflects historical spatial characteristics.
[0108] The point cloud subgraph can be the same as the first point cloud set.
[0109] S32. Using the current pose as the center, the point cloud sub-image is divided into equal-angle segments to obtain multiple fan-shaped regions with different corresponding angle ranges. All target fan-shaped regions that match the current pose are identified from each fan-shaped region. The number of the highest and lowest spatial points in all target fan-shaped regions is counted as the number of spatial points.
[0110] Specifically, to reduce computational complexity, the horizontal plane of the point cloud sub-image is divided at equal angles, using the current pose as the reference center point. Specifically, around this central pose, the point cloud sub-image is evenly divided from the center outwards at preset equal angular intervals, such as every 30 degrees or 45 degrees, resulting in a series of fan-shaped regions with varying angular ranges. Each fan-shaped region corresponds to a specific spatial angular interval, such as... Figure 2 As shown.
[0111] After the segmentation is completed, based on the attribute features of the current pose, such as position and orientation, all target sector regions that are spatially related to the current pose are identified from each sector region.
[0112] For all identified target sector regions, the number of the highest and lowest spatial points is counted, and the sum of these two types of points is taken as the number of spatial points in the point cloud sub-map. This provides quantitative data support for subsequent tasks such as analyzing scene structure and determining obstacle distribution through spatial point distribution features.
[0113] As can be seen from the above technical solution, this embodiment provides an optional method for determining the number of spatial points. This method further compresses the point cloud sub-image into a two-dimensional plane, performs equal-angle subdivision of the two-dimensional plane, and then counts the number of highest and lowest points in the associated region. This allows for the determination of the spatial relationship between obstacles related to the keyframe and the current pose, processing three-dimensional data into two-dimensional data, reducing the complexity of data processing. Furthermore, by focusing on the lowest and highest points, the environmental conditions of the current frame and the keyframe are evaluated, significantly reducing the computational load of data processing. In addition, this application converts the environmental consistency comparison between the current frame and the keyframe into a spatial point count, which simplifies the comparison compared to feature vector similarity comparison and accelerates the loop closure detection process.
[0114] In some embodiments of this application, the process of step S32, which involves dividing the point cloud sub-image into equal-angle segments centered on the current pose to obtain multiple fan-shaped regions with different corresponding angle ranges, and identifying all target fan-shaped regions matching the current pose from each fan-shaped region, is described in detail below:
[0115] S320. Sequentially take each spatial point in the point cloud sub-image as a target spatial point, and calculate the yaw angle between the target spatial point and the current pose.
[0116] Specifically, using the current pose as the origin, calculate the yaw angle of the spatial point relative to the origin.
[0117] The specific function is shown below:
[0118]
[0119] In the formula, y and x are the component values of the vector formed by the corresponding spatial point and the current pose; in order to facilitate the selection of the matching angle range, the yaw angle can be numerically rounded.
[0120] S321. The sector area whose corresponding angle range matches the yaw angle is taken as the target sector area.
[0121] Specifically, the sector regions that contain any one or more yaw angles within the corresponding angular range are designated as the target sector regions.
[0122] Specifically, the yaw angle can be used to divide the 3D point cloud into 360 sector regions in the horizontal direction (around the Z-axis). Then, it is evaluated whether there are spatial points in each sector region. In the sector regions where spatial points exist, the highest spatial point and the highest spatial point are determined, and the number of determined spatial points is calculated.
[0123] Since there are a total of 360 sector regions, if each sector region has a highest spatial point and a lowest spatial point, the maximum number of spatial points is 720.
[0124] However, in reality, when the current pose is not located at the center of the point cloud sub-image, such as when the current pose may fall in the lower left region of the point cloud sub-image, most of the spatial points of the point cloud sub-image should be located on the upper right side of the current pose. This may result in no spatial points or only one spatial point in the fan-shaped region corresponding to the lower left region of the point cloud sub-image, thus causing the number of spatial points to be less than 720.
[0125] Based on this principle, it is possible to assess whether the current pose is located at the center of the spatial region corresponding to the keyframe based on the number of spatial points.
[0126] Furthermore, considering that the motion trajectories of cameras or sensors in the same environment typically exhibit spatial clustering, specifically in loop closure detection, the essence of "loop closure" is that the sensor returns to an environmental area it has previously visited, such as a room or a street. In this case, the sensor's motion trajectory within that area will exhibit "spatial clustering" characteristics, meaning that the locations visited multiple times are usually distributed around the core area of the environment, rather than scattered in places far from the core.
[0127] In summary, by identifying the number of spatial points, it can be determined whether the current pose is located in the central region of the historical environment, thereby determining whether the historical frame corresponding to the historical environment can form a loop pair with the current frame.
[0128] As can be seen from the above technical solution, this embodiment provides an optional method for determining the target sector area. The matching sector area can be filtered by yaw angle through the above method.
[0129] In some embodiments of this application, the process of step S4, which uses three-dimensional ray tracing to identify whether the position of the current frame matches the spatial line-of-sight relationship of the key frame, is described in detail below:
[0130] S40. Remove dynamic objects from the key point cloud and the historical point cloud of N frames before and after the key frame to obtain multi-frame first point cloud data.
[0131] Specifically, N can be set according to the clarity of the point cloud sub-image.
[0132] S41. The first point cloud data of each frame and the current pose are fused to form a first point cloud set.
[0133] Specifically, the first point cloud data of each frame can be fused to form a first point cloud subset, and the first point cloud subset and the current pose can be represented in the same world coordinate system. The point that matches the current pose in the first point cloud subset is used as the current pose within the first point cloud subset, thus obtaining the first point cloud set.
[0134] S42. Remove dynamic objects from the current frame point cloud and the historical point clouds of M frames before and after the current frame to obtain multi-frame second point cloud data, where M is less than N. Specifically, the centerpoint model can be used to remove dynamic objects from the current frame point cloud and the historical point clouds of M frames before and after the current frame to obtain multi-frame second point cloud data.
[0135] Where M is much smaller than N.
[0136] The first point cloud data of each frame refers to the point cloud data obtained after removing dynamic objects from the point cloud data of the key frame and the N frames before and after it.
[0137] The second point cloud data in each frame refers to the point cloud data obtained after removing dynamic objects from the point cloud data of the current frame and the M frames before and after it.
[0138] S43. Select each contour point from each spatial point of the first point cloud set to represent the contour of the point cloud.
[0139] Specifically, for each spatial point, a sector region containing the yaw angle of that spatial point is determined within the corresponding angular range. The highest and lowest points within this sector region are found. When the height of the spatial point is greater than that of the highest point, the height of the highest point is updated to the height of the spatial point. When the height of the spatial point is less than that of the lowest point, the height of the lowest point is updated to the height of the spatial point. This sector region is then used as the target sector region.
[0140] The latest highest and lowest points within each target sector area are used as each contour point.
[0141] Each contour point can correspond to a world coordinate system.
[0142] S44. The second point cloud data of each frame, the current frame position, and each contour point are fused to form a second point cloud set.
[0143] Specifically, each second point cloud data and the current pose can be converted to the world coordinate system, and the point cloud data of each frame, each contour point and the current pose of the corresponding world coordinate system can be fused to form a second point cloud set.
[0144] S45. Construct multiple voxel meshes of the same size in the second point cloud set.
[0145] Specifically, based on the coordinate values of each contour point, the range for constructing the voxel mesh in the second point cloud set is determined to be [x]. min x max ]×[y min y max ]×[z min , z max ].
[0146] Calculate the dimension of different coordinate axes in the second point cloud set.
[0147] The specific method for calculating dimensions is as follows:
[0148]
[0149]
[0150]
[0151] In the formula, x max x is the maximum x-coordinate value among the contour points; min The minimum x-coordinate value among the contour points; y max The maximum ordinate value among the contour points; y min z is the minimum ordinate value among the contour points; max z is the maximum spatial coordinate value among the contour points. min is the minimum spatial coordinate value among the contour points; r is the voxel mesh size.
[0152] Based on each dimension, multiple voxel meshes of the same size are constructed within the range.
[0153] Each voxel mesh contains each contour point, each spatial point corresponding to the current frame, and the current pose.
[0154] S46. Identify the current spatial point contained in each voxel grid, determine whether the corresponding voxel grid corresponds to an obstacle, and if so, mark the corresponding voxel grid as an obstacle grid.
[0155] Specifically, the second point cloud set contains each current spatial point corresponding to the environment of the current frame; each voxel grid can contain current spatial points that reflect the environment information of the current frame.
[0156] Each current spatial point in the second point cloud set can be mapped to each voxel grid to determine whether the voxel grid contains more than the maximum number of current spatial points. If so, the corresponding voxel grid is marked as an obstructing grid; otherwise, no action is taken.
[0157] S47. Using a three-dimensional ray tracing method, determine whether the probability of passing through an obstructing mesh when traveling to each contour point in the current posture exceeds a probability threshold; if yes, the matching fails; if no, the matching succeeds.
[0158] Specifically, statistical variables can be initialized, and a 3D ray tracing method can be used to determine whether the current pose passes through an obstruction mesh when moving to each contour point. If so, the statistical variable is incremented by 1.
[0159] Calculate the probability based on statistical variables and the total number of contour points.
[0160] If the probability exceeds the probability threshold, it proves that if light is emitted from the current position to the contour point of the historical frame, the line of sight is likely to be obstructed, and the match is determined to be unsuccessful; if the probability does not exceed the probability threshold, it proves that if light is emitted from the current position to the contour point of the historical frame, the line of sight is less likely to be obstructed, and the match is determined to be successful.
[0161] The probability threshold can be obtained from historical statistics.
[0162] Generally, the probability threshold can be 0.9.
[0163] As can be seen from the above technical solution, this embodiment provides an optional method for identifying whether the spatial line-of-sight relationship between the current frame's position and the key frame matches using three-dimensional ray tracing. This method can simulate line-of-sight occlusion situations that may be encountered during actual movement using three-dimensional ray tracing. When the probability exceeds a threshold, it indicates that the light rays heading towards the contour point are largely blocked in the current posture, resulting in poor line-of-sight.
[0164] In some embodiments of this application, the process of step S46, identifying the current spatial point contained in each voxel mesh and determining whether the corresponding voxel mesh corresponds to an obstacle, is described in detail below:
[0165] S460. Based on the three-dimensional coordinates of each current spatial point, determine the index triplet corresponding to each current spatial point.
[0166] Specifically, the coordinate values of different current spatial points can be converted into index triples.
[0167] The conversion method is as follows:
[0168]
[0169]
[0170]
[0171] In the formula, (H) x H y H z ) is the index triplet of the current spatial point P(x, y, z).
[0172] S461. Determine the voxel mesh corresponding to the current spatial point based on the index triplet of each current spatial point.
[0173] Specifically, based on the index triples and the index range of each voxel grid, the voxel grid corresponding to different current spatial points is determined.
[0174] S462. When the cumulative number of current spatial points in each voxel grid exceeds the preset maximum number, determine the corresponding obstacle for the voxel grid.
[0175] Specifically, the cumulative number of current spatial points contained in each voxel grid can be counted. When this cumulative number exceeds a preset maximum number, the voxel grid is marked as an obstruction grid. The maximum number can be set according to the correspondence between actual obstructions and the number of current spatial points. Generally, it can be set to 5.
[0176] As can be seen from the above technical solution, this embodiment provides an optional method to identify the current spatial point contained in each voxel grid and determine whether the corresponding voxel grid corresponds to an obstacle. Through the above method, the correspondence between each current spatial point and the voxel grid can be established by converting the index triplet, thereby improving the accuracy of the cumulative quantity calculation and thus improving the reliability of this application.
[0177] In some embodiments of this application, the process of determining whether the probability of passing through the obstructing mesh when traveling to each contour point in the current pose exceeds a probability threshold using three-dimensional ray tracing is described in detail below:
[0178] S470. Construct a ray that points from the current pose in the second point cloud set to each contour point.
[0179] Specifically, a ray can be constructed from the current pose in the point cloud set, pointing to each contour point.
[0180] S471. Determine the number of contour points through which the corresponding ray passes.
[0181] Specifically, each ray can pass through multiple voxel grids. It is determined whether each voxel grid contains an obstructing grid. The number of rays passing through an obstructing grid is the number of contour points.
[0182] S472. Determine the total number of each contour point.
[0183] Specifically, the total number of contour points contained in the point cloud set can be counted.
[0184] S473. Calculate the ratio between the number of contour points and the total number to obtain the probability.
[0185] Specifically, the ratio between the number of contour points and the total number can be used as the probability.
[0186] S474. Determine whether the probability exceeds the probability threshold.
[0187] Specifically, the probability can be compared with a probability threshold.
[0188] As can be seen from the above technical solution, this embodiment provides an optional method for calculating whether the probability of passing through the obstruction grid when moving to each contour point in the current posture exceeds the probability threshold using the three-dimensional ray tracing method. In this way, the probability of encountering obstacles when moving in the current posture in the current frame in the environment of the historical frame can be converted into the ratio between the number of rays passing through the obstruction grid and the total number of contour points, which simplifies the probability calculation process.
[0189] Next, we will combine Figure 3 The loop closure detection device provided in this application is described in detail. The loop closure detection device described below can be compared with the loop closure detection method described above.
[0190] See Figure 3 It can be observed that the loop closure detection device may include:
[0191] The acquisition module 10 is used to acquire the current pose of the current frame;
[0192] The filtering module 20 is used to filter key frames that have a loopback possibility with the current frame from each historical frame, and to obtain the key point cloud of the key frame.
[0193] The judgment module 30 is used to determine the number of spatial points that characterize the spatial relationship between the current pose and the key point cloud based on the key point cloud and the current pose, and to determine whether the number of spatial points exceeds a preset number threshold.
[0194] The recognition module 40 is used to identify whether the position of the current frame matches the spatial visibility relationship of the key frame using a three-dimensional ray tracing method.
[0195] The determination module 50 is used to determine that if the current frame and the key frame form a loopback pair if the number of matches exceeds the limit and the match is successful; and to determine that the current frame and the key frame cannot form a loopback pair if the number of matches exceeds the limit and / or the match fails.
[0196] Furthermore, the filtering module 20 may include:
[0197] The key frame filtering unit is used to use a nearest neighbor search method to filter historical frames from each historical frame whose frame number interval with the current frame exceeds a frame number interval threshold as the key frames.
[0198] Furthermore, the judgment module 30 may include:
[0199] The dynamic object removal unit is used to remove dynamic objects from the key point cloud and the historical point cloud of N frames before and after the key frame to obtain multi-frame point cloud data.
[0200] The point cloud sub-image generation unit is used to integrate point cloud data from each frame to form a point cloud sub-image;
[0201] The spatial point count unit is used to divide the point cloud sub-image into equal-angle segments centered on the current pose, to obtain multiple fan-shaped regions with different corresponding angle ranges, and to identify all target fan-shaped regions that match the current pose from each fan-shaped region, and to count the number of the highest and lowest spatial points in all target fan-shaped regions as the spatial point count.
[0202] Furthermore, the spatial point count unit may include:
[0203] The yaw angle calculation subunit is used to sequentially take each spatial point in the point cloud sub-image as a target spatial point and calculate the yaw angle between the target spatial point and the current pose.
[0204] The region filtering subunit is used to select the sector region whose corresponding angle range matches the yaw angle as the target sector region.
[0205] Furthermore, the identification module 40 may include:
[0206] The point cloud data acquisition unit is used to remove dynamic objects from the key frame point cloud and the historical point cloud of N frames before and after the key frame to obtain multi-frame first point cloud data.
[0207] A point cloud set acquisition unit is used to fuse the first point cloud data of each frame and the current pose to form a first point cloud set;
[0208] The dynamic object removal unit is used to remove dynamic objects from the current frame point cloud and the historical point cloud of M frames before and after the current frame to obtain multi-frame second point cloud data, where M is less than N.
[0209] The contour point filtering and acquisition unit is used to filter each contour point used to characterize the contour of the point cloud from each spatial point of the point cloud set.
[0210] The contour point fusion unit is used to fuse the second point cloud data of each frame, the current pose, and each contour point to form a second point cloud set.
[0211] A voxel mesh construction and acquisition unit is used to construct multiple voxel meshes of the same size in the second point cloud set;
[0212] The obstacle grid identification unit is used to identify the current spatial point contained in each voxel grid, determine whether the corresponding voxel grid corresponds to an obstacle, and if so, mark the corresponding voxel grid as an obstacle grid.
[0213] The probability calculation unit is used to determine, using the three-dimensional ray tracing method, whether the probability of passing through the obstructing mesh when traveling to each contour point in the current posture exceeds the probability threshold; if not, the matching is successful; if yes, the matching fails.
[0214] Furthermore, the obstruction grid recognition unit may include:
[0215] The first obstacle grid identification sub-unit is used to determine the index triplet corresponding to each current spatial point based on the three-dimensional coordinates of each current spatial point;
[0216] The second obstacle mesh identification sub-unit is used to determine the voxel mesh corresponding to the current spatial point based on the index triplet of each current spatial point;
[0217] The third obstacle grid identification subunit is used to identify the obstacle corresponding to the voxel grid when the cumulative number of current spatial points in each voxel grid is greater than the preset maximum number.
[0218] Furthermore, the probability calculation unit may include:
[0219] The first probability calculation subunit is used to construct a ray pointing to each contour point from the current pose in the second point cloud set as the starting point.
[0220] The second probability calculation sub-unit is used to determine the number of contour points through which the corresponding ray passes the obstructing mesh;
[0221] The third probability calculation subunit is used to determine the total number of each contour point;
[0222] The fourth probability calculation subunit is used to calculate the ratio between the number of contour points and the total number to obtain the probability;
[0223] The fifth probability calculation subunit is used to determine whether the probability exceeds the probability threshold.
[0224] The loopback detection device provided in this application embodiment can be applied to loopback detection equipment, such as PC terminals, cloud platforms, servers, and server clusters. Optionally, Figure 4 The hardware structure block diagram of the loop closure detection device is shown. (Refer to...) Figure 4 The hardware structure of the loop closure detection device may include: at least one processor 1, at least one communication interface 2, at least one memory 3, and at least one communication bus 4;
[0225] In this embodiment of the application, the number of processor 1, communication interface 2, memory 3, and communication bus 4 is at least one, and processor 1, communication interface 2, and memory 3 communicate with each other through communication bus 4;
[0226] Processor 1 may be a central processing unit (CPU), an application-specific integrated circuit (ASIC), or one or more integrated circuits configured to implement embodiments of the present invention.
[0227] Memory 3 may include high-speed RAM, and may also include non-volatile memory, such as at least one disk storage device;
[0228] The memory stores a program, which the processor can call. The program is used for:
[0229] Get the current pose of the current frame;
[0230] From each historical frame, key frames that have the possibility of looping with the current frame are selected, and the key point cloud of the key frames is obtained.
[0231] Based on the key point cloud and the current pose, determine the number of spatial points used to characterize the spatial relationship between the current pose and the key point cloud, and determine whether the number of spatial points exceeds a preset threshold.
[0232] A 3D ray tracing method is used to identify whether the position of the current frame matches the spatial visibility relationship of the key frame;
[0233] If the number of frames exceeds the limit and a match is successful, then the current frame and the keyframe are determined to form a loopback pair.
[0234] If the current frame does not exceed the limit and / or the matching fails, it is determined that the current frame and the key frame cannot form a loopback pair.
[0235] Optionally, the refined and extended functions of the program can be referred to the above description.
[0236] This application embodiment also provides a readable storage medium that can store a program suitable for execution by a processor, the program being used for:
[0237] Get the current pose of the current frame;
[0238] From each historical frame, key frames that have the possibility of looping with the current frame are selected, and the key point cloud of the key frames is obtained.
[0239] Based on the key point cloud and the current pose, determine the number of spatial points used to characterize the spatial relationship between the current pose and the key point cloud, and determine whether the number of spatial points exceeds a preset threshold.
[0240] A 3D ray tracing method is used to identify whether the position of the current frame matches the spatial visibility relationship of the key frame;
[0241] If the number of frames exceeds the limit and a match is successful, then the current frame and the keyframe are determined to form a loopback pair.
[0242] If the current frame does not exceed the limit and / or the matching fails, it is determined that the current frame and the key frame cannot form a loopback pair.
[0243] Optionally, the refined and extended functions of the program can be referred to the above description.
[0244] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.
[0245] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.
[0246] The above description of the disclosed embodiments enables those skilled in the art to make or use this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of this application. The various embodiments of this application can be combined with each other. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A loop closure detection method, characterized in that, include: Get the current pose of the current frame; From each historical frame, key frames that have the possibility of looping with the current frame are selected, and the key point cloud of the key frames is obtained. Based on the key point cloud and the current pose, determine the number of spatial points used to characterize the spatial relationship between the current pose and the key point cloud, and determine whether the number of spatial points exceeds a preset threshold. A 3D ray tracing method is used to identify whether the position of the current frame matches the spatial visibility relationship of the key frame; If the number of frames exceeds the limit and a match is successful, then the current frame and the keyframe are determined to form a loopback pair. If the current frame does not exceed the limit and / or the matching fails, it is determined that the current frame and the key frame cannot form a loopback pair.
2. The loop closure detection method according to claim 1, characterized in that, The step of determining the number of spatial points used to characterize the spatial relationship between the current pose and the key point cloud based on the key point cloud includes: Dynamic objects are removed from the key point cloud and the historical point cloud of N frames before and after the key frame to obtain multi-frame point cloud data. The point cloud data from each frame is integrated to form a point cloud sub-image; Centered on the current pose, the point cloud sub-image is divided into equal-angle segments to obtain multiple fan-shaped regions with different corresponding angle ranges. All target fan-shaped regions that match the current pose are identified from each fan-shaped region. The number of the highest and lowest spatial points in all target fan-shaped regions is counted as the number of spatial points.
3. The loop closure detection method according to claim 2, characterized in that, The step of identifying all target sector regions that match the current pose from each sector region includes: Each spatial point in the point cloud sub-image is sequentially taken as a target spatial point, and the yaw angle between the target spatial point and the current pose is calculated. The sector region whose corresponding angle range matches the yaw angle is taken as the target sector region.
4. The loop closure detection method according to claim 1, characterized in that, The step of using 3D ray tracing to identify whether the position of the current frame matches the spatial visibility relationship of the keyframe includes: Dynamic objects are removed from the keyframe point cloud and the historical point cloud of N frames before and after the keyframe to obtain multi-frame first point cloud data. The first point cloud data of each frame and the current pose are fused to form a first point cloud set; Dynamic objects are removed from the current frame point cloud and the historical point clouds of M frames before and after the current frame to obtain multi-frame second point cloud data, where M is less than N; Select contour points from the spatial points of the first point cloud set to represent the contour of the point cloud. The second point cloud data of each frame, the current pose, and each contour point are fused to form a second point cloud set; Construct multiple voxel meshes of the same size in the second point cloud set; Identify the current spatial point contained in each voxel grid, determine whether the corresponding voxel grid corresponds to an obstacle, and if so, mark the corresponding voxel grid as an obstacle grid; Using a three-dimensional ray tracing method, it is determined whether the probability of passing through the obstructing mesh when traveling to each contour point in the current posture exceeds a probability threshold. If not, then the match is successful; If so, the match fails.
5. The loop closure detection method according to claim 4, characterized in that, The step of identifying the current spatial point contained in each voxel grid and determining whether the corresponding voxel grid corresponds to an obstacle includes: Based on the three-dimensional coordinates of each current spatial point, determine the index triplet corresponding to each current spatial point; The voxel grid corresponding to each current spatial point is determined based on the index triplet of each current spatial point; When the cumulative number of current spatial points in each voxel grid exceeds the preset maximum number, the corresponding obstacle in the voxel grid is determined.
6. The loop closure detection method according to claim 4, characterized in that, The step of using 3D ray tracing to determine whether the probability of passing through obstructing meshes when moving to each contour point in the current pose exceeds a probability threshold includes: Construct a ray pointing from the current pose in the second point cloud set to each contour point; Determine the number of contour points through which the corresponding ray passes in the obstructing mesh; Determine the total number of each contour point; The probability is obtained by calculating the ratio between the number of contour points and the total number. Determine whether the probability exceeds the probability threshold.
7. The loop closure detection method according to any one of claims 1-6, characterized in that, The step of filtering key frames from various historical frames that have a possibility of looping back with the current frame includes: The nearest neighbor search method is used to select historical frames from the historical frames whose frame interval with the current frame exceeds the frame interval threshold as the key frames.
8. A loop closure detection device, characterized in that, include: The acquisition module is used to acquire the current pose of the current frame; The filtering module is used to filter key frames from various historical frames that have the possibility of looping back with the current frame, and to obtain the key point cloud of the key frames. The judgment module is used to determine the number of spatial points that characterize the spatial relationship between the current pose and the key point cloud based on the key point cloud and the current pose, and to determine whether the number of spatial points exceeds a preset number threshold. The recognition module is used to identify whether the position of the current frame matches the spatial visibility relationship of the key frame using a three-dimensional ray tracing method. The determination module is used to determine that if the current frame and the key frame form a loopback pair if the number of frames exceeds the limit and the match is successful, and to determine that the current frame and the key frame cannot form a loopback pair if the number of frames exceeds the limit and / or the match fails.
9. A loop closure detection device, characterized in that, Including memory and processor; The memory is used to store programs; The processor is configured to execute the program to implement the various steps of the loop closure detection method as described in any one of claims 1-7.
10. A readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements each step of the loop closure detection method as described in any one of claims 1-7.