Indoor loopback detection method and system based on angle characteristics

Abstract

The invention belongs to the technical field of mobile robot positioning and mapping. The invention provides an indoor loopback detection method and system based on angle features, and the method comprises the steps: carrying out the filtering of a key frame point cloud in a height direction, generating an average point cloud, extracting angle feature points from the average point cloud, carrying out the discrimination of a convex angle and a concave angle of each angle feature point, and carrying out the recognition of a convex angle and a concave angle. And constructing triangular descriptors based on the angle feature points and the corresponding convex angle and concave angle discrimination results, performing unified management on the triangular descriptors in the historical key frame by adopting a hash table structure, and determining the current key frame according to each triangular descriptor of the current key frame and the triangular descriptors of the historical key frame in the hash table structure. According to the method, multiple candidate loopback frames are determined, loopback verification is carried out on each candidate key frame, an optimal loopback frame is determined, two-dimensional rigid body transformation corresponding to the optimal loopback frame is used as initial pose estimation of point cloud registration between the current key frame and the optimal loopback frame, and high-robustness loopback detection is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot positioning and mapping, and particularly relates to an indoor loop detection method and system based on angle features. Background Art

[0002] The statements in this part only provide the background art related to the present invention and do not necessarily constitute the prior art.

[0003] With the development of lidar technology, laser SLAM (Simultaneous Localization and Mapping) has become one of the core technologies in the fields of mobile robots, autonomous driving, indoor surveying and mapping, etc. The SLAM system senses the surrounding environment through sensors and realizes real-time positioning and mapping in an unknown space. In order to ensure the consistency of map construction and suppress the diffusion of cumulative errors, loop detection, as one of the key modules in the SLAM system, can correct the cumulative errors by identifying the historically visited positions, thereby optimizing the overall trajectory and map structure.

[0004] The current mainstream lidar loop detection methods can be roughly divided into two categories: one is the scan matching method based on global descriptors (such as Scan Context, M2DP, etc.), which constructs a global feature representation of the scene and calculates the similarity between features to judge the loop relationship; the other is the method based on local geometric feature extraction, such as edges or planes. This type of method relies on the feature extraction and registration of structurally significant regions to achieve loop detection. These methods have good performance in outdoor environments with clear structures and open views. However, in indoor scenes, due to factors such as enclosed spaces, strong structural repetitiveness, and many occlusions, the above methods also face challenges. Global descriptors are often prone to false matches in scenes such as corridors and open office areas, while local features have insufficient stability and recognition due to their single structure, resulting in a decrease in the robustness and accuracy of loop detection. Summary of the Invention

[0005] To solve the deficiencies of the prior art, the present invention provides an indoor loop detection method and system based on angle features, which identify the angle features in the indoor environment point cloud, extract key points at the angle features and construct triangle descriptors, realizing highly robust loop detection and improving the accuracy of loop detection.

[0006] To achieve the above object, the present invention adopts the following technical solutions: In the first aspect, the present invention provides an indoor loop detection method based on angle features.

[0007] An indoor loop detection method based on angle features includes the following processes: Construct key-frame point clouds based on the odometry pose of the robot and the lidar point cloud; Perform height-direction filtering on the key-frame point clouds to generate average point clouds; Extract angular feature points from the average point clouds and discriminate convex and concave angles for each angular feature point; Construct triangle descriptors based on the angular feature points and the corresponding convex and concave angle discrimination results, and use a hash table structure to uniformly manage the triangle descriptors in historical key frames; Determine multiple candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure; Perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the 2D rigid body transformation corresponding to the best loop closure frame as the initial pose estimate for point cloud registration between the current key frame and the best loop closure frame.

[0008] As a further limitation of the first aspect of the present invention, constructing key-frame point clouds based on the odometry pose of the robot and the lidar point cloud includes: Starting from a certain reference frame time Start accumulating Frame scan point cloud data. For each frame of point cloud data , where , and represents the timestamp of the th frame. According to the relative transformation relationship between the pose of and the reference frame pose , transform the point cloud to the reference frame coordinate system: , where is a point in the original radar point cloud, is the odometry pose of this frame, is the odometry pose of the reference frame, represents the point coordinates after transformation in the reference frame coordinate system. Merge all the point clouds after pose transformation to form a complete key-frame point cloud .

[0009] As a further limitation of the first aspect of the present invention, performing height-direction filtering on the point cloud of the key frame to generate an average point cloud includes: Taking the center of the lidar as the origin of the polar coordinate system, construct a two-dimensional polar coordinate system. Let the key-frame point cloud after height filtering be . Uniformly divide the range into multiple sectors on the horizontal plane; For any point , calculate the azimuth angle and assign it to the corresponding sector; Compute the mean and standard deviation of the point cloud projection on the two-dimensional plane in each sector. For any current sector, when and When the standard deviation in each direction is less than the corresponding set threshold, the mean of the points in the current sector is added to the average point cloud set. After all sectors are processed, at most one point is retained in each sector as the final average point cloud.

[0010] As a further limitation of the first aspect of the present invention, The range is evenly divided into sectors, and the angle corresponding to each sector is: ; For any point , calculate the azimuth ,Will Assigned to the corresponding sector, the sector ID is: ,in, and for The coordinate value in the key frame coordinate system, for azimuth, α is the size of the azimuth of each sector, for The corresponding sector ID, is the floor function.

[0011] As a further limitation of the first aspect of the present invention, extracting angle feature points from the average point cloud includes: Using the average point cloud obtained Construct a first kD-Tree for the average point cloud Perform a radius search for each point in the , and calculate the average coordinates of all neighboring points within the search radius of each point, and get the average point cloud Another point set with the same number of points ; use The point set constructs a second kD-Tree, and then for the average point cloud Each point in the second kD-Tree searches for a nearest neighbor point. If the distance between the two points is greater than the preset threshold, the point is retained, thereby obtaining the point set after the first screening. ; Use the point set after the first screening Construct a third kD-Tree, perform a radius search for each point in the third kD-Tree, count the number of neighboring points, and retain the point if the number of neighboring points is greater than the set threshold, thereby obtaining the point set after the second screening ; Point Set Perform point cloud clustering based on the Euclidean distance, select the point with the farthest distance in each cluster as the optimal feature point, and define the point set as the determined optimal feature point set, and each angular feature point in the point set is associated with an angle type.

[0012] As a further limitation of the first aspect of the present invention, perform discrimination between convex angles and concave angles for each angular feature point, including:[[]] Average point cloud The point A in it and its nearest neighbor point B in the point set constitute a vector , calculate the included angle formed by the vector and the direction vector of point A. When is greater than 90°, it is a concave angle; Average point cloud The point C in it and its nearest neighbor point D in the point set constitute a vector , calculate the included angle formed by the vector and the direction vector of point C. When is greater than 90°, it is a convex angle.

[0013] As a further limitation of the first aspect of the present invention, construct a triangle descriptor based on the angular feature points and the corresponding discrimination results of convex angles and concave angles, including:[[]] Use the fourth kD-Tree to organize and store the point set . For any current feature point in the point set , search for nearest neighbor points in the fourth kD-Tree, use the current feature point and the nearest neighbor points as vertices to construct triangle descriptors, perform uniqueness detection on all generated triangles, and remove redundant triangles with exactly the same vertex set; The triangle descriptor includes: the two-dimensional spatial coordinates of the three vertices, the lengths of the three sides arranged in ascending order, the key frame index to which the triangle descriptor belongs, and the angle types of the three vertices respectively.

[0014] As a further limitation of the first aspect of the present invention, determine multiple candidate loopback frames according to each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure, including:[[]] A hash table structure is adopted to organize and manage all triangle descriptors in historical key frames. The hash key is calculated using the three side lengths of each triangle descriptor, and the hash key is mapped to the corresponding position in the hash table. Each position stores a set of triangle descriptors with the same or less different side length structures, and the index of the historical key frame to which each descriptor belongs is recorded; For each triangle descriptor of the current key frame, the hash key is calculated using the three side lengths, and the corresponding position is searched in the hash table. Subsequently, all historical triangle descriptors stored in this position are traversed. For each historical triangle descriptor, the three side lengths are extracted and compared one by one with the three side lengths of the descriptor to be matched in the current frame. If the differences between the three sides are all within the set error tolerance, it is considered that the two descriptors are similar in geometric shape and form a set of valid matching pairs; Whenever a triangle descriptor of the current key frame successfully matches a triangle descriptor in a historical key frame, a vote is cast for this historical key frame. After all descriptors of the current key frame are processed, the cumulative number of votes of all historical key frames is counted, and the top N key frames with the most votes are selected as candidate loop closure frames; Calculate the index difference between the candidate key frame and the current key frame, eliminate the candidate key frames with a time interval less than the preset threshold, and filter out the candidate key frames that meet the time constraint as the final candidate loop closure frames.

[0015] As a further limitation of the first aspect of the present invention, loop closure verification is performed on each candidate key frame to determine the best loop closure frame, including: For all matched triangle descriptor pairs between the current key frame and the candidate key frame, consistency verification of angular characteristics is performed. If the angular characteristics between the corresponding vertices in the matched triangle descriptor pairs are inconsistent, it is considered that this match has a risk of false matching and is directly eliminated; For each pair of matched triangle descriptors, according to the matched vertex coordinates, singular value decomposition is used to calculate the two-dimensional rigid body transformation between the current key frame and the candidate key frame. The random sample consensus algorithm is used to find the two-dimensional rigid body transformation that can maximize the number of correctly matched triangle descriptors. Among all candidate key frames, the historical key frame with the largest number of triangle descriptors that are effectively matched with the current key frame is selected as the best loop closure frame.

[0016] In the second aspect, the present invention provides an indoor loop closure detection system based on angular features.

[0017] An indoor loop closure detection system based on angular features includes: A key frame construction unit configured to: construct key frame point clouds according to the odometry pose of the robot and the lidar point cloud; An average point cloud generation unit, configured to: perform height-direction filtering on the key-frame point cloud to generate an average point cloud; An angular feature point extraction unit, configured to: extract angular feature points from the average point cloud and discriminate convex angles and concave angles for each angular feature point; A descriptor construction unit, configured to: construct triangle descriptors based on the angular feature points and the corresponding convex angle and concave angle discrimination results, and uniformly manage the triangle descriptors in historical key frames by using a hash table structure; A candidate loop closure frame determination unit, configured to: determine multiple candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of historical key frames in the hash table structure; An initial pose estimation unit, configured to: perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the 2D rigid body transformation corresponding to the best loop closure frame as the initial pose estimation for point cloud registration between the current key frame and the best loop closure frame.

[0018] Compared with the prior art, the beneficial effects of the present invention are: 1. The present invention innovatively proposes an indoor loop closure detection method based on angular features, identifies angular features in indoor environment point clouds, extracts key points at the angular features and constructs triangle descriptors, uniformly manages the triangle descriptors in historical key frames by using a hash table structure, determines multiple candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of historical key frames in the hash table structure, and performs loop closure verification on each candidate key frame, realizing highly robust loop closure detection and improving the accuracy of loop closure detection.

[0019] 2. For indoor point cloud data collected by lidar, the present invention designs a highly robust angular feature extraction method, which can accurately identify stable angular features, including wall corners, door and window edges, etc. in indoor scenes with scarce textures, and uses these as significant features for indoor loop closure detection, improving the accuracy of loop closure detection.

[0020] 3. The present invention extracts key points at the angular features and constructs triangle descriptors in combination with the spatial relationships between multiple key points. This descriptor has strong structural stability and can achieve robust matching under different perspectives and occlusion conditions, thereby improving the accuracy and robustness of loop closure detection.

[0021] 4. Aiming at the problem that common concave angles (such as interior wall corners) and convex angles (such as exterior column corners) in indoor environments have different geometric characteristics, the present invention proposes a method for discriminating convex and concave angles, effectively improving the identification ability in the feature matching process and further reducing false matches.

[0022] 5. The triangle descriptor constructed in the present invention is completely based on spatial relationships and angular information, avoiding dependence on the lidar scanning order, resolution, or type. Therefore, it is applicable to various lidar types (including rotary lidar, solid-state lidar, etc.), and has good generality and engineering adaptability.

[0023] Advantages of additional aspects of the present invention will be given in part in the following description, become apparent in part from the following description, or be learned through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] The accompanying drawings forming a part of this specification are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention.

[0025] Figure 1 It is a schematic diagram of the overall framework of the indoor loop detection method based on angular features provided by the present invention; Figure 2 It is a schematic diagram of the sector division and the distribution of the average point cloud provided by the present invention; Figure 3 It is the average point cloud provided by the present invention distribution schematic diagram; Figure 4 It is a schematic diagram of the point set obtained after performing a radius search on the average point cloud provided by the present invention distribution schematic diagram; Figure 5 It is the average point cloud provided by the present invention and the point set in the distribution comparison schematic diagram of points located at angular features (such as corners) and non-angular features (such as walls). Among them, the black dots in (A) located at right angles (from the average point cloud ) and the black dots on the arc (from the point set ) will have a certain distance. Among them, the black dots in (B) from the point set and from the point set are almost coincident; Figure 6 It is a schematic diagram of the distribution of the point set provided by the present invention; Figure 7 It is a schematic diagram of the principle of extracting the optimal feature points provided by the present invention; Figure 8 It is a schematic diagram of concave angle determination provided by the present invention; Figure 9 It is a schematic diagram of convex angle determination provided by the present invention; Figure 10 It is a schematic diagram of an indoor loop detection system based on angular features provided by the present invention; Figure 11 A schematic diagram of the computer device provided by the present invention. Specific implementation manners

[0026] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0027] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0028] A large number of stable angular features generally exist in the indoor environment, including but not limited to the corners of walls, the edges of doors and windows, etc. These angular features are determined by the spatial structure and often appear as stable and recognizable geometric forms in the radar point cloud. However, most of the existing loop detection methods ignore the potential value of these angular features and pay limited attention to their role in enhancing indoor loop detection. In view of this, this implementation manner proposes an indoor loop detection method based on angular features, as Figure 1 shown. Taking the original point cloud collected by the lidar and its corresponding odometry pose as input, key frames are constructed by integrating consecutive multiple frames of radar scan data; subsequently, the key frame point cloud is filtered in the height direction and an average point cloud with stable structure is generated. On this basis, angular feature points with stable local geometric features are extracted from the average point cloud, and each feature point is discriminated between convex and concave angles to enhance the semantic discrimination ability of feature expression; then, triangle descriptors are constructed based on the angular feature points and managed uniformly using a hash table structure to support efficient retrieval of candidate key frames. For each candidate key frame, the system sequentially performs angular feature consistency determination and the RANSAC algorithm to complete the loop verification process; finally, the frame with the highest matching degree with the current key frame is selected from the candidate key frames as the best loop frame, and the rigid body transformation relationship between the two is estimated.

[0029] More specifically, it includes the following processes: S101: Cumulative key frames.

[0030] Considering the problem of sparse point cloud in a single-frame scan of the lidar in the indoor environment, in order to improve the stability of feature extraction and the construction quality of descriptors, the present invention introduces a key frame point cloud accumulation mechanism, that is, key frame point clouds are formed by integrating multiple consecutive frames of original radar scan data.

[0031] Specifically, the system continuously subscribes to the robot pose estimation from the odometry module and the original radar point cloud data collected by the lidar. Let the scan frame at the th moment be , where Denote the th 3D point obtained at time t, is the number of points in the point cloud of this scan frame, and the corresponding odometry pose is , that is, the homogeneous transformation matrix, is the special Euclidean group: (1); Among them, is the rotation matrix at time t, is the special orthogonal group, is the translation vector at time t.

[0032] In order to construct a key-frame point cloud , the present invention starts from a certain reference frame time and accumulates M frames of scan point cloud data. For each frame of point cloud data (where , and represents the timestamp of the i-th frame), according to its pose and the relative transformation relationship with the reference frame pose , the point cloud is transformed to the reference frame coordinate system: (2); Among them, is a point in the original radar point cloud, which belongs to the scan frame with timestamp , is the odometry pose of this frame; is the odometry pose of the reference frame (key frame); represents the point coordinates after transformation and located in the reference frame coordinate system.

[0033] Merge all the point clouds after pose transformation to form the complete key-frame point cloud : (3); This accumulation process can significantly improve the point cloud density of the key frame, which helps the subsequent accurate extraction of angular features and the stable construction of triangle descriptors.

[0034] S102: Generate the average point cloud.

[0035] When processing the key-frame point cloud, first perform height filtering on the point cloud data, that is, set a predefined height range based on the Z-axis direction (vertical height), and only retain the point cloud data within this height interval. This step aims to effectively exclude the interference of debris near the ground (such as furniture legs, scattered objects on the ground) and structures such as chandeliers and ventilation ducts at high places on feature extraction. At the same time, it can also significantly reduce the non-static point cloud introduced by dynamic objects such as pedestrians, thereby improving the stability and accuracy of subsequent angle feature recognition and descriptor construction.

[0036] Then, taking the radar center as the origin of the polar coordinate system, construct a two-dimensional polar coordinate system. Let the key-frame point cloud after height filtering be , where N represents the number of points, is the point 's three-dimensional coordinates in the current key-frame coordinate system. On the horizontal plane, evenly divide the 2π range into m sectors, and the corresponding angle for each sector is: (4); For any point , calculate its azimuth angle and assign it to the corresponding sector. The ID of the sector to which each point belongs is calculated as follows: (5); (6); Among them, , are the coordinate values of in the key-frame coordinate system, is its azimuth angle, α is the size of the azimuth angle of each sector, is the ID of the sector corresponding to this point, is the floor function.

[0037] Next, calculate the mean and standard deviation of the two-dimensional plane projection (i.e., ignoring the Z-axis) of the points in each sector to measure the geometric distribution characteristics of the points in this sector.

[0038] (7); (8); Among them, , are the coordinate values of the points in the sector, n is the number of points in the current sector, , are the average coordinates of the points in this sector, , are the standard deviations of the point coordinates in the x and y directions in this sector.

[0039] When the standard deviation , When they are respectively less than the set threshold values, it is considered that the points located in this sector are relatively concentrated. At this time, the mean point is added to the average point cloud set. After processing all sectors, at most one point is retained in each sector. Here, only the X and Y coordinates of the points are concerned. The finally obtained average point cloud is equivalent to compressing the three-dimensional point cloud into a dense contour on the horizontal plane, similar to the result of a 360° scan by a two-dimensional lidar, representing a horizontal slice of the indoor structure. The schematic diagram of sector division and the distribution of the average point cloud is as Figure 2 shown.

[0040] S103: Angle feature extraction based on multiple filtering and construction of triangle descriptors.

[0041] To improve the efficiency of point cloud nearest neighbor query, the present invention uses a kD-Tree space partitioning structure to organize and manage the average point cloud, and performs multiple filtering on the obtained average point cloud to extract stable angle features, which usually correspond to significant structures in the environment, such as the corners of walls or the edges of doors and windows, etc.

[0042] First, use the obtained average point cloud to construct a kD-Tree (i.e., the first kD-Tree). Assume that the schematic diagram of the distribution of the average point cloud is as Figure 3 shown, where the small dots represent the obtained average point cloud. For each point in the average point cloud, perform a radius search and calculate the average coordinates of all the nearest neighbor points within the radius, so as to obtain another point set with the same number of points as the average point cloud. The schematic diagram of the distribution of the point set is as Figure 4 shown. At the location of the wall, the distance between the average coordinate point in the point set and the corresponding point in the average point cloud will be very small, almost close to coincidence, as shown in Figure 5 (B); while at the location of the corner and other angle features, the distance between the average coordinate point in the point set and the corresponding point in the average point cloud will be relatively large, as shown in Figure 5 (A). The points in the point set will form an arc at the angle feature, and this arc is composed of multiple points.

[0043] To screen out the points located at the angle features, use the point set to construct another kD-Tree (i.e., the second kD-Tree), and then for the average point cloud Each point in it searches for a nearest neighbor point in the second kD-Tree. If the distance d between the two points is greater than a preset threshold, the point is retained, that is, the point is considered to be a point located at the angular feature, thereby obtaining the point set after the first screening. 。

[0044] Since there will inevitably be some noise points in the candidate point set , these points usually do not appear in clusters or only a very small number of points gather in the candidate point set , while the points located at the angular features usually gather with a relatively large number of points. Therefore, in order to exclude the interference of noise points, the point set after the first screening is used to construct a kD-Tree (i.e., the third kD-Tree). For each point in it, a radius search is performed, and the number of neighbor points is counted. If the number of neighbor points is greater than the threshold, the point is retained, that is, the point is considered to be a relatively stable point located at the angular feature, thereby obtaining the point set after the second screening , and the distribution schematic diagram of the point set is as shown in Figure 6 .

[0045] The points located at the angular features usually exist in a clustered form. In order to obtain a single most representative point, the present invention performs point cloud clustering based on the Euclidean distance on the candidate point set , and selects the point with the farthest distance d in each cluster as the optimal feature point. Here, d is the distance between the points in the average point cloud and their nearest neighbor points in the point set . Define the point set as the determined optimal feature point set, and the point set is the extracted angular feature point. The schematic diagram of the optimal feature point extraction is as shown in Figure 7 . The black dot located at the right angle in the figure and its nearest neighbor point (the gray dot located on the arc in the figure) in the point set have the maximum distance d in their respective clusters, so the black dots located at the right angle constitute the optimal feature point set.

[0046] In the actual indoor environment, significant angular change structures are common at the corners of walls, columns, door and window edges, etc., which can be divided into two categories: convex angles and concave angles. In the angular feature extraction stage, the present invention proposes a method for discriminating convex and concave angles to improve the structural integrity of feature expression and the robustness of loop detection.

[0047] Concave angle determination method: The schematic diagram of concave angle determination is as shown in Figure 8 . The point A in the average point cloud and its nearest neighbor point B in the point set constitute a vector , calculate its direction vector with point A to form the included angle : (9); When is greater than 90°, this angle is a concave angle.

[0048] Convex angle determination method: The schematic diagram for convex angle determination is as shown in Figure 9 The point C in the average point cloud and its nearest neighbor point D in the point set form a vector , calculate its direction vector with point C to form the included angle : (10); When is greater than 90°, this angle is a convex angle.

[0049] After completing the angle feature extraction and angle type discrimination, each angle feature point in the point set has been associated with a specific angle type (i.e., convex angle or concave angle), forming a semantic geometric feature set. On this basis, the present invention further uses the optimal feature points in the point set to construct triangle descriptors to enhance the spatial structure recognition ability in loop detection.

[0050] Specifically, the present invention uses the kD-Tree data structure to organize and store the point set . For each feature point in , search for n nearest neighbor points in the kD-Tree, and use this feature point and its nearest neighbor points as vertices to construct triangle descriptors. After the triangle descriptors are constructed, they are stored in the database ( Figure 1 as shown).

[0051] To avoid redundant triangles caused by repeatedly constructing the same vertex combinations between adjacent feature points, the present invention performs uniqueness detection on all generated triangles during the construction process and removes redundant triangles with exactly the same vertex sets. In addition, to improve the geometric reliability of the triangle descriptors, two structural constraints are further introduced: one is to perform angle filtering on approximately collinear triangles to eliminate triangles with unclear geometric shapes and lack of structural discriminability; the other is to introduce side length constraints to perform length filtering on triangles with too short or too long side lengths to exclude the risk of false matching caused by scale imbalance or scale anomaly.

[0052] The finally constructed triangle descriptor contains the following information: the two-dimensional spatial coordinates of the three vertices, the lengths of the three sides arranged in ascending order, the key frame index to which the descriptor belongs, and the corner types (i.e., convex or concave) of the three vertices respectively. Since the lengths of the three sides are sorted, the arrangement order of the triangle vertices can be uniquely determined. While maintaining geometric stability, this triangle descriptor incorporates local semantic information, significantly enhancing the robustness and matching accuracy of loop detection in indoor environments.

[0053] S104: Candidate key frame search.

[0054] To achieve fast matching of triangle descriptors and efficient retrieval of candidate key frames, the present invention uses a hash table structure to organize and manage all triangle descriptors in historical key frames. Since the lengths of the three sides of a triangle have good invariance under rotation and translation transformations and can stably reflect the geometric shape of the triangle, the present invention calculates a hash key using the lengths of the three sides of each descriptor and maps it to the corresponding position in the hash table. Each position stores a set of triangle descriptors with an approximate side length structure, and records the historical key frame index to which each descriptor belongs.

[0055] For each triangle descriptor of the current key frame, calculate a hash key using its three side lengths and look up the corresponding position in the hash table. Subsequently, traverse all the historical triangle descriptors stored at this position. For each historical triangle descriptor, extract its three side lengths and compare them one by one with the three side lengths of the descriptor to be matched in the current frame. If the differences between the three sides are all within the set error tolerance (i.e., the difference in each side length does not exceed a certain set threshold), it is considered that the two descriptors are similar in geometric shape and form a valid matching pair.

[0056] To further determine the candidate key frames for loops, the present invention designs a matching voting mechanism. Specifically, whenever a triangle descriptor of the current key frame successfully matches a triangle descriptor in a historical key frame, a vote is cast for this historical key frame. After all the descriptors of the current key frame are processed, the system will count the cumulative votes of all historical key frames and select the top N key frames with the most votes as candidate loop frames.

[0057] In addition, the present invention also introduces a time constraint filtering mechanism. On the basis of voting and sorting, further calculate the index difference between the candidate key frame and the current key frame, and eliminate the candidate key frames with a short time interval (i.e., the index difference is less than a preset threshold), so as to screen out the candidate key frames that meet the time constraints. The finally retained candidate key frames and the pairs of matching triangle descriptors between them and the current key frame will be sent as input to the subsequent loop verification module.

[0058] S105: Loop verification.

[0059] To eliminate the false positive loop detection results caused by the mis-matching of triangle descriptors, the present invention performs loop verification on each candidate key frame, including two stages: angular feature consistency determination and RANSAC geometric verification.

[0060] First, for all the matched triangle descriptor pairs between the current key frame and the candidate key frame, angular feature consistency verification is performed. Since in the present invention, geometric feature labels (convex angle or concave angle) are assigned to the three vertices of each triangle, if the angular features between the corresponding vertices in the matched triangle descriptor pair are inconsistent, i.e., one is a convex angle and the other is a concave angle, it is considered that this match has a high risk of mis-matching and is directly eliminated. This verification effectively enhances the structural consistency of the match pairs. Then, for each pair of matched triangle descriptors and , according to the matched vertex coordinates, the two-dimensional rigid body transformation between the current key frame and the candidate key frame is calculated using singular value decomposition (SVD) : (11); (12); (13); (14); (15); (16); Among them, is and the coordinates of the three vertices, is the centroid coordinates of the two triangles, is the covariance matrix, is the singular value matrix, which is a diagonal matrix, are the left and right singular vector matrices respectively, is the rotation transformation, is the translation transformation.

[0061] Next, the RANSAC algorithm is used to find a 2D rigid body transformation that can maximize the number of correctly matched triangle descriptors. Finally, among all candidate key frames, the historical key frame with the largest number of triangle descriptors that are effectively matched with the current key frame is selected as the best loop closure frame, and the corresponding 2D rigid body transformation is used as the initial pose estimate for the fine registration of the point clouds between the two frames. Since the ground is relatively flat in most indoor environments and the scanning device usually maintains a stable pose, the changes in the Z-axis direction and the Roll and Pitch angles between two frames are relatively small. Therefore, using the 2D rigid body transformation as the initial pose estimate for the fine registration of the point clouds is effective.

[0062] The above has elaborated in detail the indoor loop closure detection method based on angle features of this implementation manner. To facilitate better implementation of the above method of the embodiments of the present application, correspondingly, the system of the embodiments of the present application is provided below.

[0063] As Figure 10 shown, there is shown a system for indoor loop closure detection based on angle features provided by an exemplary embodiment of the present application, including: A key frame construction unit 201, configured to: construct key frame point clouds according to the odometry pose of the robot and the lidar point clouds; An average point cloud generation unit 202, configured to: perform height-direction filtering on the key frame point clouds to generate average point clouds; An angle feature point extraction unit 203, configured to: extract angle feature points from the average point clouds and discriminate convex angles and concave angles for each angle feature point; A descriptor construction unit 204, configured to: construct triangle descriptors based on the angle feature points and the corresponding convex angle and concave angle discrimination results, and uniformly manage the triangle descriptors in the historical key frames using a hash table structure; A candidate loop closure frame determination unit 205, configured to: determine multiple candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure; An initial pose estimation unit 206, configured to: perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the 2D rigid body transformation corresponding to the best loop closure frame as the initial pose estimate for the point cloud registration between the current key frame and the best loop closure frame.

[0064] The specific working methods of the above various units can be seen in the introduction in Embodiment 1 and will not be elaborated here.

[0065] It can be understood that the above-mentioned units can be separately or wholly combined into one or several other units, or some of them can be further split into multiple smaller units in terms of functions, which can achieve the same operations without affecting the realization of the technical effects of the embodiments of this application. The above units are divided based on logical functions. In practical applications, the function of one unit can also be realized by multiple units, or the functions of multiple units can be realized by one unit. In other embodiments of this application, the system may also include other units. In practical applications, these functions can also be assisted by other units and can be realized through the cooperation of multiple units.

[0066] According to another embodiment of this application, the system described in this embodiment can be constructed, and the method of Embodiment 1 of this application can be realized by running a computer program (including program code) that can execute the respective steps involved in the corresponding method described in Embodiment 1 on a general computing device such as a computer including processing elements and storage elements such as a central processing unit (CPU), a random access memory (RAM), and a read-only memory (ROM). The computer program can be recorded on a computer-readable recording medium, for example, and loaded into the above-mentioned computing device through the computer-readable recording medium and run therein.

[0067] Figure 11 The structural schematic diagram of a computer device provided by an exemplary embodiment of this application is shown. The electronic device includes a processor 301, a communication interface 302, and a computer-readable storage medium 303. Among them, the processor 301, the communication interface 302, and the computer-readable storage medium 303 can be connected through a bus or other means.

[0068] Among them, the communication interface 302 is used to receive and send data. The computer-readable storage medium 303 can be stored in the memory of the electronic device. The computer-readable storage medium 303 is used to store a computer program. The computer program includes program instructions. The processor 301 is used to execute the program instructions stored in the computer-readable storage medium 303.

[0069] The processor 301 (or CPU (Central Processing Unit, central processor)) is the computing core and control core of the electronic device, which is suitable for implementing one or more instructions, and is specifically suitable for loading and executing one or more instructions to realize the corresponding method flow or corresponding functions.

[0070] The processor 301 is configured to execute the following process: Construct key-frame point clouds based on the odometry pose of the robot and the lidar point cloud; Perform height-direction filtering on the key-frame point clouds to generate average point clouds; Extract angular feature points from the average point clouds and discriminate convex and concave angles for each angular feature point; Construct triangle descriptors based on the angular feature points and the corresponding convex and concave angle discrimination results, and use a hash table structure to uniformly manage the triangle descriptors in historical key frames; Determine multiple candidate loop closure frames based on each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure; Perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the 2D rigid body transformation corresponding to the best loop closure frame as the initial pose estimate for point cloud registration between the current key frame and the best loop closure frame.

[0071] The embodiment of the present application also provides a computer-readable storage medium (Memory). The computer-readable storage medium is a memory device in an electronic device for storing programs and data. It can be understood that the computer-readable storage medium here can include both the built-in storage medium in the electronic device and, of course, the extended storage medium supported by the electronic device. The computer-readable storage medium provides a storage space, and this storage space stores the processing system of the electronic device.

[0072] Moreover, one or more instructions suitable for being loaded and executed by a processor are also stored in this storage space. These instructions can be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here can be a high-speed RAM memory or a non-volatile memory, such as at least one disk memory; optionally, it can also be at least one computer-readable storage medium located far from the aforementioned processor.

[0073] In one embodiment, one or more instructions are stored in the computer-readable storage medium; the one or more instructions stored in the computer-readable storage medium are loaded and executed by the processor to implement the following process: Construct key-frame point clouds based on the odometry pose of the robot and the lidar point cloud; Perform height-direction filtering on the key-frame point clouds to generate average point clouds; Extract angular feature points from the average point clouds and discriminate convex and concave angles for each angular feature point; Construct triangle descriptors based on the angular feature points and the corresponding convex and concave angle discrimination results, and use a hash table structure to uniformly manage the triangle descriptors in historical key frames; Determine a plurality of candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure; Perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the two-dimensional rigid body transformation corresponding to the best loop closure frame as the initial pose estimate for point cloud registration between the current key frame and the best loop closure frame.

[0074] The embodiments of the present application also provide a computer program product or a computer program. The computer program product or the computer program includes computer instructions, and the computer instructions are stored in a computer-readable storage medium. The processor of the electronic device reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions to enable the electronic device to perform the following processes: Construct a key frame point cloud according to the odometry pose of the robot and the lidar point cloud; Perform height-direction filtering on the key frame point cloud to generate an average point cloud; Extract angular feature points from the average point cloud, and discriminate convex and concave angles for each angular feature point; Construct triangle descriptors based on the angular feature points and the corresponding convex and concave angle discrimination results, and use a hash table structure to uniformly manage the triangle descriptors in the historical key frames; Determine a plurality of candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure; Perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the two-dimensional rigid body transformation corresponding to the best loop closure frame as the initial pose estimate for point cloud registration between the current key frame and the best loop closure frame.

[0075] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed in the present application can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Skilled artisans can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of the present application.

[0076] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the processes or functions according to the embodiments of the present application are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium or transmitted through a computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center in a wired manner (such as coaxial cable, optical fiber, digital subscriber line (DSL)) or a wireless manner (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data processing device such as a server or a data center that includes one or more integrated available media. The available medium can be a magnetic medium (such as a floppy disk, a hard disk, a magnetic tape), an optical medium (such as a DVD), or a semiconductor medium (such as a solid state disk (SSD)), etc.

[0077] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An indoor loop detection method based on angular features, characterized in that It includes the following processes: Construct key-frame point clouds based on the odometry pose of the robot and the lidar point cloud; Perform height-direction filtering on the key-frame point clouds to generate average point clouds; Extract angular feature points from the average point clouds, and discriminate convex angles and concave angles for each angular feature point; Construct triangle descriptors based on the angular feature points and the corresponding convex-angle and concave-angle discrimination results, and use a hash table structure to uniformly manage the triangle descriptors in historical key frames; Determine multiple candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure; Perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the 2D rigid body transformation corresponding to the best loop closure frame as the initial pose estimate for point cloud registration between the current key frame and the best loop closure frame.

2. The indoor loop closure detection method based on angular features according to claim 1, characterized in that Constructing key-frame point clouds according to the odometry pose of the robot and the lidar point cloud includes: Starting from a certain reference frame time Start, accumulate Frame scan point cloud data. For each frame of point cloud data , where , and represents the time stamp of the frame. According to pose and the relative transformation relationship with the reference frame pose , convert the point cloud to the reference frame coordinate system: , where is a point in the original radar point cloud, is the odometry pose of this frame, is the odometry pose of the reference frame, represents the point coordinates after transformation and located in the reference frame coordinate system. Merge all the point clouds after pose transformation to form a complete key frame point cloud .

3. The indoor loop closure detection method based on angular features according to claim 1, characterized in that Performing height-direction filtering on the point clouds of the key frames to generate average point clouds includes: Taking the center of the lidar as the origin of the polar coordinate system, a two-dimensional polar coordinate system is constructed. Let the key-frame point cloud after height filtering be , and on the horizontal plane, is evenly divided into multiple sectors; For any point , calculate the azimuth angle and assign it to the corresponding sector; Calculate the mean and standard deviation on the two-dimensional plane projection of the point cloud in each sector. For any current sector, when and the standard deviations in the directions are respectively less than the corresponding set thresholds, add the mean of the points in the current sector to the average point cloud set. After processing all sectors, at most one point is retained in each sector as the final average point cloud.

4. The indoor loop closure detection method based on angular features according to claim 3, characterized in that On a horizontal plane, the range is evenly divided into sectors, and the angle corresponding to each sector is: ; For any point , calculate the azimuth angle , and assign to the corresponding sector. The ID of the sector it belongs to is: , where and are 's coordinate values in the key frame coordinate system, is 's azimuth angle, α is the size of the azimuth angle of each sector, is 's corresponding sector ID, is the floor function.

5. The indoor loop closure detection method based on angular features according to claim 1, characterized in that Extracting angular feature points from the average point clouds includes: Using the obtained average point cloud Construct a first kD-Tree, and perform a radius search for each point in the average point cloud Calculate the average coordinates of all neighboring points within the search radius of each point, and obtain another point set with the same number of points as the average point cloud ; ; Using a point set Construct a second kD-Tree, and then for each point in the average point cloud search for a nearest neighbor point in the second kD-Tree. If the distance between two points is greater than a preset threshold, then retain the point, and thus obtain the point set after the first screening ; Use the point set after the first screening Construct a third kD-Tree. For each point in the third kD-Tree, perform a radius search, count the number of neighboring points. If the number of neighboring points is greater than the set threshold, retain the point, thereby obtaining the point set after the second screening ; Point set Perform point cloud clustering based on Euclidean distance on the point set, select the point with the farthest distance in each cluster as the optimal feature point, and define the point set as the determined optimal feature point set. Each angular feature point in the point set is associated with an angular type.

6. The indoor loop closure detection method based on angular features according to claim 5, characterized in that Discriminating convex angles and concave angles for each angular feature point includes: Average point cloud The point A in it and its nearest neighbor point B in the point set form a vector . Calculate the included angle formed by the vector and the direction vector of point A . When is greater than 90°, it is a concave angle; Average point cloud The point C in it and its nearest neighbor point D in the point set form a vector . Calculate the included angle formed by the vector and the direction vector of point C . When is greater than 90°, it is a convex angle.

7. The indoor loop closure detection method based on angular features according to claim 5, characterized in that Constructing triangle descriptors based on the angular feature points and the corresponding convex-angle and concave-angle discrimination results includes: Use the fourth kD-Tree to organize and store the point set For any current feature point in the point set Search for the nearest neighbor points in the fourth kD-Tree, use the current feature point and the nearest neighbor points as vertices to construct triangle descriptors, perform uniqueness detection on all generated triangles, and remove redundant triangles with exactly the same vertex set; The triangle descriptor includes: the 2D spatial coordinates of the three vertices, the lengths of the three sides arranged in ascending order, the key-frame index to which the triangle descriptor belongs, and the angular types of the three vertices respectively.

8. The indoor loop closure detection method based on angular features according to any one of claims 1-7, characterized in that Determining multiple candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure includes: Use a hash table structure to organize and manage all triangle descriptors in historical key frames, calculate hash keys using the three side lengths of each triangle descriptor, map the hash keys to the corresponding positions in the hash table, each position stores a set of triangle descriptors with the same or less than a set threshold difference in side length structure, and record the historical key-frame index to which each descriptor belongs; For each triangle descriptor of the current key frame, calculate the hash key using the three side lengths, and look up the corresponding position in the hash table. Subsequently, traverse all the historical triangle descriptors stored at this position. For each historical triangle descriptor, extract the three side lengths and compare them one by one with the three side lengths of the descriptor to be matched in the current frame. If the differences between the three sides are all within the set error tolerance, it is considered that the two descriptors are geometrically similar and form a valid matching pair; Whenever a triangle descriptor of the current key frame successfully matches a triangle descriptor in a historical key frame, a vote is cast for this historical key frame. After all the descriptors of the current key frame are processed, count the cumulative number of votes for all historical key frames, and select the top N key frames with the most votes as candidate loop closure frames; Calculate the index difference between the candidate key frame and the current key frame, eliminate the candidate key frames with a time interval less than the preset threshold, and filter out the candidate key frames that meet the time constraint as the final candidate loop closure frames.

9. The indoor loop closure detection method based on angle features according to claim 8, characterized in that Perform loop closure verification on each candidate key frame to determine the best loop closure frame, including: Perform consistency verification of angular characteristics on all matched triangle descriptor pairs between the current key frame and the candidate key frame. If the angular characteristics between the corresponding vertices in the matched triangle descriptor pairs are inconsistent, it is considered that this match has a risk of false matching and is directly eliminated; For each pair of matched triangle descriptors, according to the matched vertex coordinates, use singular value decomposition to calculate the two-dimensional rigid body transformation between the current key frame and the candidate key frame, and use the random sample consensus algorithm to find the two-dimensional rigid body transformation that can maximize the number of correctly matched triangle descriptors. Among all candidate key frames, select the historical key frame with the largest number of effectively matched triangle descriptors with the current key frame as the best loop closure frame.

10. An indoor loop detection system based on angular features, characterized in that, Including: A key frame construction unit configured to: construct a key frame point cloud according to the odometry pose of the robot and the lidar point cloud; An average point cloud generation unit configured to: perform height-direction filtering on the key frame point cloud to generate an average point cloud; An angle feature point extraction unit configured to: extract angle feature points from the average point cloud and discriminate convex and concave angles for each angle feature point; A descriptor construction unit configured to: construct triangle descriptors based on the angle feature points and the corresponding convex and concave angle discrimination results, and use a hash table structure to uniformly manage the triangle descriptors in historical key frames; A candidate loop closure frame determination unit configured to: determine multiple candidate loop closure frames according to each triangle descriptor of the current key frame and the triangle descriptors of historical key frames in the hash table structure; An initial pose estimation unit configured to: perform loop closure verification on each candidate key frame to determine the best loop closure frame, and use the two-dimensional rigid body transformation corresponding to the best loop closure frame as the initial pose estimation for point cloud registration between the current key frame and the best loop closure frame.

Images