A semantic ellipsoid-based loop-back detection method and system

Abstract

The application provides a loop detection method and system based on a semantic ellipsoid, performs target detection, and obtains a preliminary ellipsoid model by constraining semantic point clouds obtained through target detection on a key frame; in subsequent observation, the model is continuously optimized through detection frame constraint and a nearest neighbor relationship semantic histogram is updated as a descriptor; in loop detection, optimal matching is performed on the current local semantic graph and global map semantics based on descriptor similarity and geometric consistency, a transformation matrix of the local graph to the global map is calculated according to the matching result, a loop matching mechanism is established based on inliers, and the map loop is corrected. The application extracts semantic information in a scene through target detection, obtains a scene semantic graph through ellipsoid modeling and optimization, and realizes loop detection based on map semantic matching suitable for large visual angle differences.

Description

Technical Field

[0001] This invention belongs to the field of map optimization technology, specifically relating to a loop closure detection method and system based on semantic ellipsoids. Background Technology

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

[0003] Traditional SLAM methods often rely on low-level geometric information, such as points, lines, and polygons, to achieve localization and subsequent map optimization. However, in complex scenes, low-level geometric features are often insufficient for a high-level understanding of the environment and the demand for scene semantic information. This is especially true for loop closure detection, where low-level features often fail to maintain stable associations under conditions of large parallax or significant lighting variations, leading to loop closure detection failure. Therefore, to improve the performance and adaptability of SLAM systems, it is necessary to introduce higher-level semantic information to achieve a deeper understanding of complex scenes and robust map representation performance.

[0004] In SLAM loop closure detection, incorporating semantic information effectively addresses challenges such as significant viewpoint differences and lighting variations. Through deep learning-based object detection and semantic segmentation, semantic information from images can be effectively extracted and applied.

[0005] According to the inventor, some existing research has achieved certain results, but still has certain shortcomings. For example, the paper (NICHOLSON LN, MILFORD M, SüNDERHAUF N. QuadricSLAM: Dual Quadrics as SLAMLandmarks; procedures of the 2018 IEEE / CVF Conference on Computer Vision and Pattern Recognition Workshops (CVPRW), F 18-22 June 2018, 2018 [C]) proposes a method for constructing an ellipsoidal semantic model based on detection box constraints and applying it to semantic maps; the paper (LI J, KOREITEM K, MEGER D, et al. View-Invariant Loop Closure with Oriented Semantic Landmarks; procedures of the 2020 IEEE International Conference on Robotics and Automation (ICRA), F 31 May-31 Aug. 2020, 2020 [C]) uses an exhaustive search method to search for the match between local map semantics and global semantics. Although it can effectively retrieve closed loops, it is very time-consuming.To further improve the efficiency of semantic search, the literature (GAWEL A, DON CD, SIEGWART R, et al. X-View: Graph-Based Semantic Multi-View Localization[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 1687-94) and (GUO X, HU J, CHEN J, et al. Semantic Histogram Based Graph Matching for Real-Time Multi-Robot Global Localization in Large Scale Environment[J]. IEEE Robotics and Automation Letters, 2021, 6(4): 8349-56.) generate descriptors by random walk and semantic histogram construction respectively after extracting the semantic topology graph of the image. Semantic loop closure matching of local graph and global graph is achieved based on semantic descriptors. Although it can improve the loop closure detection speed, the former descriptor extraction is time-consuming, and the latter does not make full use of geometric information, which is prone to mismatch. Moreover, both only abstract semantic points from incompletely observed point clouds, which has certain errors.

[0006] In summary, how to quickly abstract semantic features for accurate loop closure detection while ensuring accurate semantic modeling remains one of the problems that need to be solved. Summary of the Invention

[0007] To address the aforementioned problems, this invention proposes a loop closure detection method and system based on a semantic ellipsoid. This invention extracts semantic information from the scene through target detection, and obtains a scene semantic map through ellipsoid modeling and optimization, thereby achieving loop closure detection based on map semantic matching applicable to large viewpoint differences.

[0008] According to some embodiments, the present invention adopts the following technical solution:

[0009] A loop closure detection method based on semantic ellipsoids includes the following steps:

[0010] Perform target detection, obtain semantic target detection boxes in the image, extract semantic point clouds from them, and build an ellipsoid model based on the semantic point clouds;

[0011] Extract the center of the ellipsoidal model as the semantic center, and construct a nearest neighbor semantic histogram as a descriptor;

[0012] A preliminary matching based on descriptor similarity is performed between the current local semantic graph and the global map semantics, and those that meet the similarity are taken as candidate matching items for local semantics;

[0013] For semantics and corresponding candidate matches in a local map, geometric similarity is calculated based on geometric consistency.

[0014] A bipartite graph is constructed based on the local graph and its matching terms, and the optimal matching is performed based on the descriptor and geometric similarity.

[0015] Based on the optimal matching result, the transformation matrix from the local map to the global map is calculated. If the transformation matrix has a number of inliers that meet a preset value, the loop closure matching is successful and the map loop closure is corrected; otherwise, target detection is performed again.

[0016] As an alternative implementation method, the specific process of extracting semantic point clouds and building an ellipsoidal model based on the semantic point clouds includes: extracting ORB feature points from the semantic target detection boxes of the acquired image, performing depth filtering, and associating them with the corresponding semantic boxes.

[0017] For a semantic point cloud containing multiple feature points, under the assumption that the roll and pitch angles are aligned with the horizontal coordinates, the yaw angle is estimated through principal component analysis, thereby obtaining an ellipsoidal representation of the semantic point cloud.

[0018] As an alternative implementation, the method further includes the following steps: optimizing the ellipsoidal model established based on the semantic point cloud, and extracting the center of the optimized ellipsoidal model as the semantic center. The optimization process includes: calculating the observation error between the ellipsoidal projection box and the semantically associated detection box, calculating the error cost based on the observation error, and minimizing the map semantic error under the current key frame by using the nonlinear optimization method bundle adjustment, thereby optimizing the ellipsoidal model.

[0019] As a further implementation, the calculation process of the ellipsoidal projection frame includes introducing a dual quadratic surface to calculate the projection curve of the ellipsoid on the image plane, and obtaining the projection frame of the ellipsoid by finding the extreme points of the boundary line.

[0020] As a further implementation, before optimizing the ellipsoidal model built based on the semantic point cloud, the method further includes associating the detected semantics with the map semantics on a keyframe basis. If the association is successful, the point cloud model is updated. The specific process includes:

[0021] For targets detected in keyframes, the intersection-union ratio of the projection box and the target detection box is calculated, and the map semantic with the highest overlap is associated with it. At the same time, the feature points of semantic association are expanded, and outliers are removed based on the iForest algorithm to ensure that the point cloud is correctly associated with the actual semantics. If there is no overlap of map semantics, a new semantic is created.

[0022] As an alternative implementation method, the specific process of establishing a semantic histogram of nearest neighbor relationships as a descriptor includes: establishing a descriptor based on category information; for each semantic point in the map, searching for semantics within a set radius as first-level nearest neighbors; and encoding the semantic class information frequency histogram of the current semantic, first-level nearest neighbor semantics, and the nearest neighbors of the nearest neighbor semantics into a vector in a certain order as a semantic descriptor.

[0023] As an alternative implementation, the local semantic map and the global map semantics are divided according to the keyframes for detecting loop closures. For all semantics in the map, if the difference between the keyframe number of the most recently observed semantic and the loop closure detection frame number is less than a set value, it is classified as local map semantics; otherwise, it is classified as global map semantics.

[0024] As an alternative implementation, the descriptor similarity is calculated using cosine similarity.

[0025] As an alternative implementation, the specific process of calculating geometric similarity based on geometric consistency includes: calculating the distance matrix between candidate matches, and then calculating the geometric consistency difference matrix between the candidate match and the current local graph. Each row of the geometric consistency difference matrix represents the cost of the candidate match semantics in relative distance with other candidate matches. For the distance difference cost of each candidate match for the semantics of the same local graph, the minimum value is selected. Combined with the set boundary, the distance weight vector is calculated, and candidate match semantics with zero weight are removed.

[0026] As an alternative implementation, the specific process of optimal matching based on descriptor and geometric similarity includes weighted summation of descriptor similarity and geometric similarity, abstracting the matching of local graph and global graph into an optimal allocation problem of weighted bipartite graph, and solving it using the Kuhn-Munkres algorithm.

[0027] As an alternative implementation, an interior point is a semantic point whose distance error with the matching semantic in the global graph is less than a threshold after the local graph is pose-transformed according to the transformation matrix.

[0028] A loop closure detection system based on a semantic ellipsoid includes:

[0029] The keyframe processing module is configured to perform target detection, obtain semantic target detection boxes of the image, extract semantic point clouds from them, and establish an ellipsoid model based on the semantic point clouds;

[0030] The map semantic optimization module is configured to extract the center of the ellipsoidal model as the semantic center and establish a nearest neighbor semantic histogram as a descriptor.

[0031] The local semantic matching module is configured to perform a preliminary matching between the current local semantic graph and the global map semantics based on descriptor similarity, and take those that meet the similarity as candidate matching items for local semantics; calculate geometric similarity based on geometric consistency for the semantics in the local map and the corresponding candidate matching; construct a bipartite graph based on the local graph and its matching items, and perform optimal matching based on descriptor and geometric similarity;

[0032] The closed-loop correction module is configured to calculate the transformation matrix from the local map to the global map based on the optimal matching result. If the transformation matrix has a number of inliers that meet a preset value, the loop closure matching is successful and the map loop closure is corrected; otherwise, target detection is performed again.

[0033] As an alternative implementation, the map semantic optimization module includes:

[0034] The semantic association module is configured to associate detected semantics with map semantics on a keyframe basis, and update the point cloud model if the association is successful.

[0035] The ellipsoid optimization module is configured to calculate the observation error between the ellipsoid projection box and the semantically associated detection box, calculate the error cost based on the observation error, minimize the map semantic error in the current keyframe, and optimize the ellipsoid model.

[0036] The semantic descriptor building module is configured to build descriptors based on category information. For each semantic point in the map, it searches for semantics within a set radius as first-level neighbors. By statistically analyzing the frequency histogram of semantic class information of the current semantic, first-level neighbor semantics, and the neighbors of the neighbor semantics, it encodes them into vectors in a certain order as semantic descriptors.

[0037] As an alternative implementation, the local semantic matching module includes:

[0038] The descriptor matching module is configured to perform a preliminary matching between the current local semantic graph and the global map semantics based on descriptor similarity, and to use those that meet the similarity as candidate matching items for local semantics;

[0039] The geometric consistency matching module is configured to calculate geometric similarity based on geometric consistency for semantics and corresponding candidate matches in a local map.

[0040] The optimal matching module is configured to construct a bipartite graph based on the local graph and its matching items, and perform optimal matching based on descriptors and geometric similarity.

[0041] A navigation system employing the above-described method or system.

[0042] A robot comprising the aforementioned navigation system.

[0043] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0044] This invention utilizes map semantics to create an accurate ellipsoidal model, which can quickly establish the ellipsoidal model, enrich map semantic information, and combine point cloud constraints in the ellipsoidal model optimization process, which can effectively assist in the generation of map semantics. Furthermore, in subsequent optimization, the cross-section constraints are reasonably selected, so that the final ellipsoidal optimization model is realistic.

[0045] This invention performs initial modeling based on the feature point cloud of a single keyframe, and subsequently optimizes it solely using keyframe detection boxes, thus improving the system's accuracy and efficiency. Furthermore, it only performs detection of targets at different distances within the field of view on keyframes, and considers only category consistency during semantic matching, while avoiding exhaustive searching during geometric consistency matching, thereby improving the system's real-time performance. By comprehensively considering descriptor similarity and geometric consistency matching, it improves localization accuracy and loop closure detection accuracy.

[0046] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0047] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0048] Figure 1 This is a flowchart of a semantic ellipsoid-based loop closure detection system according to one embodiment;

[0049] Figure 2 This is a schematic diagram illustrating the construction of a semantic histogram in one embodiment.

[0050] Figure 3 This is a schematic diagram of an ellipsoidal model based on point cloud constraints, representing one embodiment.

[0051] Figure 4 This is a schematic diagram showing the overall and partial views of a sofa according to one embodiment;

[0052] Figure 5 A schematic diagram of the iterative update of a point cloud model in one embodiment.

[0053] Figure 6 This is a schematic diagram of a semantic optimization model for one embodiment.

[0054] Figure 7 A schematic diagram of a USTC_02 sequence scenario according to one embodiment;

[0055] Figure 8 This is a schematic diagram of the USTC_02_short sequence trajectory of one embodiment;

[0056] Figure 9 This is a schematic diagram of ORB-SLAM2 in the USTC_02_short sequence of one embodiment and APE in this embodiment. Detailed Implementation

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

[0058] It should be noted that the following detailed description is illustrative and intended to provide further explanation of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0059] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0060] Where there is no conflict, the embodiments and features described in this application may be combined with each other.

[0061] Example 1

[0062] A loop closure detection method based on semantic ellipsoids includes the following steps:

[0063] Target detection is performed to obtain semantic target detection boxes in the image, semantic point clouds are extracted from them, an ellipsoidal model is built based on the semantic point clouds, and the model is continuously optimized in subsequent observations through detection box constraints.

[0064] Extract the center of the ellipsoidal model as the semantic center, and construct a nearest neighbor semantic histogram as a descriptor;

[0065] A preliminary matching based on descriptor similarity is performed between the current local semantic graph and the global map semantics, and those that meet the similarity are taken as candidate matching items for local semantics;

[0066] For semantics and corresponding candidate matches in a local map, geometric similarity is calculated based on geometric consistency.

[0067] A bipartite graph is constructed based on the local graph and its matching terms, and optimal matching or maximum weight KM matching is performed based on descriptors and geometric similarity.

[0068] Based on the optimal matching result, the transformation matrix from the local map to the global map is calculated. If the transformation matrix has a number of inliers that meet a preset value, the loop closure matching is successful and the map loop closure is corrected; otherwise, target detection is performed again.

[0069] In this embodiment, the target detection method is performed in keyframes. Highly real-time deep learning methods such as YOLO and NanoDet can be used to obtain detection boxes. Based on static scene semantic information, feature point clouds within the detection boxes are extracted to construct a preliminary semantic ellipsoid.

[0070] In this embodiment, the descriptor is established based on category information. For each semantic point in the map, the semantics within a certain radius are searched as first-level neighbors. The frequency histogram of the semantic class information of the current semantic, the first-level neighbor semantic, and the neighbors of the neighbor semantic (second-level neighbors) is statistically analyzed and encoded into a vector in a certain order, which serves as the semantic descriptor.

[0071] In this embodiment, the local semantic map and the global semantic map are divided according to the key frames for detecting loop closures. For all semantics in the map, if the key frame number of the most recently observed semantic is close to the loop closure detection frame number, it is classified as a local map semantic; otherwise, it is classified as a global map semantic.

[0072] In some embodiments, descriptor similarity is calculated using cosine similarity.

[0073] Geometric consistency refers to the consistency of distance relationships between map semantics. If a local map and a global map are matched, the distance between them should be consistent.

[0074] In this embodiment, the weight of KM matching is a weighted sum of descriptor similarity and geometric similarity. That is, by weighting the descriptor similarity and geometric similarity, the matching of the local graph and the global graph is abstracted into an optimal allocation problem of a weighted bipartite graph, which is solved by the Kuhn-Munkres algorithm.

[0075] Interior points are semantic points whose distance error with the matching semantics in the global graph is small after the local graph is pose-transformed according to the transformation matrix.

[0076] Example 2

[0077] A semantic ellipsoid-based loop closure detection system is proposed. For input keyframes, a preliminary ellipsoid model is established using semantic point cloud constraints obtained from object detection. In subsequent observations, this model is continuously optimized using bounding box constraints, and the nearest neighbor semantic histogram is updated as a descriptor. In loop closure detection, maximum weighted KM matching based on descriptor similarity and geometric consistency is performed between the current local semantic graph and the global map semantics. The transformation matrix from the local graph to the global map is calculated based on the matching results, and a loop closure matching mechanism is established based on interior points to correct map loop closures.

[0078] Specifically, it includes:

[0079] The keyframe processing module is configured to perform target detection, obtain semantic target detection boxes of the image, extract semantic point clouds from them, and establish an ellipsoid model based on the semantic point clouds;

[0080] The map semantic optimization module is configured to extract the center of the ellipsoidal model as the semantic center and establish a nearest neighbor semantic histogram as a descriptor.

[0081] The local semantic matching module is configured to perform a preliminary matching between the current local semantic graph and the global map semantics based on descriptor similarity, and take those that meet the similarity as candidate matching items for local semantics; calculate geometric similarity based on geometric consistency for the semantics in the local map and the corresponding candidate matching; construct a bipartite graph based on the local graph and its matching items, and perform optimal matching based on descriptor and geometric similarity;

[0082] The closed-loop correction module is configured to calculate the transformation matrix from the local map to the global map based on the optimal matching result. If the transformation matrix has a number of inliers that meet a preset value, the loop closure matching is successful and the map loop closure is corrected; otherwise, target detection is performed again.

[0083] The execution steps for each module are described below:

[0084] The keyframe processing module performs the following steps:

[0085] For each keyframe input from the front-end thread, the semantic object detection box of the RGB image is obtained through the object detection algorithm. The ORB feature points are extracted, and after depth filtering, they are associated with the corresponding semantic box.

[0086] Since the ellipsoid has nine degrees of freedom, including rotation angle, translation, and the three semi-axis parameters of the ellipsoid, its compact vector expression is q=(θ1,θ2,θ3,t1,t2,t3,s1,s2,s3). T Therefore, it is necessary to estimate its rough position and size from the point cloud. For a semantic point cloud containing n feature points, under the same assumption that the roll and pitch angles are aligned with the horizontal coordinates, the yaw angle θ is estimated through principal component analysis. First, assume that the projection matrix of the point cloud on the z-axis is:

[0087]

[0088] Calculate the mean values ​​of the point cloud in the x and y directions in the world coordinate system. and After point cloud decentralization, we have:

[0089]

[0090] The covariance matrix is ​​then:

[0091]

[0092] By calculating the eigenvalues ​​of Cov, the eigenvector corresponding to its largest eigenvalue is obtained. This is the direction vector in the reduced xy-coordinate system, from which the yaw angle θ can be obtained. Then, projecting the direction vector onto the point cloud using the x-axis, we have:

[0093]

[0094] The extreme values ​​of each row vector of M' are x'. min ,x' max ,y' min ,y' max and the original point cloud z-axis extreme value z' min ,z' max Then the semi-axis length of the point cloud model can be obtained as:

[0095]

[0096] The center of the point cloud can be estimated as:

[0097]

[0098] This allows us to obtain an ellipsoidal representation of the semantic point cloud.

[0099] The map semantic optimization module performs the following steps:

[0100] For semantics detected in keyframes, they are first associated with existing semantics in the map. Since the semantic ellipsoid is a type of second-degree surface, and a second-degree surface can be parameterized using a 4x4 symmetric matrix Q:

[0101] X T QX=0

[0102] Where the homogeneous vector X = (x1, x2, x3, x4) T When x4≠0, it represents the coordinate point (x1 / x4, x2 / x4, x3 / x4) in three-dimensional space, and when x4=0, it represents the point at infinity.

[0103] Since points and planes are dual in 3D space, a quadratic surface can be expressed using a series of tangents in dual space. Because Q is a symmetric matrix, the dual form of the quadratic surface can be represented by the corresponding 4x4 matrix Q. * Expressed as:

[0104] π T Q* π = 0

[0105] Q * Let Q be the adjoint matrix of Q. If Q is invertible, then Q... * =Q -1 .

[0106] Introducing a dual quadric surface allows us to calculate the projection curve C of the ellipsoid onto the image plane. Since the dual form of the quadric surface defines its tangent plane, and the orthographic projection reflects the tangency of the outline, when projecting a quadric surface, we have:

[0107] C * =PQ * P T

[0108] Where C * For the dual curve corresponding to C, P = K[R|t] is the projection matrix of the camera, K is the intrinsic parameter of the camera, and R and t are the rotation matrix and translation vector, respectively.

[0109] After obtaining the projection curve C, the projection box of the ellipsoid can be obtained by finding the extreme points of the boundary line. For targets detected in the keyframe, the intersection-union ratio (IUU) of the projection box and the target detection box D is calculated, and the map semantic with the highest overlap is associated with it. At the same time, the feature points of semantic association are expanded, and outliers are removed based on the iForest algorithm to ensure that the point cloud is correctly associated with the actual semantics. If there is no map semantic overlap, a new semantic is created.

[0110] For model Q constructed from point clouds p Its planar projection box has the corresponding target detection box in the key frame. If the overlap is low, it will not be used in the subsequent optimization process to avoid unreasonable tangent constraints from affecting the ellipsoid optimization.

[0111] In SLAM, for map semantics, ideally, if the ellipsoid model is accurate, its projection box in the image coordinate system should be aligned with the target detection box. However, due to noise and other reasons, the initial ellipsoid model also has errors, meaning that the ellipsoid projection box and the detection box cannot perfectly match, requiring continuous optimization in subsequent observations.

[0112] Suppose that in n frames of observation, there is S = {s1, s2, ..., sn} l} map semantics, for x i The semantics of s k Its projection frame is B ik Abstract it as B ik =h(ξ i ,s k Since the semantic s in the i-th frame k The observation is the detection box D associated with it.ik Therefore, the observation error can be expressed as:

[0113] e(ξ i ,s k ) = D ik -B ik

[0114] Similarly, for all map semantic observations, the error cost can be obtained as follows:

[0115]

[0116] By using the nonlinear optimization method Bundle Adjustment and the g2o tool, the semantic error of the map in the current keyframe can be minimized, and the ellipsoid model can be optimized.

[0117] After optimization, the ellipsoidal model is considered to be stable in the local environment, at which point semantic descriptors are extracted. Specifically, as follows... Figure 2 As shown, different colored dots represent different categories. The left side of the diagram is an abstract local semantic topology graph. The red circle represents the first-level semantics of the green nodes. By statistically analyzing the semantic class information of the current semantics, the first-level nearest neighbor semantics, and the nearest neighbors (second-level nearest neighbors) of the nearest neighbor semantics, the frequency histogram on the right can be obtained. These are encoded into vector descriptors in a certain order: H d = [...,1,2,0,2,0,1,...], where the length d of the vector is the cube of all map semantic categories.

[0118] The local semantic matching module performs the following steps:

[0119] For each semantic element within the local map, firstly, matching candidates in the global map are obtained based on descriptor similarity, and their similarity is calculated as follows:

[0120]

[0121] In the matching module, global map semantics with a similarity greater than 0.5 are used as matching candidates for the current search term in the local graph. Since descriptors constructed solely based on local category histograms have many false matches, further filtering and matching based on geometric consistency is required after obtaining all matching local graph semantics and their corresponding matching candidates.

[0122] Suppose semantics A, B, and C exist in the local map, and candidate matches A[a1,a2], B[b1], and C[c1,c2] are found in the global map by the descriptor. Assuming that some candidate matches are correct, the relative distance between correct matches should be closer to the relative distance between semantics in the local map. Therefore, the distance matrix between a1, a2, b1, c1, and c2 is calculated as follows:

[0123]

[0124] Expanding A, B, C to A,A,B,C,C, the distance matrix is:

[0125]

[0126] Therefore, the geometric consistency difference matrix between the candidate match and the current local graph is obtained as follows:

[0127]

[0128] Each row of this matrix represents the cost of the candidate matching semantics relative to other candidate matchings. For the same local graph semantics, the minimum cost of the distance difference between candidate matchings is selected.

[0129]

[0130] Using the maximum distance difference of 0.2 as the boundary, the distance weight vector Weight is generated from Cost:

[0131]

[0132] If the weight is 0, it indicates that the geometric verification failed, and the semantic meaning is removed from the candidates. For candidate matches of the same local graph semantic meaning, their geometric similarity is derived from the normalized weight of the n candidate matches for that semantic meaning:

[0133]

[0134] For each candidate semantic, the final matching score is obtained by combining the cosine similarity and geometric similarity of the descriptors:

[0135] Score=λ·Sim+(1-λ)·Geo

[0136] λ is the weight.

[0137] Since a local graph semantic may have multiple global map semantics as candidate matches, and a global map semantic may also be a candidate match for multiple local graph semantics, the matching of local and global graphs can be abstracted as an optimal allocation problem of a weighted bipartite graph. This optimal matching problem can be solved using the Kuhn-Munkres algorithm. The KM algorithm converts weights into feasible vertex labels and uses these labels to find a perfect match for each vertex, where each vertex is connected to a weighted edge. The algorithm flow is as follows:

[0138] (1) Initialize feasible top labels;

[0139] (2) Find the maximum matching using the Hungarian algorithm. If it is a perfect matching, then the process ends.

[0140] (3) If no perfect match is found, modify the value of the feasible top label;

[0141] (4) Repeat steps (2) and (3) until a perfect match of equal subgraphs is found.

[0142] The maximum weight allocation of the local semantic graph and the global map is obtained by using the Kuhn-Munkre algorithm. If there are at least 5 matching results, the local graph and the global map are considered similar, and there may be loops.

[0143] The closed-loop correction module attempts to calculate the transformation matrix between the local and global graphs and correct the camera pose if sufficient matching exists in the local semantics.

[0144] For the semantics of a local graph S = {s1, s2, ..., s} n Its matching in the global map is S'={s'1,s'2,...,s'}. n The pose transformation matrix R,t can be obtained by iterating over the nearest point.

[0145] Because of the potential for erroneous semantic matching, directly calculating all matching points in the ICP problem may result in significant errors. To eliminate interference from erroneous matches, a Random Sample Consensus algorithm is used to obtain the transformation matrix of the maximum convergent inliers. If the calculated transformation matrix has a sufficient number of inliers, it indicates that a loop closure has been detected and the transformation matrix is ​​valid. In this case, graph optimization methods are used to correct the global map and pose.

[0146] Example 3

[0147] As a verification embodiment of the methods / systems described in Embodiments 1 and 2 above, such as Figure 3As shown, in the freiburg3_office scene, an ellipsoidal model is established using single-frame point cloud constraints. It can be seen that point cloud-based modeling can quickly build an ellipsoidal model using object detection bounding boxes, enriching the semantic information of the map.

[0148] like Figure 4 As shown, a large sofa in a scene is partially identified as a semantic object by the object detection algorithm due to the viewing angle. During the SLAM process, as the sofa moves from partially entering the frame to leaving it, a large number of observations are identified by the object detection algorithm, resulting in incorrect sectional constraints. This leads to a significant discrepancy between the final model and reality, and even makes it inconsistent with actual observations.

[0149] At this time, the ellipsoidal model established based on point cloud constraints changes as follows during the observation process: Figure 5 As shown. The blue box represents the projection frame of the semantic ellipsoid model onto the observation frame. The green ellipsoid represents the point cloud constraint model of the sofa.

[0150] As can be seen, due to the sparsity and uneven distribution of feature points, some edge feature points are incorrectly excluded in semantic association. Therefore, the model built from pure point clouds is not accurate enough, but it can be used as a preliminary model to filter associated object detection boxes. After continuous observation and optimization, the final semantic sofa model is as follows: Figure 6 As shown.

[0151] The optimization model shows that the ellipsoidal model combined with point cloud constraints can effectively assist in the generation of map semantics. Furthermore, in subsequent optimization, the cross-section constraints are reasonably selected, so that the final optimized ellipsoidal model is more realistic.

[0152] The USTC_02 sequence was selected to verify the improvement of semantic loop closure on the algorithm's accuracy. The dataset scenario is as follows: Figure 7 As shown, the red numbers indicate the recording order of the entire sequence. It can be seen that the beginning and end of the sequence form a closed loop with the same viewpoint. After removing scene 6, the sequence forms a large parallax loop through scenes 2 and 5 in the figure. To distinguish it from the original sequence, it is called the USTC_02_short sequence.

[0153] In the USTC_02_short sequence, the trajectories of keyframes in ORB_SLAM2 and the schemes provided in the above embodiments in the spatial coordinate system are compared with GroundTruth, for example... Figure 8 As shown in the figure, the black trajectory is the Ground Truth trajectory, while the blue and yellow trajectories are the ORB_SLAM2 system trajectory and the trajectory of the system described in this paper, respectively. It can be seen that the trajectory obtained by the scheme provided in the above embodiments is closer to the real trajectory.

[0154] The errors between the trajectory obtained by ORB-SLAM2 and the scheme provided in the above embodiments and the actual trajectory in the USTC_02_short sequence, as well as the real-time APE error, are as follows: Figure 9 As shown, (a) is the ORB-SLAM2 algorithm, and (b) is the algorithm provided in this paper. The first row represents the trajectory APE error, and the second row represents the real-time APE error. It can be seen that ORB-SLAM2 cannot detect the loop closure when there is a large parallax loop, so the accumulated error cannot be eliminated. However, the scheme provided in the above embodiments has higher trajectory estimation accuracy.

[0155] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0156] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0157] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0158] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1The steps of the function specified in one or more boxes.

[0159] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made by those skilled in the art without creative effort within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A loop closure detection method based on semantic ellipsoids, characterized in that, Includes the following steps: Perform target detection, obtain semantic target detection boxes in the image, extract semantic point clouds from them, and build an ellipsoid model based on the semantic point clouds; Extract the center of the ellipsoidal model as the semantic center, and construct a nearest neighbor semantic histogram as a descriptor; A preliminary matching based on descriptor similarity is performed between the current local semantic graph and the global map semantics, and those that meet the similarity are taken as candidate matching items for local semantics; For semantics and corresponding candidate matches in a local map, geometric similarity is calculated based on geometric consistency. A bipartite graph is constructed based on the local graph and its matching terms, and the optimal matching is performed based on the descriptor and geometric similarity. Based on the optimal matching result, the transformation matrix from the local map to the global map is calculated. If the transformation matrix has a number of inliers that meet the preset value, the loop closure matching is successful and the map loop closure is corrected; otherwise, the target detection is performed again. The specific process of extracting semantic point clouds and building an ellipsoidal model based on the semantic point clouds includes: extracting ORB feature points from the semantic target detection boxes of the acquired image, performing depth filtering, and associating them with the corresponding semantic boxes. For a semantic point cloud containing multiple feature points, under the assumption that the roll and pitch angles are aligned with the horizontal coordinates, the yaw angle is estimated through principal component analysis, thereby obtaining an ellipsoidal representation of the semantic point cloud; The ellipsoidal model established based on the semantic point cloud is optimized, and the center of the optimized ellipsoidal model is extracted as the semantic center. The optimization process includes: calculating the observation error between the ellipsoidal projection box and the semantically associated detection box; calculating the error cost based on the observation error; and minimizing the map semantic error in the current keyframe by using the nonlinear optimization method bundle adjustment to optimize the ellipsoidal model.

2. The loop closure detection method based on semantic ellipsoids as described in claim 1, characterized in that, Before optimizing the ellipsoidal model built based on the semantic point cloud, the process further includes associating the detected semantics with the map semantics on a keyframe basis. If the association is successful, the point cloud model is updated. The specific process includes: For targets detected in keyframes, the intersection-union ratio of the projection box and the target detection box is calculated, and the map semantic with the highest overlap is associated with it. At the same time, the feature points of semantic association are expanded, and outliers are removed based on the iForest algorithm to ensure that the point cloud is correctly associated with the actual semantics. If there is no overlap of map semantics, a new semantic is created.

3. The loop closure detection method based on semantic ellipsoids as described in claim 1, characterized in that, The local semantic map and the global map semantic map are divided according to the key frames for detecting loop closures. For all semantic maps, if the difference between the key frame number of the most recently observed semantic map and the loop closure detection frame number is less than a set value, it is classified as a local map semantic map; otherwise, it is classified as a global map semantic map.

4. The loop closure detection method based on semantic ellipsoids as described in claim 1, characterized in that, The specific process of calculating geometric similarity based on geometric consistency includes: calculating the distance matrix between candidate matches, and then calculating the geometric consistency difference matrix between the candidate matches and the current local graph. Each row of the geometric consistency difference matrix represents the cost of the candidate match semantics in relative distance with other candidate matches. For the distance difference cost of each candidate match semantics of the same local graph, the minimum value is selected. Combined with the set boundary, the distance weight vector is calculated, and candidate match semantics with zero weight are removed.

5. The loop closure detection method based on semantic ellipsoid as described in claim 1, characterized in that, The specific process of optimal matching based on descriptors and geometric similarity includes weighted summation of descriptor cosine similarity and geometric similarity, abstracting the matching of local and global graphs into an optimal allocation problem of weighted bipartite graphs, and solving it using the Kuhn-Munkres algorithm.

6. A loop closure detection system based on a semantic ellipsoid, employing the method described in claim 1, characterized in that, include: The keyframe processing module is configured to perform target detection, obtain semantic target detection boxes of the image, extract semantic point clouds from them, and establish an ellipsoid model based on the semantic point clouds; The map semantic optimization module is configured to extract the center of the ellipsoidal model as the semantic center and establish a nearest neighbor semantic histogram as a descriptor. The local semantic matching module is configured to perform a preliminary matching between the current local semantic graph and the global map semantics based on descriptor cosine similarity, and take those that meet the similarity as candidate matching items for local semantics; for the semantics in the local map and the corresponding candidate matching, geometric similarity is calculated based on geometric consistency. A bipartite graph is constructed based on the local graph and its matching terms, and the optimal matching is performed based on the descriptor and geometric similarity. The closed-loop correction module is configured to define the points whose distance error with the matching semantics in the global map is less than a threshold after the pose transformation of the local map according to the transformation matrix based on the optimal matching result as inner points, calculate the transformation matrix from the local map to the global map, and if the transformation matrix has a number of inner points that meet the preset value, the loop closure matching is successful and the map loop closure is corrected; otherwise, the target detection is re-performed.

7. A loop closure detection system based on a semantic ellipsoid as described in claim 6, characterized in that, The map semantic optimization module includes: The semantic association module is configured to associate detected semantics with map semantics on a keyframe basis, and update the point cloud model if the association is successful. The ellipsoid optimization module is configured to calculate the observation error between the ellipsoid projection box and the semantically associated detection box, calculate the error cost based on the observation error, minimize the map semantic error in the current keyframe, and optimize the ellipsoid model. The semantic descriptor building module is configured to build descriptors based on category information. For each semantic point in the map, it searches for semantics within a set radius as first-level neighbors. By statistically analyzing the frequency histogram of semantic class information of the current semantic, first-level neighbor semantics, and the neighbors of the neighbor semantics, it encodes them into vectors in a certain order as semantic descriptors.

8. A loop closure detection system based on a semantic ellipsoid as described in claim 6, characterized in that, The local semantic matching module includes: The descriptor matching module is configured to perform a preliminary matching between the current local semantic graph and the global map semantics based on the cosine similarity of the descriptors, and to use those that meet the similarity as candidate matching items for the local semantics. The geometric consistency matching module is configured to calculate geometric similarity based on geometric consistency for semantics and corresponding candidate matches in a local map. The optimal matching module is configured to construct a bipartite graph based on the local graph and its matching items, and perform optimal matching based on descriptors and geometric similarity.

Images