Feature point cloud descriptor-based loopback detection method
Through a combination method based on the European-style distance, smoothness formula, polar coordinates and Log-Gabor filter, the problems of real-time and robustness in lidar loop detection are solved, and efficient loop detection in micro embedded robots is realized.
Patent Information
- Application Number
- CN202411213791.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-31
- Publication Date
- 2025-01-07
AI Technical Summary
The existing lidar loop detection methods have shortcomings in real-time and robustness, especially in micro embedded robots, the computing power demand of deep learning methods is too large and the training model is poor, and the direct matching method is computationally large. Description Although the sub-match method reduces the data processing volume, there are challenges in designing a robust detection method.
The closest keyframe search based on the European distance is used, the smoothness formula calculates feature points, polar coordinates are used to construct a two-dimensional descriptive sub-image, the Log-Gabor filter strengthens the features, and the position transformation is searched through Fourier transform and standardized power spectrum. Improve the robustness and accuracy of loopback detection.
It realizes improving robustness and accuracy in lidar loop detection, reducing computing power requirements, and is suitable for micro embedded robots, enhancing the real-time and adaptability of loop detection.
Smart Images
Figure CN119273625A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of loop closure detection of laser radar, and in particular to a loop closure detection method based on feature point cloud descriptors. Background Art
[0002] The loop detection technology of LiDAR is an important part of the robot's construction of global maps and correction of accumulated errors. The robot uses LiDAR data to calculate its own posture changes and create a three-dimensional map at the same time. Due to the noise of LiDAR, the accumulated error increases after the long-distance laser odometer calculation. The loop detection technology is introduced to correct the map and the robot's posture estimation. Loop detection calculates and adds posture constraints by identifying whether the robot returns to the previously visited location. The loop detection methods based on LiDAR can be roughly divided into three categories: direct matching methods, descriptor matching, and deep learning. The direct matching method uses point-to-point and feature geometry to feature geometry direct registration methods to detect loops. The calculation data is huge and cannot meet the real-time requirements well. Deep learning matching methods such as: CNN algorithms that use convolutional neural networks to extract features from point clouds, models based on Transformer architecture, and Transformer-based Models algorithms for processing serialized point cloud data. These algorithms require huge computing power and cannot be deployed in micro embedded robots. In addition, the trained models are not very applicable and have poor loop robustness in various environments. The descriptor matching method converts a 3D point cloud into a 2D polar coordinate image by designing a loop detection descriptor such as the scan context algorithm, uses the number of point clouds to represent the value in the 2D image, and determines the change in the position of the loop point by matching the 2D image. This type of method reduces the amount of data processing while not requiring huge computing power, and ensures the robustness and accuracy of loop detection. However, it is crucial to design an excellent loop detection descriptor and a robust detection and registration method. Summary of the invention
[0003] The purpose of the present invention is to provide a loop detection method based on a feature point cloud descriptor, so as to improve the robustness and accuracy of loop detection based on the design of the loop descriptor and the design of the loop detection method.
[0004] To achieve the above object, the present invention provides a loop detection method based on feature point cloud descriptors, comprising the following steps:
[0005] Step 1: Find the nearest key frame based on Euclidean distance;
[0006] Step 2: Calculate and extract feature points of the current frame and the nearest key frame based on the smoothness formula;
[0007] Step 3: Construction of two-dimensional descriptor image based on polar coordinates;
[0008] Step 4: Image feature enhancement based on Log-Gabor filter;
[0009] Step 5: Look up the pose transformation of the two images based on Fourier transform and normalized power spectrum;
[0010] Optionally, during the execution of step 1, the Euclidean distance between the current frame and the previous key frame is first calculated, and when the Euclidean distance is less than the loop distance threshold, the key frame data is saved;
[0011] Optionally, during the execution of step 2, the point clouds of the previous and next two frames use a smoothness calculation formula to distinguish different types of feature points, including plane points and edge points. The specific formula is as follows:
[0012]
[0013] Among them, it means The curvature of a point, Indicates the distance value of the curvature point that needs to be calculated. Indicates at point i The distance values of the surrounding 10 points, A is a constant.
[0014] Optionally, during the execution of step 3, the following steps are included:
[0015] Step 3.1: Initialize a two-dimensional matrix with 80 rows and 360 columns;
[0016] Step 3.2: Divide the 3D point cloud into 80*360 blocks according to polar coordinates;
[0017] Step 3.3: Count the number of plane points and edge points in each small area, and use the following formula to combine the number of two types of feature points:
[0018]
[0019] Where N is the combined value, is the number of plane points, is the number of edge points;
[0020] Step 3.4: Fill the two-dimensional matrix with N;
[0021] Optionally, during the execution of step 4, a radial filter and an angular filter constituting a Log-Gabor filter are constructed according to the following formula to enhance the two-dimensional image features:
[0022] {G}_{r}(r)=exp(-\frac {{[log(r / {f}_{0})]}^{2}} {2*A{}^{2}_{r}})
[0023]
[0024]
[0025] in is a radial filter, is the angle filter, is the Log-Gabor filter, is the meridian coordinate, is the angle coordinate, , are the filter center frequency and direction angle respectively, , are the radial bandwidth and angular bandwidth of the filter respectively;
[0026] Step 5.1: Use the Fourier transform formula to transform the two-dimensional image obtained in step 3, filter the two-dimensional image with the Log-Gabor filter constructed in step 4, and obtain the filtered image using the inverse Fourier transform;
[0027] Step 5.2: Use the standardized power spectrum formula to calculate the correlation strength of the two filtered images at each frequency, find the maximum correlation position, determine the loop point and the posture transformation between the two images. The specific standardized power spectrum formula is as follows:.
[0028]
[0029] in is the normalized power spectrum value, , They are two filtered two-dimensional images respectively.
[0030] The present invention provides a loop detection method based on feature point cloud descriptors, which includes searching for the nearest key frame based on Euclidean distance; calculating and extracting feature points of the current frame and the nearest key frame based on a smoothness formula; constructing a two-dimensional descriptor image based on polar coordinate transformation; enhancing image features based on a Log-Gabor filter; and finally searching for the posture transformation of two images based on Fourier transform and normalized power spectrum to find loop situations and posture transformations existing in two point clouds. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0032] Figure 1 It is a flow chart of a loop closure detection method based on a feature point cloud descriptor of the present invention.
[0033] Figure 2 It is a schematic diagram of converting a three-dimensional point cloud image into a two-dimensional polar coordinate image according to the present invention. DETAILED DESCRIPTION
[0034] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.
[0035] See also Figure 1 , where represents the i The curvature of a point indicates the distance value of the curvature point to be calculated, indicating the distance between the points i The distance values of the surrounding 10 points, A is a constant, the object of the present invention is to provide a loop detection method based on a feature point cloud descriptor, comprising the following steps:
[0036] S1: nearest keyframe search based on Euclidean distance;
[0037] S2: Calculation and extraction of feature points of the current frame and the nearest key frame based on the smoothness formula;
[0038] S3: Construction of 2D descriptor images based on polar coordinates;
[0039] S4: Image feature enhancement based on Log-Gabor filter;
[0040] S5: Two-image pose transformation search based on Fourier transform and normalized power spectrum.
[0041] The following is a further explanation based on the specific implementation steps:
[0042] Step S1: searching for the nearest key frame based on Euclidean distance;
[0043] Specifically, the Euclidean distance between the current frame and the previous key frame is first calculated. When the Euclidean distance is less than the loop distance threshold, the key frame data is saved;
[0044] Step S2: Calculate and extract feature points of the current frame and the nearest key frame based on the smoothness formula;
[0045] Specifically, the point clouds of the previous and next two frames use the smoothness calculation formula to distinguish different types of feature points, including plane points and edge points. The specific formula is as follows:
[0046]
[0047] Among them, it means The curvature of a point, Indicates the distance value of the curvature point that needs to be calculated. Indicates at point i The distance values of the surrounding 10 points, A is a constant.
[0048] Step S3: constructing a two-dimensional descriptor image based on polar coordinates;
[0049] For details, please refer to Figure 2 Schematic diagram of converting a 3D point cloud image to a 2D polar coordinate image, where R1 is the minimum radar observation distance and R2 is the maximum radar observation distance. First, initialize the 2D matrix, where the rows are 80 and the columns are 360. Then, divide the 3D point cloud into 80*360 blocks in polar coordinates. Then, count the number of plane points and edge points in each small area. Finally, fill the 2D matrix with the number of plane points and edge points. The formula for the number of feature point combinations is as follows:
[0050]
[0051] Where N is the combined value, is the number of plane points, is the number of edge points;
[0052] Step S4: Image feature enhancement based on Log-Gabor filter;
[0053] Specifically, firstly, the radial filter and the angular filter constituting the Log-Gabor filter are constructed according to the following formula to enhance the two-dimensional image features:
[0054] {G}_{r}(r)=exp(-\frac {{[log(r / {f}_{0})]}^{2}} {2*A{}^{2}_{r}})
[0055]
[0056]
[0057] in is a radial filter, is the angle filter, is the Log-Gabor filter, is the meridian coordinate, is the angle coordinate, , are the filter center frequency and direction angle respectively, , are the radial bandwidth and angular bandwidth of the filter respectively;
[0058] Step S5: searching for the pose transformation of the two images based on Fourier transform and normalized power spectrum;
[0059] Step 5.1: Use the Fourier transform formula to transform the two-dimensional image obtained in step 3, filter the two-dimensional image with the Log-Gabor filter constructed in step S4, and obtain the filtered image using the inverse Fourier transform;
[0060] Step 5.2: Use the standardized power spectrum formula to calculate the correlation strength of the two filtered images at each frequency, find the maximum correlation position, determine the loop point and the posture transformation between the two images. The specific standardized power spectrum formula is as follows:.
[0061]
[0062] in is the normalized power spectrum value, , They are two filtered two-dimensional images respectively.
[0063] What is disclosed above is only a preferred embodiment of the present invention, and it certainly cannot be used to limit the scope of rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of the above embodiment and equivalent changes made according to the claims of the present invention still fall within the scope of the invention.
Claims
1. A loop detection method based on feature point cloud descriptor, characterized in that: The following steps are involved: Step 1: Find the nearest key frame based on Euclidean distance; Step 2: Calculate and extract feature points of the current frame and the nearest key frame based on the smoothness formula; Step 3: Construction of two-dimensional descriptor image based on polar coordinates; Step 4: Image feature enhancement based on Log-Gabor filter; Step 5: Look up the pose transformation of the two images based on Fourier transform and normalized power spectrum.
2. The loop detection method based on feature point cloud descriptor according to claim 1, characterized in that: During the execution of step 1, the Euclidean distance between the current frame and the previous key frame is first calculated. When the Euclidean distance is less than the loop distance threshold, the key frame data is saved.
3. The loop detection method based on feature point cloud descriptor according to claim 1, characterized in that: During the execution of step 2, the two frames of point cloud use the smoothness calculation formula to distinguish different types of feature points, including plane points and edge points. The specific formula is as follows: Among them, it means The curvature of a point, Indicates the distance value of the curvature point that needs to be calculated. Indicates at point i The distance values of the surrounding 10 points, A is a constant.
4. The loop detection method based on feature point cloud descriptor according to claim 1, characterized in that: The execution process of step 3 includes the following steps: Step 3.1: Initialize a two-dimensional matrix with 80 rows and 360 columns; Step 3.2: Divide the 3D point cloud into 80*360 blocks according to polar coordinates; Step 3.3: Count the number of plane points and edge points in each small area, and use the following formula to combine the number of two types of feature points: Where N is the combined value, is the number of plane points, is the number of edge points; Step 3.4: Fill the two-dimensional matrix with N.
5. The loop detection method based on feature point cloud descriptor according to claim 1, characterized in that: During the execution of step 4, the radial filter and the angular filter constituting the Log-Gabor filter are constructed according to the following formula to enhance the two-dimensional image features: {G}_{r}(r)=exp(-\frac {{[log(r / {f}_{0})]}^{2}} {2*A{}^{2}_{r}}) in is a radial filter, is the angle filter, is the Log-Gabor filter, is the meridian coordinate, is the angle coordinate, , are the filter center frequency and direction angle respectively, , are the radial bandwidth and angular bandwidth of the filter respectively.
6. The loop detection method based on feature point cloud descriptor according to claim 1, characterized in that: The execution process of step 5 includes the following steps: Step 5.1: Use the Fourier transform formula to transform the two-dimensional image obtained in step 3, filter the two-dimensional image with the Log-Gabor filter constructed in step 4, and obtain the filtered image using the inverse Fourier transform; Step 5.2: Use the normalized power spectrum formula to calculate the correlation strength of the two filtered images at each frequency, find the maximum correlation position, determine the loop point and the pose transformation between the two images. The specific normalized power spectrum formula is as follows: in is the normalized power spectrum value, , They are two filtered two-dimensional images respectively.