Indoor loop detection method and system based on angle features

Abstract

The present invention belongs to the technical field of mobile robot positioning and mapping. A method and system for indoor loop detection based on angle features is provided. Keyframe point clouds are subjected to highly directional filtering to generate an average point cloud. Angle feature points are extracted from the average point cloud. Convex and concave angles are discriminated for each angle feature point. A triangle descriptor is constructed based on the angle feature points and the corresponding convex and concave angle discrimination results. A hash table structure is used to uniformly manage triangle descriptors in historical keyframes. Multiple candidate loop frames are determined based on each triangle descriptor of the current keyframe and the triangle descriptors of the historical keyframes in the hash table structure. Loop verification is performed on each candidate keyframe to determine the optimal loop frame. The two-dimensional rigid body transformation corresponding to the optimal loop frame is used as the initial pose estimate for point cloud registration between the current keyframe and the optimal loop frame, thereby achieving highly robust loop detection.

Description

Technical Field

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

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

[0003] With the development of lidar technology, laser SLAM (Simultaneous Localization and Mapping) has become a core technology in fields such as mobile robotics, autonomous driving, and indoor mapping. SLAM systems use sensors to perceive the surrounding environment and achieve real-time positioning and mapping in unknown spaces. To ensure map consistency and prevent the spread of accumulated errors, loop closure detection, a key module in SLAM systems, identifies historically visited locations and corrects accumulated errors, thereby optimizing the overall trajectory and map structure.

[0004] Current mainstream lidar loop detection methods can be roughly divided into two categories: one is scan matching methods based on global descriptors (such as Scan Context and M2DP), which construct a global feature representation of the scene and calculate the similarity between features to determine loop relationships; the other is methods based on local geometric feature extraction, such as edges or planes. These methods rely on feature extraction and registration of structurally significant areas to achieve loop detection. These methods have good performance in outdoor environments with clear structures and wide fields of view. However, in indoor scenes, these methods also face challenges due to factors such as closed spaces, strong structural repetitiveness, and multiple occlusions. Global descriptors are often prone to mismatches in scenes such as corridors and open office areas, while local features lack stability and recognition due to their simple structure, resulting in reduced robustness and accuracy of loop detection. Summary of the Invention

[0005] In order to address the shortcomings of the existing technology, the present invention provides an indoor loop closure detection method and system based on angular features, which identifies angular features in the point cloud of indoor environments, extracts key points at the angular features and constructs triangle descriptors, thereby achieving highly robust loop closure detection and improving the accuracy of loop closure detection.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] In a first aspect, the present invention provides an indoor loop detection method based on angle features.

[0008] An indoor loop detection method based on angle features includes the following steps:

[0009] Construct keyframe point cloud based on the robot's odometry pose and lidar point cloud;

[0010] Perform high-direction filtering on the keyframe point cloud to generate an average point cloud;

[0011] Extract angle feature points from the average point cloud and distinguish convex angles from concave angles for each angle feature point;

[0012] A triangle descriptor is constructed based on the angle feature points and the corresponding convex and concave angle discrimination results, and a hash table structure is used to uniformly manage the triangle descriptors in the historical key frames.

[0013] Determine multiple candidate loop 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;

[0014] A loop closure verification is performed on each candidate keyframe to determine the best loop closure frame, and the two-dimensional rigid body transformation corresponding to the best loop closure frame is used as the initial pose estimate for point cloud registration between the current keyframe and the best loop closure frame.

[0015] As a further limitation of the first aspect of the present invention, constructing a keyframe point cloud based on the robot's odometry pose and the lidar point cloud includes:

[0016] From a reference frame moment Start, accumulate Frame scanning point cloud data, for each frame of point cloud data ,in, ,and Indicates the The timestamp of the frame, according to Posture Pose with reference frame The relative transformation relationship is used to transform the point cloud into the reference frame coordinate system: ,in, is a point in the original radar point cloud, is the odometry pose of the frame, is the odometry pose of the reference frame, Represents the coordinates of the point in the reference frame coordinate system after transformation. All point clouds after pose transformation are merged to form a complete keyframe point cloud. .

[0017] As a further limitation of the first aspect of the present invention, performing height directional filtering on the point cloud of the key frame to generate an average point cloud includes:

[0018] Taking the center of the laser radar as the polar coordinate origin, a two-dimensional polar coordinate system is constructed, and the key frame point cloud after height filtering is set as , on the horizontal plane The range is evenly divided into multiple sectors;

[0019] For any point , calculate the azimuth and assign it to the corresponding sector;

[0020] 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 processing all sectors, at most one point is retained in each sector as the final average point cloud.

[0021] 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: ;

[0022] 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.

[0023] As a further limitation of the first aspect of the present invention, extracting angular feature points from the average point cloud includes:

[0024] 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 to obtain the average point cloud Another point set with the same number of points ;

[0025] 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, and the point set after the first screening is obtained. ;

[0026] 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 second filtered point set. ;

[0027] Point Set Perform point cloud clustering based on Euclidean distance, select the point with the longest distance in each cluster as the optimal feature point, and define the point set To determine the optimal feature point set, point set Each angle feature point in is associated with an angle type.

[0028] As a further limitation of the first aspect of the present invention, distinguishing between a convex angle and a concave angle for each angular feature point includes:

[0029] Average point cloud Point A in the point set The nearest neighbor point B in the vector , calculate the vector Direction vector to point A The angle of the composition ,when When it is greater than 90°, it is a concave angle;

[0030] Average point cloud Point C in the point set The nearest neighbor point D in the vector , calculate the vector Direction vector to point C The angle of the composition ,when When it is greater than 90°, it is a convex angle.

[0031] As a further limitation of the first aspect of the present invention, constructing a triangle descriptor based on the angle feature points and the corresponding convex angle and concave angle discrimination results includes:

[0032] Use the fourth kD-Tree to point set For point set organization and storage Any current feature point in the fourth kD-Tree is searched The current feature point and the nearest neighbor point are used as vertices to construct a triangle descriptor, and a uniqueness check is performed on all generated triangles to remove redundant triangles with exactly the same vertex set.

[0033] The triangle descriptor includes: the two-dimensional space coordinates of the three vertices, the lengths of the three sides in ascending order, the keyframe index to which the triangle descriptor belongs, and the angle type of each of the three vertices.

[0034] As a further limitation of the first aspect of the present invention, determining multiple candidate loop 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 includes:

[0035] A hash table structure is used to organize and manage all triangle descriptors in historical keyframes. The hash key is calculated using the three side lengths of each triangle descriptor and mapped to the corresponding position in the hash table. Each position stores a set of triangle descriptors with the same side length structure or a difference less than a set threshold, and records the historical keyframe index to which each descriptor belongs.

[0036] For each triangle descriptor in the current keyframe, the hash key is calculated using the three side lengths, and the corresponding position is found in the hash table. Then, all historical triangle descriptors stored at that 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 difference between the three sides is within the set error tolerance, the two descriptors are considered to be geometrically similar and constitute a valid matching pair.

[0037] Whenever a triangle descriptor of the current keyframe successfully matches a triangle descriptor in a historical keyframe, a vote is cast for the historical keyframe. After all descriptors of the current keyframe are processed, the cumulative votes of all historical keyframes are counted, and the top N keyframes with the most votes are selected as candidate loop frames.

[0038] The index difference between the candidate key frame and the current key frame is calculated, and the candidate key frames with a time interval less than the preset threshold are eliminated. The candidate key frames that meet the time constraint are screened out as the final candidate loop frames.

[0039] As a further limitation of the first aspect of the present invention, performing loop verification on each candidate key frame to determine the best loop frame includes:

[0040] Verify the consistency of the angular characteristics of all matched triangle descriptor pairs between the current keyframe and the candidate keyframe. If the angular characteristics of the corresponding vertices in the matched triangle descriptor pairs are inconsistent, the match is considered to have a mismatch risk and is directly eliminated.

[0041] For each pair of matching triangle descriptors, singular value decomposition is used to calculate the two-dimensional rigid body transformation between the current keyframe and the candidate keyframe based on the matching vertex coordinates. A random sampling consensus algorithm is used to find the two-dimensional rigid body transformation that can maximize the number of correctly matching triangle descriptors. Among all candidate keyframes, the historical keyframe with the largest number of triangle descriptors that effectively match the current keyframe is selected as the best loop frame.

[0042] In a second aspect, the present invention provides an indoor loop detection system based on angle features.

[0043] An indoor loop detection system based on angle features, comprising:

[0044] The key frame construction unit is configured to: construct a key frame point cloud according to the robot's odometry pose and the lidar point cloud;

[0045] The average point cloud generation unit is configured to: perform height direction filtering on the key frame point cloud to generate an average point cloud;

[0046] The angle feature point extraction unit is configured to: extract angle feature points from the average point cloud, and distinguish convex angles from concave angles for each angle feature point;

[0047] The descriptor construction unit is configured to: construct a triangle descriptor based on the angle 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 the historical key frames;

[0048] The candidate loop frame determining unit is configured to: determine a plurality of candidate loop 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;

[0049] The initial pose estimation unit is configured to: perform loop closure verification on each candidate key frame, 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 alignment between the current key frame and the best loop closure frame.

[0050] Compared with the prior art, the present invention has the following beneficial effects:

[0051] 1. The present invention innovatively proposes an indoor loop detection method based on angle features, identifies angle features in indoor environment point clouds, extracts key points at the angle features and constructs triangle descriptors, and uses a hash table structure to uniformly manage triangle descriptors in historical key frames. Based on each triangle descriptor of the current key frame and the triangle descriptors of historical key frames in the hash table structure, multiple candidate loop frames are determined, and loop verification is performed on each candidate key frame, thereby achieving highly robust loop detection and improving the accuracy of loop detection.

[0052] 2. For indoor point cloud data collected by lidar, the present invention designs a highly robust angle feature extraction method, which can accurately identify stable angle features in texture-poor indoor scenes, including wall corners, door and window corners, etc., and use them as significant features for indoor loop detection, thereby improving the accuracy of loop detection.

[0053] 3. The present invention extracts key points at the angle features and constructs a triangle descriptor based on the spatial relationship between multiple key points. The descriptor has strong structural stability and can achieve robust matching under different viewing angles and occlusion conditions, thereby improving the accuracy and robustness of loop detection.

[0054] 4. To address the problem that concave corners (such as inner corners of walls) and convex corners (such as outer corners of columns) commonly found in indoor environments have different geometric characteristics, the present invention proposes a method for distinguishing concave and convex corners, which effectively improves the recognition capability during feature matching and further reduces mismatches.

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

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

[0057] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0058] Figure 1 Schematic diagram of the overall framework of the indoor loop detection method based on angle features provided by the present invention;

[0059] Figure 2 Schematic diagram of sector division and average point cloud distribution provided by the present invention;

[0060] Figure 3 The average point cloud provided by the present invention Schematic diagram of the distribution of;

[0061] Figure 4 The point set obtained by performing radius search on the average point cloud provided by the present invention Schematic diagram of the distribution of;

[0062] Figure 5The average point cloud provided by the present invention Hedianji Schematic diagram of the distribution comparison of points at angle features (such as corners) and non-angle features (such as walls), where (A) is a black point at a right angle (from the average point cloud ) and the black points on the arc (from the point set ) There will be a distance between them, where (B) comes from the point set and from the point set The black spots almost overlap;

[0063] Figure 6 Point set provided by the present invention Schematic diagram of the distribution of;

[0064] Figure 7 Schematic diagram of the optimal feature point extraction principle provided by the present invention;

[0065] Figure 8 A schematic diagram of concave angle determination provided by the present invention;

[0066] Figure 9 A schematic diagram of convex angle determination provided by the present invention;

[0067] Figure 10 A schematic diagram of an indoor loop detection system based on angle features provided by the present invention;

[0068] Figure 11 A schematic diagram of the computer device provided by the present invention. DETAILED DESCRIPTION

[0069] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0070] It should be noted that the following detailed descriptions are exemplary and 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 skilled in the art to which the present invention belongs.

[0071] There are a large number of stable angular features in indoor environments, including but not limited to corners of walls, 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 radar point clouds. However, most 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 proposes an indoor loop detection method based on angular features, such as Figure 1As shown in the figure, the system uses the raw point cloud collected by the lidar and its corresponding odometry pose as input, and constructs keyframes by integrating multiple frames of continuous lidar scan data. Subsequently, the keyframe point cloud is subjected to highly directional filtering to generate a structurally stable average point cloud. On this basis, angular feature points with stable local geometric features are extracted from the average point cloud, and each feature point is distinguished between convex and concave angles to enhance the semantic differentiation ability of the feature expression. Next, a triangle descriptor is constructed based on the angular feature points and uniformly managed using a hash table structure to support efficient candidate keyframe retrieval. For each candidate keyframe, the system sequentially performs angular feature consistency determination and the RANSAC algorithm to complete the loop closure verification process. Finally, the frame with the highest degree of match with the current keyframe is selected from the candidate keyframes as the best loop closure frame, and the rigid body transformation relationship between the two is estimated.

[0072] More specifically, the following processes are involved:

[0073] S101: Accumulating key frames.

[0074] Taking into account the problem of sparse point clouds in single-frame scanning of lidar in indoor environments, in order to improve the stability of feature extraction and the quality of descriptor construction, the present invention introduces a keyframe point cloud accumulation mechanism, that is, forming a keyframe point cloud by integrating multiple frames of continuous original radar scanning data.

[0075] Specifically, the system continuously subscribes to the robot pose estimation from the odometer module and the raw radar point cloud data collected by the lidar. The scanning frame at time ,in, Indicates the first three-dimensional points, is the number of points in the point cloud of the scan frame, and the corresponding odometer pose is , that is, the homogeneous transformation matrix, is a special Euclidean group:

[0076] (1);

[0077] in, is the rotation matrix at time t, is a special orthogonal group, is the translation vector at time t.

[0078] To construct a keyframe point cloud , the present invention starts from a certain reference frame moment Start by accumulating M frames of scanned point cloud data. For each frame of point cloud data (in, ,and represents the timestamp of the i-th frame), according to its pose Pose with reference frame The relative transformation relationship is used to transform the point cloud into the reference frame coordinate system:

[0079] (2);

[0080] in, is a point in the original radar point cloud, which has a timestamp of The scan frame, is the odometry pose of the frame; is the odometry pose of the reference frame (keyframe); Indicates the coordinates of the point in the reference frame coordinate system after transformation.

[0081] Merge all the point clouds after pose transformation to form a complete keyframe point cloud :

[0082] (3);

[0083] This accumulation process can significantly improve the point cloud density of the keyframe, which helps in the subsequent accurate extraction of angle features and the stable construction of triangle descriptors.

[0084] S102: Generate an average point cloud.

[0085] When processing the keyframe point cloud, the point cloud data is first subjected to height filtering, that is, a predefined height range is set based on the Z-axis direction (vertical height), and only the point cloud data within this height range is retained. This step is intended to effectively eliminate the interference of debris close to the ground (such as furniture legs, scattered objects on the ground) and structures such as chandeliers and ventilation ducts located at high places on feature extraction. At the same time, it can also significantly reduce the non-static point clouds introduced by dynamic objects such as pedestrians, thereby improving the stability and accuracy of subsequent angular feature recognition and descriptor construction.

[0086] Then, a two-dimensional polar coordinate system is constructed with the center of the radar as the origin of the polar coordinates. Assume that the key frame point cloud after height filtering is , where N represents the number of points, for point The three-dimensional coordinates in the current keyframe coordinate system. The 2π range is evenly divided into m sectors on the horizontal plane, and the angle corresponding to each sector is:

[0087] (4);

[0088] For any point , calculate its azimuth and assign it to the corresponding sector. The ID of the sector to which each point belongs is calculated as follows:

[0089] (5);

[0090] (6);

[0091] in, 、 for The coordinate value in the key frame coordinate system, is its azimuth, α is the size of the azimuth of each sector, is the sector ID corresponding to the point, is the floor function.

[0092] Next, the mean and standard deviation of the point cloud in each sector on its two-dimensional plane projection (i.e., ignoring the Z axis) are calculated to measure the geometric distribution characteristics of the points in the sector.

[0093] (7);

[0094] (8);

[0095] in, 、 is the coordinate value of the point in the sector, n is the number of points in the current sector, 、 is the average coordinate of the points in the sector, 、 is the standard deviation of the point coordinates in the sector in the x and y directions.

[0096] When the standard deviation 、 When the points are less than the set threshold, it is considered that the points in this sector are concentrated. Add the average point cloud set. After processing all sectors, at most one point is retained in each sector. Here we only focus on the X and Y coordinates of the point. The resulting average point cloud is equivalent to compressing the 3D point cloud into a dense contour on the horizontal plane, similar to the result of a 360° scan by a 2D LiDAR, representing a horizontal slice of the indoor structure. The diagram of sector division and average point cloud distribution is shown below. Figure 2 shown.

[0097] S103: Angle feature extraction and triangle descriptor construction based on multiple filtering.

[0098] In order to improve the efficiency of point cloud neighbor query, the present invention adopts the kD-Tree spatial partitioning structure to organize and manage the average point cloud, and filters the obtained average point cloud multiple times to extract stable angular features. These features usually correspond to significant structures in the environment, such as corners of walls or edges of doors and windows.

[0099] First, the average point cloud obtained is used Construct a kD-Tree (i.e. the first kD-Tree), assuming the average point cloud The distribution diagram of Figure 3 As shown, the small dots represent the average point cloud obtained. For each point in the average point cloud, a radius search is performed and the average coordinates of all neighboring points within the radius are calculated. In this way, another point set with the same number of points as the average point cloud is obtained. , point set The distribution diagram of Figure 4 As shown. At the wall, the point set The average coordinate point in the corresponding average point cloud The distance between the points in will be very small, almost close to coincidence, such as Figure 5 As shown in (B); at the corners and other angle features, the point set The average coordinate point in the corresponding average point cloud The distance between the points in will be relatively large, such as Figure 5 As shown in (A), the point set The points in the will form an arc at the angle feature, which is composed of multiple points.

[0100] In order to filter out the points located at the angle features, the point set Build another kD-Tree (the 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 d between the two points is greater than the preset threshold, the point is retained, that is, the point is considered to be located at the angle feature, thereby obtaining the point set after the first screening. .

[0101] Since the candidate point set There are inevitably some noise points in the candidate point set. In the image, there are only a few points that do not appear in clusters or only a few points are clustered, while the points located at the angle features are usually clustered with relatively more points. Therefore, in order to eliminate the interference of noise points, the point set after the first screening is used Construct a kD-Tree (the third kD-Tree), perform a radius search for each point in it, count the number of neighboring points, and retain the point if the number of neighboring points is greater than the threshold. That is, the point is considered to be a relatively stable point at the angle feature, thus obtaining the point set after the second screening. , point set The distribution diagram of Figure 6 shown.

[0102] Points located at angle features usually exist in clusters. In order to obtain the most representative single point, the present invention performs a multi-step process on the candidate point set. Perform point cloud clustering based on Euclidean distance, and select the point with the longest distance d in each cluster as the optimal feature point, where d is the average point cloud mentioned above The points in the point set The distance between the nearest neighbor points in the point set is defined as To determine the optimal feature point set, point set This is the extracted angle feature point. The optimal feature point extraction principle diagram is as follows Figure 7 As shown, the black dot at the right angle in the figure and its The nearest neighbor point in (the gray dots on the arc in the figure) has the largest distance d in the cluster to which it belongs, so the black dots at the right angles constitute the optimal feature point set.

[0103] In real-world indoor environments, significant angular variations are common at corners of walls, columns, doors, and windows. These can be categorized as convex and concave angles. This paper proposes a convex and concave angle discrimination method during the angular feature extraction phase to improve the structural integrity of feature representation and the robustness of loop detection.

[0104] Concave angle determination method: Concave angle determination diagram is as follows Figure 8 As shown, the average point cloud Point A in the point set The nearest neighbor point B in the vector , calculate its direction vector with point A The angle of the composition :

[0105] (9);

[0106] when When the angle is greater than 90°, the angle is considered concave.

[0107] Convex angle determination method: Convex angle determination diagram is as follows Figure 9 As shown, the average point cloud Point C in the point set The nearest neighbor point D in the vector , calculate its direction vector with point C The angle of the composition :

[0108] (10);

[0109] when When it is greater than 90°, the angle is considered convex.

[0110] After completing angle feature extraction and angle type identification, the point set Each angle feature point in is associated with a specific angle type (i.e. convex angle or concave angle), forming a semantic geometric feature set. The optimal feature points in the image are used to construct a triangle descriptor to enhance the spatial structure recognition ability in loop closure detection.

[0111] Specifically, the present invention uses kD-Tree data structure to For organization and storage. For each feature point in the kD-Tree, search for n nearest neighbor points, use the feature point and its nearest neighbor points as vertices to construct a triangle descriptor, and store the triangle descriptor in the database after it is constructed. Figure 1 shown).

[0112] To avoid redundant triangles caused by repeatedly constructing the same vertex combination between adjacent feature points, the present invention performs uniqueness checks on all generated triangles during the construction process, removing redundant triangles with identical vertex sets. Furthermore, to improve the geometric reliability of the triangle descriptor, two structural constraints are introduced: first, angle filtering is performed on nearly collinear triangles to eliminate triangles with unclear geometric shapes and lacking structural discernibility; second, side length constraints are introduced to filter triangles with sides that are too short or too long, eliminating the risk of mismatches due to disproportionate or scale anomalies.

[0113] The resulting triangle descriptor contains the following information: the 2D coordinates of the three vertices, the lengths of the three edges in ascending order, the keyframe index to which the descriptor belongs, and the angle type (convex or concave) of each vertex. Because the three edge lengths are sorted, the order of the triangle vertices can be uniquely determined. This triangle descriptor maintains geometric stability while incorporating local semantic information, significantly improving the robustness and matching accuracy of loop closure detection in indoor environments.

[0114] S104: Search for candidate key frames.

[0115] In order to achieve fast matching of triangle descriptors and efficient retrieval of candidate key frames, the present invention adopts a hash table structure to organize and manage all triangle descriptors in historical key frames. Since the three side lengths of a triangle have good invariance under rotation and translation transformations and can stably reflect the geometric shape of the triangle, the present invention uses the three side lengths of each descriptor to calculate the hash key and maps it to the corresponding position of 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.

[0116] For each triangle descriptor in the current keyframe, a hash key is calculated using its three side lengths, and the corresponding position is searched in the hash table. Then, all historical triangle descriptors stored at that position are traversed. For each historical triangle descriptor, its 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 difference between the three sides is within the set error tolerance (i.e., the difference in each side length does not exceed a set threshold), the two descriptors are considered geometrically similar and constitute a valid matching pair.

[0117] To further identify candidate keyframes for loop closure, the present invention designs a matching voting mechanism. Specifically, whenever a triangle descriptor in the current keyframe successfully matches a triangle descriptor in a previous keyframe, a vote is cast for that previous keyframe. After all descriptors of the current keyframe have been processed, the system calculates the cumulative votes of all previous keyframes and selects the top N keyframes with the most votes as candidate loop frames.

[0118] Furthermore, the present invention introduces a time-constrained filtering mechanism. Based on the voting ranking, the index difference between the candidate keyframes and the current keyframe is further calculated, and candidate keyframes with a short time interval (i.e., an index difference less than a preset threshold) are eliminated. This method then filters out candidate keyframes that meet the time constraint. The remaining candidate keyframes and their matching triangle descriptor pairs with the current keyframe are then fed into the subsequent loop verification module.

[0119] S105: Loopback verification.

[0120] In order to eliminate false positive loop detection results caused by mismatching of triangle descriptors, the present invention performs loop verification on each candidate keyframe, including two stages: corner feature consistency determination and RANSAC geometry verification.

[0121] First, the consistency of angular characteristics of all matched triangle descriptor pairs between the current keyframe and the candidate keyframe is verified. Since the three vertices of each triangle in the present invention are assigned geometric feature labels (convex or concave), if the angular characteristics of the corresponding vertices in the matched triangle descriptor pair are inconsistent, that is, one is convex and the other is concave, the match is considered to have a high risk of mismatch and is directly rejected. This verification effectively enhances the structural consistency of the matching pairs.

[0122] Then, for each pair of matching triangle descriptors and , based on the matched vertex coordinates, singular value decomposition (SVD) is used to calculate the two-dimensional rigid body transformation between the current key frame and the candidate key frame :

[0123] (11);

[0124] (12);

[0125] (13);

[0126] (14);

[0127] (15);

[0128] (16);

[0129] in, for and The coordinates of the three vertices, are the centroid coordinates of the two triangles, is the covariance matrix, is a singular value matrix, which is a diagonal matrix. are the left and right singular vector matrices, is the rotation transformation, is a translation transformation.

[0130] Next, the RANSAC algorithm is used to find a 2D rigid body transformation that maximizes the number of correctly matched triangle descriptors. Ultimately, among all candidate keyframes, the historical keyframe with the largest number of effectively matched triangle descriptors is selected as the optimal loopback frame, and its corresponding 2D rigid body transformation is used as the initial pose estimate for fine point cloud registration between the two frames. Because the ground in most indoor environments is relatively flat and the scanning device typically maintains a stable posture, the changes between the two frames in the Z-axis direction and the roll and pitch angles are relatively small. Therefore, using a 2D rigid body transformation as the initial pose estimate for fine point cloud registration is effective.

[0131] The above describes in detail the indoor loop detection method based on angle features of the present implementation. In order to facilitate better implementation of the above method of the embodiment of the present application, the system of the embodiment of the present application is provided below accordingly.

[0132] like Figure 10 FIG. 1 shows an indoor loop detection system based on angle features provided by an exemplary embodiment of the present application, including:

[0133] The key frame construction unit 201 is configured to: construct a key frame point cloud based on the robot's odometry pose and the lidar point cloud;

[0134] The average point cloud generating unit 202 is configured to: perform height direction filtering on the key frame point cloud to generate an average point cloud;

[0135] The angle feature point extraction unit 203 is configured to: extract angle feature points from the average point cloud, and distinguish convex angles from concave angles for each angle feature point;

[0136] The descriptor construction unit 204 is configured to: construct a triangle descriptor based on the angle 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 the historical key frames;

[0137] The candidate loop frame determining unit 205 is configured to: determine a plurality of candidate loop 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;

[0138] The initial pose estimation unit 206 is configured to perform loop closure verification on each candidate key frame, 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.

[0139] The specific working methods of the above-mentioned units are described in Example 1 and will not be repeated here.

[0140] It is understandable that each of the above-mentioned units can be separately or completely combined into one or several other units to constitute, or one (or some) of the units can be further divided into multiple functionally smaller units to constitute, which can achieve the same operation without affecting the realization of the technical effects of the embodiments of the present application. The above-mentioned units are divided based on logical functions. In actual applications, the functions of one unit can also be implemented by multiple units, or the functions of multiple units can be implemented by one unit. In other embodiments of the present application, the system may also include other units. In actual applications, these functions can also be implemented with the assistance of other units, and can be implemented by the collaboration of multiple units.

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

[0142] Figure 11 The figure shows a schematic diagram of a computer device provided by an exemplary embodiment of the present application, which includes a processor 301, a communication interface 302, and a computer-readable storage medium 303. The processor 301, the communication interface 302, and the computer-readable storage medium 303 can be connected via a bus or other means.

[0143] 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 computer programs, the computer programs include program instructions, and the processor 301 is used to execute the program instructions stored in the computer-readable storage medium 303.

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

[0145] The processor 301 is configured to perform the following process:

[0146] Construct keyframe point cloud based on the robot's odometry pose and lidar point cloud;

[0147] Perform high-direction filtering on the keyframe point cloud to generate an average point cloud;

[0148] Extract angle feature points from the average point cloud and distinguish convex angles from concave angles for each angle feature point;

[0149] A triangle descriptor is constructed based on the angle feature points and the corresponding convex and concave angle discrimination results, and a hash table structure is used to uniformly manage the triangle descriptors in the historical key frames.

[0150] Determine multiple candidate loop 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;

[0151] A loop closure verification is performed on each candidate keyframe to determine the best loop closure frame, and the two-dimensional rigid body transformation corresponding to the best loop closure frame is used as the initial pose estimate for point cloud registration between the current keyframe and the best loop closure frame.

[0152] The present application also provides a computer-readable storage medium (Memory). This computer-readable storage medium is a memory device in an electronic device that is used to store programs and data. It is understood that the computer-readable storage medium herein may include both built-in storage media in the electronic device and, of course, extended storage media supported by the electronic device. The computer-readable storage medium provides storage space that stores the processing system of the electronic device.

[0153] Furthermore, the storage space also stores one or more instructions suitable for being loaded and executed by the processor. These instructions may be one or more computer programs (including program code). It should be noted that the computer-readable storage medium herein may be a high-speed RAM memory or a non-volatile memory, such as at least one disk storage device; alternatively, it may be at least one computer-readable storage medium located remotely from the processor.

[0154] In one embodiment, the computer-readable storage medium stores one or more instructions; the processor loads and executes the one or more instructions stored in the computer-readable storage medium to implement the following process:

[0155] Construct keyframe point cloud based on the robot's odometry pose and lidar point cloud;

[0156] Perform high-direction filtering on the keyframe point cloud to generate an average point cloud;

[0157] Extract angle feature points from the average point cloud and distinguish convex angles from concave angles for each angle feature point;

[0158] A triangle descriptor is constructed based on the angle feature points and the corresponding convex and concave angle discrimination results, and a hash table structure is used to uniformly manage the triangle descriptors in the historical key frames.

[0159] Determine multiple candidate loop 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;

[0160] A loop closure verification is performed on each candidate keyframe to determine the best loop closure frame, and the two-dimensional rigid body transformation corresponding to the best loop closure frame is used as the initial pose estimate for point cloud registration between the current keyframe and the best loop closure frame.

[0161] The present application also provides a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of an electronic device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the electronic device to perform the following process:

[0162] Construct keyframe point cloud based on the robot's odometry pose and lidar point cloud;

[0163] Perform high-direction filtering on the keyframe point cloud to generate an average point cloud;

[0164] Extract angle feature points from the average point cloud and distinguish convex angles from concave angles for each angle feature point;

[0165] A triangle descriptor is constructed based on the angle feature points and the corresponding convex and concave angle discrimination results, and a hash table structure is used to uniformly manage the triangle descriptors in the historical key frames.

[0166] Determine multiple candidate loop 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;

[0167] A loop closure verification is performed on each candidate keyframe to determine the best loop closure frame, and the two-dimensional rigid body transformation corresponding to the best loop closure frame is used as the initial pose estimate for point cloud registration between the current keyframe and the best loop closure frame.

[0168] Those skilled in the art will appreciate that the units and algorithmic steps of each example described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional technical personnel may use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0169] The above embodiments can be implemented in whole or in part using software, hardware, firmware, or any combination thereof. When implemented using software, they can be implemented in whole or in part in the form of a computer program product. A computer program product comprises 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 device. The computer instructions can be stored in a computer-readable storage medium or transmitted via 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 via wired (e.g., coaxial cable, optical fiber, digital line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. 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 data center that integrates one or more available media. Available media can include magnetic media (e.g., floppy disks, hard disks, tapes), optical media (e.g., DVDs), or semiconductor media (e.g., solid-state drives (SSDs)).

[0170] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. An indoor loop detection method based on angle features, characterized in that: The following processes are included: Construct keyframe point cloud based on the robot's odometry pose and lidar point cloud; Perform high-direction filtering on the keyframe point cloud to generate an average point cloud; Extract angle feature points from the average point cloud and distinguish convex angles from concave angles for each angle feature point; A triangle descriptor is constructed based on the angle feature points and the corresponding convex and concave angle discrimination results, and a hash table structure is used to uniformly manage the triangle descriptors in the historical key frames. Determine multiple candidate loop 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; A loop closure verification is performed on each candidate keyframe to determine the best loop closure frame, and the two-dimensional rigid body transformation corresponding to the best loop closure frame is used as the initial pose estimate for point cloud registration between the current keyframe and the best loop closure frame.

2. The indoor loop detection method based on angle features according to claim 1, wherein: Based on the robot's odometer pose and lidar point cloud, a keyframe point cloud is constructed, including: From a reference frame moment Start, accumulate Frame scanning point cloud data, for each frame of point cloud data ,in, ,and Indicates the The timestamp of the frame, according to Posture Pose with reference frame The relative transformation relationship is used to transform the point cloud into the reference frame coordinate system: ,in, is a point in the original radar point cloud, is the odometry pose of the frame, is the odometry pose of the reference frame, Represents the coordinates of the point in the reference frame coordinate system after transformation. All point clouds after pose transformation are merged to form a complete keyframe point cloud. .

3. The indoor loop detection method based on angle features according to claim 1, wherein: Perform high-direction filtering on the point cloud of the key frame to generate an average point cloud, including: Taking the center of the laser radar as the polar coordinate origin, a two-dimensional polar coordinate system is constructed, and the key frame point cloud after height filtering is set as , on the horizontal plane The range is evenly divided into multiple sectors; For any point , calculate the azimuth 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 processing all sectors, at most one point is retained in each sector as the final average point cloud.

4. The indoor loop detection method based on angle features according to claim 3, wherein: On the horizontal plane 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.

5. The indoor loop detection method based on angle features according to claim 1, wherein: Extract angular feature points from the average point cloud, including: 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 to obtain the average point cloud Another point set with the same number of points ; Utilize point set Construct 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, and the point set after the first screening is obtained. ; 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 second filtered point set. ; Point Set Perform point cloud clustering based on Euclidean distance, select the point with the longest distance in each cluster as the optimal feature point, and define the point set To determine the optimal feature point set, point set Each angle feature point in is associated with an angle type.

6. The indoor loop detection method based on angle features according to claim 5, characterized in that: The convex and concave angles of each angle feature point are judged, including: Average point cloud Point A in the point set The nearest neighbor point B in the vector , calculate the vector Direction vector to point A The angle of the composition ,when When it is greater than 90°, it is a concave angle; Average point cloud Point C in the point set The nearest neighbor point D in the vector , calculate the vector Direction vector to point C The angle of the composition ,when When it is greater than 90°, it is a convex angle.

7. The indoor loop detection method based on angle features according to claim 5, characterized in that: Construct a triangle descriptor based on the angle feature points and the corresponding convex and concave angle discrimination results, including: Use the fourth kD-Tree to point set For point set organization and storage Any current feature point in the fourth kD-Tree is searched The current feature point and the nearest neighbor point are used as vertices to construct a triangle descriptor, and a uniqueness check is performed on all generated triangles to remove redundant triangles with exactly the same vertex set. The triangle descriptor includes: the two-dimensional space coordinates of the three vertices, the lengths of the three sides in ascending order, the keyframe index to which the triangle descriptor belongs, and the angle type of each of the three vertices.

8. The indoor loop detection method based on angle features according to any one of claims 1 to 7, wherein: Based on each triangle descriptor of the current key frame and the triangle descriptors of the historical key frames in the hash table structure, multiple candidate loop frames are determined, including: A hash table structure is used to organize and manage all triangle descriptors in historical keyframes. The hash key is calculated using the three side lengths of each triangle descriptor and mapped to the corresponding position in the hash table. Each position stores a set of triangle descriptors with the same side length structure or a difference less than a set threshold, and records the historical keyframe index to which each descriptor belongs. For each triangle descriptor in the current keyframe, the hash key is calculated using the three side lengths, and the corresponding position is found in the hash table. Then, all historical triangle descriptors stored at that 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 difference between the three sides is within the set error tolerance, the two descriptors are considered to be geometrically similar and constitute a valid matching pair. Whenever a triangle descriptor of the current keyframe successfully matches a triangle descriptor in a historical keyframe, a vote is cast for the historical keyframe. After all descriptors of the current keyframe are processed, the cumulative votes of all historical keyframes are counted, and the top N keyframes with the most votes are selected as candidate loop frames. The index difference between the candidate key frame and the current key frame is calculated, and the candidate key frames with a time interval less than the preset threshold are eliminated. The candidate key frames that meet the time constraint are screened out as the final candidate loop frames.

9. The indoor loop 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: Verify the consistency of the angular characteristics of all matched triangle descriptor pairs between the current keyframe and the candidate keyframe. If the angular characteristics of the corresponding vertices in the matched triangle descriptor pairs are inconsistent, the match is considered to have a mismatch risk and is directly eliminated. For each pair of matching triangle descriptors, singular value decomposition is used to calculate the two-dimensional rigid body transformation between the current keyframe and the candidate keyframe based on the matching vertex coordinates. A random sampling consensus algorithm is used to find the two-dimensional rigid body transformation that can maximize the number of correctly matching triangle descriptors. Among all candidate keyframes, the historical keyframe with the largest number of triangle descriptors that effectively match the current keyframe is selected as the best loop frame.

10. An indoor loop detection system based on angle features, characterized in that: include: The key frame construction unit is configured to: construct a key frame point cloud according to the robot's odometry pose and the lidar point cloud; The average point cloud generation unit is configured to: perform height direction filtering on the key frame point cloud to generate an average point cloud; The angle feature point extraction unit is configured to: extract angle feature points from the average point cloud, and distinguish convex angles from concave angles for each angle feature point; The descriptor construction unit is configured to: construct a triangle descriptor based on the angle 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 the historical key frames; The candidate loop frame determining unit is configured to: determine a plurality of candidate loop 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; The initial pose estimation unit is configured to: perform loop closure verification on each candidate key frame, 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 alignment between the current key frame and the best loop closure frame.

Images