Global localization method based on semantic distribution feature retrieval and semantic topology histogram

Abstract

The present invention discloses a global positioning method based on semantic distribution feature retrieval and semantic topology histogram, comprising the following steps: first, pre-screening candidate retrievals using global semantic distribution features in a scene, and preliminarily constructing a candidate retrieval queue; second, constructing a cost matrix based on the semantic topology histogram using the geometric topological relationship between semantic instances in the same scene, and then solving vertex matching; finally, using the rough pose estimate constructed by vertex matching as the initial value of an iterative closest point algorithm, coupling position recognition through point cloud matching, and outputting the final pose estimate; this method effectively overcomes the problems faced by current one-time global positioning based on lidar in the absence of prior pose, such as local feature matching error, sensitivity to dynamic objects, and insufficient computational efficiency. By introducing semantic information and utilizing the semantic information of objects in the environment to assist one-time global positioning without initial pose, the accuracy and robustness of global positioning are improved.

Description

Technical Field

[0001] The present 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-shot global localization plays a key role in most robotics applications, offering significant advantages during initialization and relocalization. However, current LiDAR-based one-shot global localization suffers from local feature matching errors, sensitivity to dynamic objects, and insufficient computational efficiency without prior pose.

[0003] Existing one-shot global localization methods have limitations when dealing with complex environments. First, they may result in incorrect candidate retrieval when local features are similar. Second, localization performance is also challenging in complex environments affected by dynamic objects. Therefore, we propose a global localization method based on semantic distribution feature retrieval and semantic topology histograms to address these issues. 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. By introducing semantic information and utilizing the semantic information of objects in the environment to assist in one-time global positioning without initial pose, the accuracy and robustness of global positioning are improved.

[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 topology histogram to solve 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 preliminarily 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 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 difference in their scenes 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 better it can represent the semantic information of the scene. For example, scenes with traffic signs often belong to intersections. First, count the number of semantic categories in the global prior map and construct the semantic feature weight W. 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 to the formula yields:

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

[0025] SF features 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 topology 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 maps. 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] Where x in the STH matrix ij It represents the number of semantic instances with semantic label i in the jth part divided into h parts 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 in the current scene semantic graph, query and any node in the corresponding map semantic graph map The matching cost 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 can be constructed to remove some error points. Finally, select L CQ The frames with the most matching point pairs are selected as the final candidate queue, and the 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 with 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 in the current scene semantic graph and the optimal retrieval map semantic graph can be obtained. query and M map The relative pose 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 transformation and translation transformation, 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 of the semantic instance to change, it is difficult to obtain a precise solution based 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 fine 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 directly select the final result, otherwise select score fitness The smallest candidate frame.

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

[0042] The present invention demonstrates superior performance on all six tested datasets (DCC02, KAIST03, NTU02, NTU10, APO01, and APO02): the median of its translation error generally remains below 0.5, and the span of its box and whiskers is significantly smaller than that of comparable methods (such as BTC and BEVPlace). This not only reflects the algorithm's high positioning accuracy, but also demonstrates good stability and consistency in its results. In particular, in the challenging DCC02 dataset, when the error fluctuation range of other methods reaches 4-5, the present invention still maintains a smaller error range, fully demonstrating the robustness and reliability of the present invention in translation error control. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 This is a flowchart of the implementation of 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-consuming analysis diagram of each module in the method described in the present invention. DETAILED DESCRIPTION

[0046] The present invention will be further described below with reference to 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 has the following specific steps:

[0048] Step 1: Use the global semantic distribution features in the scene to pre-screen candidate retrievals and preliminarily build a candidate retrieval 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 concluded that the difference in the number of the same semantic categories between two semantic graphs can represent the difference in their scenes 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 better it can represent the semantic information of the scene. For example, scenes with traffic signs often belong to intersections. First, count the number of semantic categories in the global prior map and construct the semantic feature weight W. 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 maps, 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 solution, 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 to the formula yields:

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

[0062] SF features 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 topology 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 maps. 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] Where x in the STH matrix ij It represents the number of semantic instances with semantic label i in the jth part divided into h parts 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 in the current scene semantic graph, query and any node in the corresponding map semantic graph map The matching cost 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 can be constructed to remove some error points. Finally, select L CQ The frames with the most matching point pairs are selected as the final candidate queue, and the 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 in the current scene semantic graph and the optimal retrieval map semantic graph can be obtained. query and M map The relative pose 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 equations R and t 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 of the semantic instance to change, it is difficult to obtain a precise solution based 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 fine 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 merely illustrates the technical idea of ​​the present invention and cannot be used to limit the scope of protection of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications all fall within the scope of protection of the claims of the present invention.

Claims

1. A global positioning method based on semantic distribution feature retrieval and semantic topology histogram, characterized by: include: Step 1: Use the global semantic distribution features in the scene to pre-screen candidate searches and initially build a candidate search queue; Step 2: Utilize the geometric topological relationships between semantic instances in the same scene to construct a cost matrix based on the semantic topology histogram, and then solve vertex matching; the details are as follows: Based on the semantic correlation, the geometric information between semantic objects is added to construct a new semantic topology histogram as the cost matrix of the Hungarian algorithm. The semantic topology histogram of each semantic node is defined as a two-dimensional matrix STH. Where x in the STH matrix ij It represents the number of semantic instances with semantic label i in the jth part divided into h parts by the distance threshold d near the instance object; the surrounding semantic description of the corresponding semantic instance is obtained by the above formula, that is, for any node Node in the current scene semantic graph query and any node in the corresponding map semantic graph map The matching cost 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 error points; and the constructed paired points are stored in the corresponding matching point set M query and M map ;Finally select L CQ The frames with the most matching points are selected as the final candidate queue, and pose estimation is further used for similarity evaluation; 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 according to 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 )) (4) 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 )) (8) SF features 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 according to claim 1, characterized in that: Step 3 is as follows: Obtain 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 to obtain the relative posture between two semantic scenes, 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 are, and represents the matching point set M query and M map The jth point in the corresponding scene; 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 directly select the final result, otherwise select score fitness The smallest candidate frame.

Images