Global positioning method based on semantic distribution feature retrieval and semantic topological histogram

Abstract

The invention discloses a global positioning method based on semantic distribution feature retrieval and a semantic topological histogram, which comprises the following steps of: firstly, pre-screening candidate retrieval by utilizing global semantic distribution features in a scene, and preliminarily constructing a candidate retrieval queue; secondly, constructing a cost matrix based on a semantic topological histogram by utilizing a geometric topological relation between semantic examples in the same scene, and further solving vertex matching; and finally, rough pose estimation constructed by vertex matching is used as an initial value of an iterative nearest point algorithm, and final pose estimation is output through point cloud matching coupling position identification. The method effectively overcomes the problems of local feature matching errors, sensitivity to dynamic objects, insufficient calculation efficiency and the like of current laser radar-based one-time global positioning under the condition of no prior poses, semantic information is introduced, the semantic information of objects in the environment is utilized to assist one-time global positioning without initial poses, and the positioning accuracy is improved. And the global positioning precision and robustness are improved.

Description

Technical Field

[0001] The invention belongs to the technical field of laser radar global positioning, and in particular relates to a global positioning method based on semantic distribution feature retrieval and semantic topology histogram. Background Art

[0002] One-time global localization plays a key role in most robot application scenarios, and has significant advantages in the initialization and re-localization process. However, the current one-time global localization based on LiDAR faces problems such as local feature matching errors, sensitivity to dynamic objects, and insufficient computational efficiency without prior pose.

[0003] Existing one-shot global localization methods have certain limitations when dealing with complex environments. First, incorrect candidate retrieval may occur when local features are similar. Second, the localization performance is also difficult when it is affected by dynamic objects in complex environments. Therefore, a global localization method based on semantic distribution feature retrieval and semantic topology histogram is proposed to solve the above problems. Summary of the invention

[0004] To solve the above problems, the present invention discloses a global positioning method based on semantic distribution feature retrieval and semantic topology histogram, which introduces semantic information and utilizes the semantic information of objects in the environment to assist one-time global positioning without initial pose, thereby improving the accuracy and robustness of global positioning.

[0005] To achieve the above object, the technical solution of the present invention is as follows:

[0006] The global positioning method based on semantic distribution feature retrieval and semantic topology histogram includes the following steps:

[0007] 1. Use the global semantic distribution features in the scene to pre-screen candidate retrievals and initially build a candidate retrieval queue.

[0008] 2. Utilize the geometric topological relationship between semantic instances in the same scene to construct a cost matrix based on the semantic topological histogram, and then solve the vertex matching.

[0009] 3. Use the rough pose estimate constructed by vertex matching as the initial value of the iterative closest point algorithm, and output the final pose estimate through point cloud matching coupled position recognition.

[0010] The specific steps are:

[0011] Step 1. Use the global semantic distribution features in the scene to pre-screen candidate retrievals and initially build a candidate retrieval queue

[0012] The global semantic distribution features in the scene are used to pre-screen candidate retrievals and initially build a candidate retrieval queue; in real space, objects in the scene can represent the semantic information of this location to a certain extent. Based on this, it can be determined that the difference in the number of identical semantic categories between two semantic graphs can represent the scene differences to a certain extent. Therefore, the semantic graph G relative to the current query scene is defined as query , the semantic features of the prior semantic graph can be constructed as:

[0013] SF=sum(sqrt((Num map -Num query ) 2 )) (1)

[0014] Where Num = {num0, num1, ..., num m} represents the number of corresponding semantic categories in the semantic map, m represents the total number of semantic categories, where the subscript map represents the data in the map, and the subscript query represents the current query;

[0015] However, in reality, the rarer the semantic category, the more it can represent the semantic information of the scene. For example, the scene with traffic signs often belongs to the intersection. The number of semantic categories in the global prior map is counted first, and the semantic feature weight W is constructed. The steps are as follows:

[0016] Count the number of all semantic categories in the prior semantic graph:

[0017]

[0018] Where n represents the number of prior semantic maps, and map-all represents all data in the map;

[0019] 1) For Num map-all Normalize, the upper bound of the normalized result is Q H , the lower bound is Q L (Q L >0). The processed value is:

[0020]

[0021] 2) Take its reciprocal and perform normalization to obtain the semantic feature weight W.

[0022]

[0023] The final semantic feature weight W satisfies the property that the number of semantic features is inversely proportional to the weight size. Adding W into the formula yields:

[0024] SF=sum(sqrt(W*(Num map -Numquery ) 2 )) (5)

[0025] The SF feature can be used to quickly retrieve map frames with similar semantic scenes.

[0026] Step 2. Using the geometric topological relationship between semantic instances in the same scene as described in step 1, a cost matrix based on the semantic topological histogram is constructed to solve vertex matching;

[0027] After the semantic feature pre-screening in step 1, a map frame with similar semantic scenes is obtained. In the case of similar semantic features, further solutions are performed through the correlation between semantic instances in the scene. In the vision-based object-level relocalization algorithm, the Hungarian algorithm is often used to obtain the optimal match through the difference and spatial proximity between two semantic graphs. Referring to this scheme, this scheme adds geometric information between semantic objects on the basis of their semantic correlation, and constructs a new semantic topology histogram as the cost matrix of the Hungarian algorithm. Among them, the semantic topology histogram for each semantic node is defined as a two-dimensional matrix STH.

[0028]

[0029] The x in the STH matrix ij It represents the number of semantic instances with semantic label i in the jth portion divided into h portions by the distance threshold d near the instance object. The above formula can be used to obtain the surrounding semantic description of the corresponding semantic instance, that is, for any node Node query and any node Node in the corresponding map semantic graph map The matching cost of can be defined as a, which is calculated as follows:

[0030]

[0031] For the current scene semantic graph G query and the semantic graph G of the map to be retrieved map The Hungarian cost matrix A is:

[0032]

[0033] The optimal vertex pairing can be obtained by solving the Hungarian algorithm, and then the geometric relationship between the paired points is constructed to remove some erroneous points. Finally, select L CQ The frames with the most frame matching point pairs are selected as the final candidate queue, and pose estimation is further used for similarity evaluation.

[0034] Step 3. Use the rough pose estimate constructed by vertex matching in step 2 as the initial value of the iterative closest point algorithm, and output the final pose estimate through point cloud matching coupled position recognition;

[0035] The center of mass position Center of the semantic instance in the semantic graph can reflect its position in the real world. In step 2, the corresponding matching point set M of the current scene semantic graph and the optimal retrieval map semantic graph can be obtained. query and M map The relative posture between two semantic scenes can be obtained by solving SVD, and the solution equation is defined as:

[0036]

[0037] Among them, R coarse and t coarse To solve the obtained rotation and translation transformations, the variables in the equations R and t, c j query and c j map represents the matching point set M query and M map Considering that the original point cloud may have a change in viewpoint, which may cause the mass point of the semantic instance to change, it is difficult to obtain a precise solution based only on the SVD solution between semantic instances. The original point cloud of the current scene and the optimal retrieval map frame is introduced to describe its details as much as possible, and R coarse and t coarse As the initial pose of the GICP algorithm to obtain a refined pose estimate:

[0038]

[0039] where p query is the point of the original point cloud P of the current scene, q map is the point Q in the map point cloud, R gicp , t gicp is the final 6-Dof pose.

[0040] Since only the semantic point positions and semantic labels are considered when extracting semantic instance objects, the shape details of the semantic instances are ignored. Therefore, in some scenes, there may be situations where the semantic geometric distribution is similar. Therefore, this solution introduces the score in the GICP algorithm. fitness As the last step of similarity evaluation, if the score in the candidate queue fitness Less than the fitting threshold δ fitness Then select it as the final result directly, otherwise select score fitness The smallest candidate frame.

[0041] The beneficial effects of the present invention are:

[0042] The present invention shows excellent performance on the six tested data sets (DCC02, KAIST03, NTU02, NTU10, APO01, APO02): the median of its translation error is generally maintained below 0.5, and the span of the box and whisker is significantly smaller than that of the comparison methods (such as BTC and BEVPlace), which not only reflects that the algorithm has high positioning accuracy, but also shows that its results have good stability and consistency. Especially in the challenging DCC02 data set, when the error fluctuation range of other methods reaches 4-5, the present invention still maintains a smaller error range, which fully confirms the robustness and reliability of the present invention in translation error control. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is a flowchart for implementing the global positioning method based on semantic distribution feature retrieval and semantic topology histogram described in the present invention;

[0044] Figure 2 It is the error box plot of the method of the present invention compared with other open source methods on various data sets;

[0045] Figure 3 It is a time consumption analysis diagram of each module in the method described in the present invention. DETAILED DESCRIPTION

[0046] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0047] As shown in the figure, the global positioning method based on semantic distribution feature retrieval and semantic topology histogram described in the present invention comprises the following specific steps:

[0048] Step 1: Use the global semantic distribution features in the scene to pre-screen candidate searches and initially build a candidate search queue.

[0049] In real space, objects in a scene can represent the semantic information of the location to a certain extent. Based on this, it can be determined that the difference in the number of identical semantic categories between two semantic graphs can represent the scene differences to a certain extent. Therefore, the semantic graph G relative to the current query scene is defined as query , the semantic features of the prior semantic graph can be constructed as:

[0050] SF=sum(sqrt((Num map -Num query ) 2 )) (1)

[0051] Where Num = {num0, num1, ..., num m} represents the number of corresponding semantic categories in the semantic map, m represents the total number of semantic categories, where the subscript map represents the data in the map, and the subscript query represents the current query; the total number of semantic categories used in this scheme is 9.

[0052] However, in reality, the rarer the semantic category, the more it can represent the semantic information of the scene. For example, the scene with traffic signs often belongs to the intersection. The number of semantic categories in the global prior map is counted first, and the semantic feature weight W is constructed. The steps are as follows:

[0053] Count the number of all semantic categories in the prior semantic graph:

[0054]

[0055] Where n represents the number of prior semantic graphs, and map-all represents all data in the map.

[0056] 3) For Num map-all Normalize, the upper bound of the normalized result is Q H , the lower bound is Q L (Q L >0). In this scheme, 0.9 and 0.1 are selected as the upper and lower bounds, and the processed values ​​are:

[0057]

[0058] 4) Take its reciprocal and perform normalization to obtain the semantic feature weight W.

[0059]

[0060] The final semantic feature weight W satisfies the property that the number of semantic features is inversely proportional to the weight size. Adding W into the formula yields:

[0061] SF=sum(sqrt(W*(Num map -Num query ) 2 )) (5)

[0062] The SF feature can be used to quickly retrieve map frames with similar semantic scenes.

[0063] Step 2: Using the geometric topological relationship between semantic instances in the same scene as described in step 1, a cost matrix based on the semantic topological histogram is constructed to solve vertex matching;

[0064] After the semantic feature pre-screening in step 1, a map frame with similar semantic scenes is obtained. In the case of similar semantic features, further solutions are performed through the correlation between semantic instances in the scene. In the vision-based object-level relocalization algorithm, the Hungarian algorithm is often used to obtain the optimal match through the difference and spatial proximity between two semantic graphs. Referring to this scheme, this scheme adds geometric information between semantic objects on the basis of their semantic correlation, and constructs a new semantic topology histogram as the cost matrix of the Hungarian algorithm. Among them, the semantic topology histogram for each semantic node is defined as a two-dimensional matrix STH.

[0065]

[0066] The x in the STH matrix ij It represents the number of semantic instances with semantic label i in the jth portion divided into h portions by the distance threshold d near the instance object. The above formula can be used to obtain the surrounding semantic description of the corresponding semantic instance, that is, for any node Node query and any node Node in the corresponding map semantic graph map The matching cost of can be defined as a, which is calculated as follows:

[0067]

[0068] For the current scene semantic graph G query and the semantic graph G of the map to be retrieved map The Hungarian cost matrix A is:

[0069]

[0070] The optimal vertex pairing can be obtained by solving the Hungarian algorithm, and then the geometric relationship between the paired points is constructed to remove some erroneous points. Finally, select L CQ The frames with the most frame matching point pairs are selected as the final candidate queue, and pose estimation is further used for similarity evaluation.

[0071] Step 3: Use the rough pose estimate constructed by vertex matching in step 2 as the initial value of the iterative closest point algorithm, and output the final pose estimate through point cloud matching coupled position recognition;

[0072] The center of mass position Center of the semantic instance in the semantic graph can reflect its position in the real world. In step 2, the corresponding matching point set M of the current scene semantic graph and the optimal retrieval map semantic graph can be obtained. query and M map The relative posture between two semantic scenes can be obtained by solving SVD, and the solution equation is defined as:

[0073]

[0074] Among them, R coarse and t coarse To solve the obtained rotation and translation transformations, the variables in the R and t equations are, and represents the matching point set M query and M map Considering that the original point cloud may have a change in viewpoint, which may cause the mass point of the semantic instance to change, it is difficult to obtain a precise solution based only on the SVD solution between semantic instances. We introduce the original point cloud of the current scene and the optimal retrieval map frame to describe its details as much as possible, and convert R coarse and t coarse As the initial pose of the GICP algorithm to obtain a refined pose estimate:

[0075]

[0076] where p query is the point of the original point cloud P of the current scene, q map is the point Q in the map point cloud, R gicp , t gicp is the final 6-Dof pose.

[0077] Since only the semantic point positions and semantic labels are considered when extracting semantic instance objects, the shape details of the semantic instances are ignored. Therefore, in some scenes, there may be situations where the semantic geometric distribution is similar. Therefore, this solution introduces the score in the GICP algorithm. fitness As the last step of similarity evaluation, if the score in the candidate queue fitness Less than the fitting threshold δ fitness Then directly select the final result, otherwise select score fitness The smallest candidate frame.

[0078] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. A global positioning method based on semantic distribution feature retrieval and semantic topology histogram, characterized in that: include: Step 1: Use the global semantic distribution features in the scene to pre-screen candidate searches and preliminarily build a candidate search queue; Step 2: Using the geometric topological relationship between semantic instances in the same scene, a cost matrix based on the semantic topological histogram is constructed to solve vertex matching; Step 3: Use the rough pose estimate constructed by vertex matching as the initial value of the iterative closest point algorithm, and output the final pose estimate through point cloud matching coupled position recognition.

2. The global positioning method based on semantic distribution feature retrieval and semantic topology histogram as claimed in claim 1, characterized in that: Step 1 uses the global semantic distribution features in the scene to pre-screen candidate searches and preliminarily build a candidate search queue; the details are as follows: Define the semantic graph G relative to the current query scene query , the semantic features of the prior semantic graph are constructed as: SF=sum(sqrt((Num map -Number query ) 2 )) (1) Where Num={num0,num1,...,num m } represents the number of corresponding semantic categories in the semantic map, and m represents the total number of semantic categories; the subscript map represents the data in the map, and the subscript query represents the current query; Prioritize counting the number of semantic categories in the global prior map and constructing the semantic feature weight W; The steps are as follows: Count the number of all semantic categories in the prior semantic graph: Where n represents the number of prior semantic graphs; map-all means all data in the map; 1) For Num map-all Normalize, the upper bound of the normalized result is Q H , the lower bound is Q L Where Q L >0, the processed value is: 2) Take its reciprocal and perform normalization to obtain the semantic feature weight W; The final semantic feature weight W satisfies the property that the number of semantic features is inversely proportional to the weight size. Adding W into the formula yields: SF=sum(sqrt(W*(Num map -Num query ) 2 )) (5) The SF feature can be used to quickly retrieve map frames with similar semantic scenes.

3. The global positioning method based on semantic distribution feature retrieval and semantic topology histogram as claimed in claim 2, characterized in that: Step 2 is as follows: Based on the semantic correlation, the geometric information between semantic objects is added, and a new semantic topology histogram is constructed as the cost matrix of the Hungarian algorithm; wherein, the semantic topology histogram for each semantic node is defined as a two-dimensional matrix STH; The x in the STH matrix ij represents the number of semantic instances with semantic label i in the jth portion divided into h portions by the distance threshold d near the instance object; the surrounding semantic description of the corresponding semantic instance is obtained from the above formula, that is, for any node Node in the current scene semantic graph query and any node Node in the corresponding map semantic graph map The matching cost of is defined as a, which is calculated as follows: For the current scene semantic graph G query and the semantic graph G of the map to be retrieved map The Hungarian cost matrix A is: The optimal vertex pairing is obtained by solving the Hungarian algorithm, and then the geometric relationship between the paired points is constructed to remove some erroneous points; and the paired points are stored in the corresponding matching point set M. query and M map ; Finally select L CQ The frames with the most frame matching point pairs are selected as the final candidate queue, and pose estimation is further used for similarity evaluation.

4. The global positioning method based on semantic distribution feature retrieval and semantic topology histogram as claimed in claim 3, characterized in that: Step 3 is as follows: Get the corresponding matching point set M between the current scene semantic map and the optimal retrieval map semantic map query and M map ; According to the SVD solution, the relative posture between two semantic scenes is obtained, and the solution equation is defined as: Among them, R coarse and t coarse To solve the obtained rotation and translation transformations, the variables in the equations R and t, c j query and c j map represents the matching point set M query and M map The original point cloud of the current scene and the optimal retrieval map frame is introduced to describe its details, and R coarse and t coarse As the initial pose of the GICP algorithm to obtain a fine pose estimate: where p query is the point of the original point cloud P of the current scene, q map is the point Q in the map point cloud, R gicp ,t gicp is the final 6-Dof pose; Introducing the score in the GICP algorithm fitness As the last step of similarity evaluation, if the score in the candidate queue fitness Less than the fitting threshold δ fitness Then select it as the final result directly, otherwise select score fitness The smallest candidate frame.

Images