Pose map pruning methods, equipment, and storage media

Abstract

This application relates to the field of high-precision map technology and discloses a pose graph pruning method, device, and storage medium. The method includes: calculating the pose confidence of each node in the initial pose graph, dividing each node into a first confidence node and a second confidence node according to the pose confidence, searching for overlapping region nodes among multiple pose graphs to be processed, marking the overlapping region nodes as selected nodes, using the overlapping region nodes as seed nodes to search for target nodes through breadth-first search, marking the target nodes as selected nodes, and pruning the initial pose graph according to the edges between the selected nodes to obtain a pruned pose graph. Since this application prunes the pose graph according to the node pose confidence and whether it is in the overlapping region during the breadth-first search, it can remove redundant nodes and edges in the pose graph, thereby improving the efficiency of pose graph optimization.

Description

Technical Field

[0001] This application relates to the field of high-precision mapping technology, and in particular to a pose graph pruning method, device and storage medium. Background Technology

[0002] Currently, high-precision maps are typically generated by processing point clouds, images, IMU (Inertial Measurement Unit), wheel speed, and GPS (Global Positioning System) data using SLAM (Simultaneous Localization and Mapping) technology. SLAM technology usually models the mapping process as a pose graph optimization problem.

[0003] Related pose graph optimization methods typically decompose the original pose graph into multiple sub-pose graphs for separate optimization. However, the above methods require optimization of all nodes, including redundant nodes, resulting in low optimization efficiency. Summary of the Invention

[0004] The main purpose of this application is to provide a pose graph pruning method, device and storage medium, which aims to solve the technical problem that related pose graph optimization methods require optimization of all nodes, including redundant nodes, resulting in low optimization efficiency.

[0005] To achieve the above objectives, this application provides a pose graph pruning method, which includes:

[0006] Calculate the pose confidence of each node in the initial pose graph, and divide each node into a first confidence node and a second confidence node based on the pose confidence. The initial pose graph is a set of multiple pose graphs to be processed. The initial pose graph includes multiple nodes and edges between nodes. The nodes are used to represent the pose of keyframes, and the edges are used to represent the relative constraints between poses. The pose confidence of the first confidence node is greater than a preset confidence, and the pose confidence of the second confidence node is not greater than the preset confidence.

[0007] Search for overlapping region nodes among the multiple pose graphs to be processed, and mark the overlapping region nodes as selected nodes;

[0008] Using the overlapping region nodes as seed nodes, a breadth-first search is used to search for target nodes, and the target nodes are marked as selected nodes. The breadth-first search method involves searching for second confidence nodes adjacent to the seed node with the seed node as the search center, until all nodes have been searched or a preset node is found. The preset node is a first confidence node that is not in the overlapping region. The target node includes the preset node and the second confidence nodes passed during the search of the preset node.

[0009] The initial pose graph is pruned based on the edges between the selected nodes to obtain a pruned pose graph.

[0010] Optionally, the step of using the overlapping region nodes as seed nodes to search for the target node using a breadth-first search method includes:

[0011] Add the overlapping region nodes to the queue, and pop the head node of the queue in sequence;

[0012] When the head node does not meet the preset conditions, the unselected nodes connected to the head node are added to the queue, and the unselected nodes added to the queue are marked as selected nodes. The process returns to the step of popping the head node from the queue in turn until the queue is empty. The preset conditions are that the head node is not in the overlapping area and the head node is a first confidence node.

[0013] Optionally, the step of calculating the pose confidence of each node in the initial pose graph, and dividing each node into a first confidence node and a second confidence node based on the pose confidence, includes:

[0014] The positioning signal quality of each node in the initial pose graph is obtained, wherein the positioning signal quality is the performance index of the positioning signal at the location of the node.

[0015] Calculate the algorithmic estimated pose of each node and obtain the localization measurement value of each node;

[0016] Calculate the position error between the pose estimated by the algorithm and the positioning measurement value;

[0017] The pose confidence of each node in the initial pose graph is calculated based on the positioning signal quality and the position error.

[0018] Determine whether the pose confidence level is greater than a preset confidence level, and divide each node into a first confidence level node and a second confidence level node based on the determination result.

[0019] Optionally, obtaining the positioning signal quality of each node in the initial pose graph includes:

[0020] Obtain the solution identifier of each node in the initial pose graph and the number of satellites corresponding to the node's location, wherein the solution identifier is used to represent the data processing index during the parsing of the positioning signal;

[0021] The positioning signal quality of each node is analyzed based on the solution identifier and the number of satellites.

[0022] Optionally, the algorithm for calculating the pose estimate of each node includes:

[0023] Acquire data from multiple sensors and extract significant feature points from the sensor data;

[0024] Matching feature points corresponding to the salient feature points between consecutive frames or different sensor data;

[0025] Feature point pairs are formed based on the salient feature points and the matching feature points, and the algorithmic estimated pose of each node is calculated based on the feature point pairs.

[0026] Optionally, the search for overlapping region nodes among the plurality of pose maps to be processed includes:

[0027] A tree-shaped data structure is constructed based on the location information of each node, wherein the tree-shaped data structure is a hierarchical structure in a multi-dimensional space;

[0028] The overlapping region nodes between the multiple pose graphs to be processed are searched using the tree data structure.

[0029] Optionally, constructing a tree data structure based on the position information of each node includes:

[0030] Select one dimension of the multidimensional space as the dividing axis to sort the position information of each node, and select the middle point as the root node according to the sorting result. Then, divide the remaining nodes except the root node into a left subset and a right subset according to the sorting result.

[0031] The left subtree is obtained by recursively performing the step of selecting one dimension of the multidimensional space as the dividing axis to sort the position information of each node, and dividing the remaining nodes except the root node into the left and right subtrees according to the sorting result, until the remaining leaf nodes in the left subtree are obtained.

[0032] The steps of selecting one dimension of the multidimensional space as the dividing axis to sort the position information of each node are recursively executed for the nodes in the right subset. Based on the sorting result, the remaining nodes except the root node are divided into the left subset and the right subset until the remaining leaf nodes in the right subset are obtained, thus obtaining the right subtree.

[0033] A tree data structure is constructed based on the root node, the left subtree, and the right subtree.

[0034] Optionally, the step of searching for overlapping region nodes among the multiple pose maps to be processed through the tree data structure includes:

[0035] Traverse each node and use the traversed node as the node to be searched;

[0036] The values ​​of the node to be searched and the root node of the tree data structure on the split axis are compared, and the neighboring nodes on the tree data structure are recursively searched according to the comparison results, wherein the neighboring nodes are nodes within a preset neighborhood range of the node to be searched.

[0037] When the node to be searched and the neighboring nodes do not belong to the same pose graph to be processed, the node to be searched is determined to be an overlapping region node.

[0038] Optionally, before calculating the pose confidence of each node in the initial pose graph and dividing each node into a first confidence node and a second confidence node based on the pose confidence, the method further includes:

[0039] When simultaneously fusing and pruning multiple acquisition task pose maps corresponding to multiple acquisition tasks, multiple acquisition task pose maps corresponding to multiple acquisition tasks are obtained.

[0040] An initial pose graph is constructed based on the multiple acquisition task pose graphs, wherein the initial pose graph is a set of the multiple acquisition task pose graphs.

[0041] Optionally, before calculating the pose confidence of each node in the initial pose graph and dividing each node into a first confidence node and a second confidence node based on the pose confidence, the method further includes:

[0042] When performing incremental fusion pruning on multiple acquisition task pose maps of multiple acquisition tasks, the fused pose map and the pose map to be fused are obtained. The fused pose map is the pruned pose map obtained from the previous fusion pruning, and the pose map to be fused is the acquisition task pose map corresponding to the acquisition task to be fused and pruned at the current time.

[0043] An initial pose graph is constructed based on the fused pose graph and the pose graph to be fused, wherein the initial pose graph is a set of the fused pose graph and the pose graph to be fused;

[0044] Accordingly, after pruning the initial pose graph based on the edges between the selected nodes to obtain the pruned pose graph, the process further includes:

[0045] The pose graph loops between the fused pose graph and the pose graph to be fused are adjusted to pose graph intraloops within the pruned pose graph. The adjusted pruned pose graph is then used as the new fused pose graph. The process of obtaining the fused pose graph and the pose graph to be fused is repeated until the pose graphs of all acquisition tasks are fused and pruned. Here, the pose graph loops represent the relative constraints between nodes in the pose graphs, and the pose graph intraloops represent the relative constraints between nodes within the pose graph.

[0046] Furthermore, to achieve the above objectives, this application also proposes a pose graph pruning device, which includes:

[0047] A node partitioning module is used to calculate the pose confidence of each node in the initial pose graph, and to partition each node into a first confidence node and a second confidence node based on the pose confidence. The initial pose graph is a set of multiple pose graphs to be processed, and includes multiple nodes and edges between nodes. The nodes are used to represent the pose of keyframes, and the edges are used to represent the relative constraints between poses. The pose confidence of the first confidence node is greater than a preset confidence, and the pose confidence of the second confidence node is not greater than the preset confidence.

[0048] The overlap search module is used to search for overlapping region nodes among the multiple pose maps to be processed, and to mark the overlapping region nodes as selected nodes.

[0049] A breadth-first search module is used to search for target nodes using the overlapping region nodes as seed nodes in a breadth-first search manner, and to mark the target nodes as selected nodes. The breadth-first search method involves searching for second confidence nodes adjacent to the seed node with the seed node as the search center, until all nodes have been searched or a preset node has been found. The preset node is a first confidence node that is not in the overlapping region. The target node includes the preset node and the second confidence nodes that were passed during the search of the preset node.

[0050] The pruning module is used to prune the initial pose graph based on the edges between the selected nodes to obtain a pruned pose graph.

[0051] In addition, to achieve the above objectives, this application also proposes a pose graph pruning device, which includes a memory, a processor, and a pose graph pruning program stored in the memory and executable on the processor. The pose graph pruning program is configured to implement the pose graph pruning method as described above.

[0052] In addition, to achieve the above objectives, this application also proposes a storage medium storing a pose graph pruning program, which, when executed by a processor, implements the pose graph pruning method as described above.

[0053] In addition, to achieve the above objectives, this application also provides a computer program product, which includes a pose graph pruning program, which, when executed by a processor, implements the pose graph pruning method as described above.

[0054] One or more technical solutions proposed in this application have at least the following technical effects:

[0055] This application discloses a method for calculating the pose confidence of each node in the initial pose graph, dividing each node into a first-confidence node and a second-confidence node based on the pose confidence, searching for overlapping nodes among multiple pose graphs to be processed, and marking the overlapping nodes as selected nodes. Using the overlapping nodes as seed nodes, a breadth-first search is used to search for target nodes, and the target nodes are marked as selected nodes. The initial pose graph is pruned based on the edges between the selected nodes to obtain a pruned pose graph. Since this application prunes the pose graph using a breadth-first search, the decision to add the searched nodes to the pruned pose graph is based on the node pose confidence and whether they are in the overlapping region. This removes redundant nodes and edges from the pose graph, thereby improving the efficiency of pose graph optimization while reducing memory consumption. Attached Figure Description

[0056] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0057] To more clearly illustrate the technical solutions in the embodiments of this application or related technologies, the accompanying drawings used in the description of the embodiments or related technologies will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0058] Figure 1 This is a flowchart illustrating the first embodiment of the pose graph pruning method of this application.

[0059] Figure 2 This is a schematic diagram illustrating the pose confidence determination of simultaneous fusion and pruning of multiple pose graphs in an embodiment of the pose graph pruning method of this application.

[0060] Figure 3 This is a schematic diagram of the search for overlapping region nodes in a multi-pose graph fusion and pruning method according to an embodiment of the pose graph pruning method of this application.

[0061] Figure 4 This is a schematic diagram of a breadth-first search for simultaneous fusion and pruning of multiple pose graphs in an embodiment of the pose graph pruning method of this application.

[0062] Figure 5 This is a breadth-first search result graph of multiple pose graphs simultaneously fused and pruned in an embodiment of the pose graph pruning method of this application.

[0063] Figure 6 This is an illustration of the effect of simultaneous fusion and pruning of multiple pose graphs in one embodiment of the pose graph pruning method of this application.

[0064] Figure 7 This is a flowchart illustrating an embodiment of the pose graph pruning method of this application.

[0065] Figure 8 This is a schematic diagram illustrating the pose confidence determination in incremental fusion pruning of multiple pose graphs, as described in an embodiment of the pose graph pruning method of this application.

[0066] Figure 9 This is a schematic diagram of the overlapping region node search in an embodiment of the pose graph pruning method of this application, which involves incremental fusion pruning of multiple pose graphs.

[0067] Figure 10 This is a schematic diagram of breadth-first search for incremental fusion pruning of multiple pose graphs in an embodiment of the pose graph pruning method of this application.

[0068] Figure 11 This is an illustration of the effect of multi-pose graph incremental fusion pruning in one embodiment of the pose graph pruning method of this application.

[0069] Figure 12 This is a flowchart illustrating the second embodiment of the pose graph pruning method of this application;

[0070] Figure 13 This is a flowchart illustrating the third embodiment of the pose graph pruning method of this application;

[0071] Figure 14 This is a flowchart illustrating the fourth embodiment of the pose graph pruning method of this application;

[0072] Figure 15 This is a schematic diagram of a KD tree representing an embodiment of the pose graph pruning method of this application;

[0073] Figure 16 This is a schematic diagram of the module structure of the pose graph pruning device according to an embodiment of this application;

[0074] Figure 17 This is a schematic diagram of the device structure of the hardware operating environment involved in the pose graph pruning method in the embodiments of this application.

[0075] The realization of the purpose, functional features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0076] It should be understood that the specific embodiments described herein are merely illustrative of the technical solutions of this application and are not intended to limit this application.

[0077] To better understand the technical solution of this application, a detailed description will be provided below in conjunction with the accompanying drawings and specific implementation methods.

[0078] Currently, high-precision maps are typically generated from point clouds, images, IMU (Inertial Measurement Unit), wheel speed, and GPS (Global Positioning System) data through SLAM (Simultaneous Localization and Mapping) technology. SLAM technology usually models the mapping process as a pose graph optimization problem. Nodes in the pose graph represent the pose of keyframes, and edges represent the relative constraints between poses, usually including odometry edges and loop closure edges. The amount of data required to build city-level high-precision maps is enormous, requiring multiple data acquisitions and mappings, followed by fusion of the pose graphs from multiple mappings. This results in a very large pose graph to be fused, and the memory consumption of pose graph optimization exceeds the capacity limit of a single processor.

[0079] For large-scale pose graph solving problems, a divide-and-conquer strategy can be adopted, that is, the original pose graph is decomposed into multiple sub-pose graphs and optimized separately. For example, firstly, the pose graph to be optimized is divided into multiple sub-pose graphs according to a preset segmentation method. The sub-pose graphs include boundary sub-graphs close to the sub-pose graph segmentation boundary and internal sub-pose graphs located inside the boundary sub-pose graphs. Then, anchor frames are determined from the boundary sub-pose graphs and internal sub-pose graphs respectively. Next, the poses of all anchor frames in the pose graph to be optimized are optimized. Finally, the boundary anchor frame poses are fixed, and the poses of the internal anchor frames are optimized.

[0080] Although the above pose graph optimization method reduces the memory consumption of pose graph optimization through the divide-and-conquer strategy, it does not prune the pose graph. Instead, it optimizes all nodes, including redundant nodes, resulting in low optimization efficiency.

[0081] Therefore, to overcome the above-mentioned defects, this application provides a solution, which includes: calculating the pose confidence of each node in the initial pose graph, dividing each node into a first confidence node and a second confidence node according to the pose confidence, searching for overlapping region nodes among multiple pose graphs to be processed, and marking the overlapping region nodes as selected nodes, using the overlapping region nodes as seed nodes to search for target nodes through breadth-first search, and marking the target nodes as selected nodes, and pruning the initial pose graph according to the edges between the selected nodes to obtain a pruned pose graph; since this application prunes the pose graph through breadth-first search, in the breadth-first search, it determines whether to add the searched nodes to the pruned pose graph according to the node pose confidence and whether they are in the overlapping region, thereby removing redundant nodes and edges in the pose graph, and thus improving the pose graph optimization efficiency while reducing memory consumption.

[0082] It should be noted that the execution subject of this application embodiment may be a pose graph pruning device with data processing, network communication and program running functions, such as a computer, or other electronic devices that can achieve the same or similar functions. This embodiment does not limit this.

[0083] Based on this, embodiments of this application provide a pose graph pruning method, referring to... Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the pose graph pruning method of this application.

[0084] In the first embodiment, the pose graph pruning method includes:

[0085] Step S10: Calculate the pose confidence of each node in the initial pose graph, and divide each node into a first confidence node and a second confidence node according to the pose confidence.

[0086] The initial pose graph is a collection of multiple pose graphs to be processed. The initial pose graph includes multiple nodes and edges between nodes. The nodes are used to represent the pose of keyframes, and the edges are used to represent the relative constraints between poses. The pose confidence of the first confidence node is greater than the preset confidence, and the pose confidence of the second confidence node is not greater than the preset confidence.

[0087] It should be understood that the application scenario of this application embodiment can be to perform fusion and pruning of the acquisition task pose graphs when constructing a high-precision map through multiple acquisition task pose graphs corresponding to multiple acquisition tasks.

[0088] The data acquisition task can be the acquisition of environmental data through multiple sensors (such as LiDAR, cameras, GPS, IMU, etc.); pose can be used to describe the position and orientation of the sensor relative to a reference coordinate system, where position can be represented by a 3×1 vector t, and orientation can be represented by a 3×3 rotation matrix R; pose graph is a graph structure used to represent the pose of the sensor in the environment, which includes multiple nodes and edges between nodes. Nodes are used to represent the pose of the sensor in keyframes, which can be representative and important frames in the SLAM problem, containing sufficient spatial and motion information; edges can be used to represent the relative constraints between poses, and edges can include odometry edges and loop closure edges. Odometry edges can be edges connecting two consecutive pose nodes in the pose graph, representing the relative transformation of the sensor moving from one pose to the next; loop closure edges can be edges connecting two non-consecutive pose nodes in the pose graph, used to represent the relative transformation of the sensor observing the same location at two different time points.

[0089] It is understandable that calculating the pose confidence of each node in the initial pose graph can involve obtaining the pose association information of each node in the initial pose graph, performing a confidence analysis on the pose association information, and calculating the pose confidence of each node in the initial pose graph based on the analysis results. The pose association information can be information related to the pose confidence of each node. This pose association information includes, but is not limited to, sensor data quality, positional error between the pose estimated by the SLAM algorithm and the GPS measurement value, etc. Sensor data quality includes LiDAR data quality (such as point cloud density, noise level, etc.), image data quality (such as sharpness and resolution, etc.), and positioning signal quality (such as GPS signal quality), etc. This embodiment does not impose any limitations on this. Performing a confidence analysis on the pose association information can be done through a preset network model. The preset network model can be a pre-set neural network model used to analyze the quality of the data.

[0090] Dividing nodes into first-confidence nodes and second-confidence nodes based on pose confidence can be achieved by classifying nodes whose pose confidence is greater than a preset confidence level as first-confidence nodes and nodes whose pose confidence is not greater than the preset confidence level as second-confidence nodes. In other words, nodes whose pose confidence is greater than the preset confidence level are classified as high-pose confidence nodes, and nodes whose pose confidence is not greater than the preset confidence level are classified as low-pose confidence nodes. The preset confidence level can be set in advance to distinguish between high-pose confidence nodes and low-pose confidence nodes.

[0091] For ease of understanding, please refer to Figure 2 Examples are provided for illustration, but this does not limit the scope of this application. Figure 2This diagram illustrates the pose confidence determination during simultaneous fusion pruning of multiple pose graphs in an embodiment of the pose graph pruning method of this application. As an example, it assumes simultaneous fusion pruning of pose graphs corresponding to multiple acquisition tasks. There are three acquisition tasks (acquisition task 1, acquisition task 2, and acquisition task 3). The initial pose graph is a set of pose graphs for acquisition task 1, acquisition task 2, and acquisition task 3. The pose graph for acquisition task 1 includes 9 pose nodes and 1 internal loop edge; the pose graph for acquisition task 2 includes 10 pose nodes and 1 internal loop edge. The pose graph contains 9 pose nodes and 1 pose graph closure edge. The positions of multiple pose graphs in the initial pose graph can be determined based on the pose graph closure edges between the pose graphs of the multiple acquisition tasks. There is one pose graph closure edge between the pose graphs of acquisition task 1 and acquisition task 2, and another between the pose graphs of acquisition task 2 and acquisition task 3. The pose graph closure edges represent the relative constraints between nodes in the pose graphs, while the pose graph closure edges represent the relative constraints between nodes within the pose graph.

[0092] To determine the pose confidence of each node in the initial pose graph, the specific steps are as follows: perform a confidence analysis on the pose association information, calculate the pose confidence of each node in the initial pose graph based on the analysis results, where the pose association information can be information related to the pose confidence of each node, classify nodes with pose confidence greater than a preset confidence level as high pose confidence nodes, and classify nodes with pose confidence not greater than the preset confidence level as low pose confidence nodes, such as... Figure 2 As shown, red nodes represent nodes with high pose confidence, while blue nodes represent nodes with low pose confidence.

[0093] Step S20: Search for overlapping region nodes among the multiple pose maps to be processed, and mark the overlapping region nodes as selected nodes.

[0094] It should be understood that searching for overlapping region nodes among multiple pose maps to be processed can be done by traversing each node in the initial pose map and taking the traversed nodes as the nodes to be searched. It is then determined whether there are other nodes in the preset neighborhood of the node to be searched that do not belong to the same acquisition task pose map as the node to be searched. If they exist, the node to be searched is determined to be an overlapping region node; if they do not exist, the node to be searched is determined not to be an overlapping region node. The preset neighborhood can be set in advance according to actual needs, and this embodiment does not limit it.

[0095] Of course, in order to improve the search efficiency of overlapping region nodes, in this embodiment, the search for overlapping region nodes between multiple pose maps to be processed can first construct a tree data structure based on the position information of each node, and then search for overlapping region nodes between multiple pose maps to be processed through the tree data structure. The tree data structure is a hierarchical structure in multidimensional space. In specific implementation, for example, a KD tree can be constructed first based on the position information of each node, and then the overlapping region nodes of multiple pose maps can be searched through the KD tree. The KD tree is a tree data structure used to organize K-dimensional spatial data, mainly used for point query, range search and nearest neighbor search in multidimensional space.

[0096] For ease of understanding, please refer to Figure 3 Examples are provided for illustration, but this does not limit the scope of this application. Figure 3 This diagram illustrates the search for overlapping region nodes in a multi-pose graph fusion and pruning method according to an embodiment of the pose graph pruning method of this application. The diagram shows that a KD-tree is used to search for overlapping region nodes in each pose graph. Specifically, the position information of all nodes in the pose graph is used to form a KD-tree, and the KD-tree is used to search for overlapping region nodes in multiple acquisition task pose graphs. The specific search condition is that if a node has other nodes within its preset neighborhood that do not belong to the same acquisition task pose graph, then that node is in the overlapping region and is considered an overlapping region node. Figure 3 As shown, the nodes in the yellow area are the overlapping nodes found during the search. The overlapping nodes found are marked as selected nodes and marked in black. They are the seed nodes for subsequent breadth-first search.

[0097] Step S30: Using the overlapping region nodes as seed nodes, search for target nodes using a breadth-first search method, and mark the target nodes as selected nodes;

[0098] The breadth-first search method involves searching for second confidence nodes adjacent to the seed node, with the seed node as the search center, until all nodes have been searched or a preset node is found. The preset node is a first confidence node that is not in the overlapping area. The target node includes the preset node and the second confidence nodes passed during the search of the preset node.

[0099] Understandably, breadth-first search is an algorithm that systematically traverses all vertices in a graph. It searches in a hierarchical manner, first visiting the seed node of the graph, then visiting the nodes directly adjacent to the seed node, then visiting the neighboring nodes of these nodes, and so on, until all nodes have been visited or a preset node is found. In this embodiment, the preset node is the first confidence node that is not in the overlapping area, and the target node includes the preset node and the second confidence node that is visited when searching for the adjacent preset node.

[0100] For ease of understanding, please refer to Figure 4 and Figure 5 Examples are provided for illustration, but this does not limit the scope of this application. Figure 4 This is a schematic diagram of a breadth-first search for simultaneous fusion pruning of multiple pose graphs in an embodiment of the pose graph pruning method of this application. As an example, the target node is searched using a breadth-first search method with node D1 as the seed node. Using D1 as the search center, the low pose confidence node adjacent to D1 is searched, and D2 can be found. Then, using D2 as the search center, the preset node D3 (a high pose confidence node not in the overlapping area) can be directly searched. At this time, the target node includes the preset node D3 and D2, which is passed through when searching adjacent to D3. It can be seen that D2 and D3 can be marked as selected nodes through the above operation.

[0101] Figure 5 This embodiment of the pose graph pruning method of this application uses a breadth-first search result graph of simultaneous fusion pruning of multiple pose graphs. The specific steps are as follows: using overlapping region nodes as seed nodes, the breadth-first search is performed. First, the seed node of the graph is visited, then the nodes directly adjacent to the seed node are visited, then the neighboring nodes of these nodes are visited, and so on, until all nodes have been visited or a preset node is found. The preset node is a first-confidence node not located within the overlapping region. The target node includes the preset node and the second-confidence nodes visited during the search for the adjacent preset node. The target node is also marked as a selected node. Figure 5 As shown, the target node found in the search is selected and marked in black.

[0102] Step S40: Prune the initial pose graph according to the edges between the selected nodes to obtain a pruned pose graph.

[0103] It should be understood that pose graph pruning refers to reducing redundant nodes and edges in the pose graph, aiming to simplify the graph structure, improve computational efficiency, and maintain the accuracy of pose estimation. Therefore, in this embodiment, pruning the initial pose graph based on the edges between selected nodes can be done by removing all nodes and edges in the initial pose graph except for the edges between selected nodes, and forming a new pose graph by combining all the edges between selected nodes. This new pose graph is the pruned pose graph of the initial pose graph.

[0104] For ease of understanding, please refer to Figure 6 Examples are provided for illustration, but this does not limit the scope of this application. Figure 6This is an illustration of the effect of simultaneous fusion and pruning of multiple pose graphs in an embodiment of the pose graph pruning method of this application. In the figure, all nodes and edges in the initial pose graph are removed except for the edges between selected nodes. All selected nodes and their corresponding edges are then combined to form a new pose graph, which is the pruned pose graph of the initial pose graph. Figure 6 As shown, the number of nodes in the pruned pose graph is 23, which is 5 fewer redundant nodes than the 28 in the initial pose graph.

[0105] For ease of understanding, please refer to Figure 7 This explanation is provided, but does not limit the scope of this application. Figure 7 This is a flowchart illustrating an embodiment of the pose graph pruning method of this application. In the flowchart, the embodiment first constructs an initial pose graph containing pose graphs from multiple acquisition tasks. Then, based on the GPS signal quality of the nodes (e.g., resolution markers, number of satellites, etc.) and the error magnitude between the pose estimated by the SLAM algorithm and the GPS measurement value, nodes with high pose confidence and nodes with low pose confidence are divided. Next, the position information of all nodes in the pose graph is used to form a KD-tree. The KD-tree is then used to search for overlapping nodes in the pose graphs of multiple acquisition tasks. Specifically, if a node has other nodes within its preset neighborhood that do not belong to the same acquisition task pose graph, then that node is in the overlapping area, and that node is considered an overlapping node. Points are identified as overlapping region nodes, which are marked as selected nodes. Then, using the overlapping region nodes as seed nodes, a breadth-first search is performed. First, the seed nodes of the graph are visited, then the nodes directly adjacent to the seed node are visited, then the neighboring nodes of these nodes are visited, and so on, until all nodes have been visited or a preset node is found. The preset node is the first confidence node that is not in the overlapping region. The target node includes the preset node and the second confidence nodes passed during the search of the adjacent preset node, and the target node is also marked as a selected node. Finally, a new pose graph is formed based on the selected nodes and the edges between the selected nodes. This pose graph is the pruned pose graph of the initial pose graph.

[0106] Furthermore, in order to improve the fusion and pruning efficiency of multiple acquisition task pose maps, in this embodiment, multiple acquisition task pose maps corresponding to multiple acquisition tasks can be simultaneously fused and pruned. Before step S10, the method includes: when simultaneously fusion and pruning multiple acquisition task pose maps corresponding to multiple acquisition tasks, obtaining multiple acquisition task pose maps corresponding to multiple acquisition tasks; constructing an initial pose map based on the multiple acquisition task pose maps, wherein the initial pose map is a set of the multiple acquisition task pose maps.

[0107] It should be understood that when simultaneously fusing and pruning multiple acquisition task pose maps corresponding to multiple acquisition tasks, the multiple pose maps to be processed are multiple acquisition task pose maps, that is, the initial pose map is a set of multiple acquisition task pose maps.

[0108] For ease of understanding, please refer to Figure 2 The examples provided are for illustrative purposes only and do not limit the scope of this application. As an example, suppose we simultaneously perform fusion and pruning on the pose graphs of multiple acquisition tasks. There are three acquisition tasks (i.e., acquisition task 1, acquisition task 2, and acquisition task 3), and the initial pose graph is constructed as follows: Figure 2 As shown, there is a loop edge between the pose graphs of acquisition task 1 and acquisition task 2, and there is a loop edge between the pose graphs of acquisition task 2 and acquisition task 3.

[0109] Furthermore, considering the large computational load of simultaneously fusing and pruning multiple acquisition task pose maps, this embodiment can also perform incremental fusing and pruning on multiple acquisition task pose maps of multiple acquisition tasks to reduce the computational load. Before step S10, the method further includes: when performing incremental fusing and pruning on multiple acquisition task pose maps of multiple acquisition tasks, obtaining a fused pose map and a pose map to be fused, wherein the fused pose map is the pruned pose map obtained from the previous fusing and pruning, and the pose map to be fused is the acquisition task pose map corresponding to the acquisition task to be fusing and pruned; constructing an initial pose map based on the fused pose map and the pose map to be fused, wherein the initial pose map is a set of the fused pose map and the pose map to be fused;

[0110] Accordingly, after step S40, the method further includes: adjusting the inter-pose map loop edges between the fused pose map and the pose map to be fused to intra-pose map loop edges within the pruned pose map, and using the adjusted pruned pose map as the new fused pose map, returning to the step of obtaining the fused pose map and the pose map to be fused, until the pose maps of all acquisition tasks are fused and pruned, wherein the inter-pose map loops are used to represent the relative constraints between nodes in the pose maps, and the intra-pose map loop edges are used to represent the relative constraints between nodes within the pose map.

[0111] It should be understood that when performing incremental fusion pruning on multiple acquisition task pose maps of multiple acquisition tasks, the multiple pose maps to be processed include fused pose maps and pose maps to be fused. The fused pose map is the pruned pose map obtained from the previous fusion pruning, and the pose map to be fused is the acquisition task pose map corresponding to the current acquisition task to be fused and pruned. That is, the initial pose map is a set of fused pose maps and pose maps to be fused.

[0112] Understandably, after a single incremental fusion is completed, the nodes and edges of the pose graph to be fused are added to the fused pose graph to obtain a new fused pose graph. At this time, the loop edges between the original pose graphs become loop edges within the pose graph. The above incremental fusion process is repeated until the pose graphs of all acquisition tasks are fused.

[0113] For ease of understanding, please refer to Figure 8 , Figure 9 , Figure 10 as well as Figure 11 Examples are provided for illustration, but this does not limit the scope of this application. Figure 8 This diagram illustrates the pose confidence determination in incremental fusion pruning of multiple pose graphs, as described in an embodiment of the pose graph pruning method of this application. Figure 9 This diagram illustrates the search for overlapping region nodes in incremental fusion pruning of multiple pose graphs, as shown in one embodiment of the pose graph pruning method of this application. Figure 10 This diagram illustrates a breadth-first search for incremental fusion pruning of multiple pose graphs, as described in an embodiment of the pose graph pruning method of this application. Figure 11 This is an illustration of the effect of multi-pose graph incremental fusion pruning in one embodiment of the pose graph pruning method of this application:

[0114] As an example, suppose incremental fusion and pruning is performed on the pose maps of multiple acquisition tasks from multiple acquisition tasks, such as... Figure 8 As shown, the fused pose graph is the pruned pose graph obtained from the previous fusion pruning, and the pose graph to be fused is the acquisition task pose graph corresponding to the current acquisition task to be pruned. The fused pose graph contains 19 pose nodes and 3 loop edges within the pose graph, while the pose graph to be fused contains 9 pose nodes and 1 loop edge within the pose graph. Figure 8 As shown, there is a loop edge between the fused pose graph and the pose graph to be fused. The process of single incremental fusion is basically similar to that of simultaneous fusion, sequentially performing pose confidence judgment, overlapping region node search, and breadth-first search to obtain the pruned pose graph, as detailed below. Figure 8 , Figure 9 , Figure 10 As shown.

[0115] Finally, remove all nodes and edges from the initial pose graph except for the edges between selected nodes, and form a new pose graph by combining all the edges between selected nodes. This new pose graph is the pruned pose graph of the initial pose graph, such as... Figure 11 As shown, compared to the initial pose graph, the number of nodes decreased from 28 to 10, and redundant nodes and edges were significantly reduced.

[0116] After a single incremental fusion is completed, the nodes and edges of the pose graph to be fused are added to the fused pose graph to obtain a new fused pose graph. At this time, the loop edges between the original pose graphs become loop edges within the pose graph. The above incremental fusion process is repeated until the pose graphs of all acquisition tasks are fused.

[0117] This embodiment prunes the pose graph using a breadth-first search method. In the breadth-first search, the decision to add a found node to the pruned pose graph is based on the node pose confidence and whether it is in an overlapping region. This removes redundant nodes and edges from the pose graph, thereby reducing memory consumption and improving the efficiency of pose graph optimization.

[0118] Reference Figure 12 , Figure 12 This is a flowchart illustrating the second embodiment of the pose graph pruning method of this application, based on the above. Figure 1 The first embodiment shown is followed by a second embodiment of the pose graph pruning method of this application.

[0119] In the second embodiment, step S30 includes:

[0120] Step S301: Add the overlapping region nodes to the queue, and pop the head node of the queue in sequence.

[0121] It should be understood that, in order to improve the efficiency of breadth-first search, this embodiment first adds overlapping region nodes to the queue, then pops the head node of the queue in turn, and then determines the target node through cyclic judgment and adjacent search.

[0122] Step S302: When the head node does not meet the preset conditions, add the unselected nodes connected to the head node to the queue, return to the step of popping the head nodes in the queue in turn, until the queue is empty, and take the unselected nodes added to the queue as the target nodes.

[0123] The preset condition is that the head node is not in the overlapping area and the head node is a first confidence node.

[0124] Understandably, breadth-first search can be performed by first adding overlapping nodes to a queue, then popping the head node from the queue. If the head node is not in an overlapping area and has a high pose confidence, no operation is performed. Otherwise, the unselected nodes connected to the head node are added to the queue and marked as selected. This process is repeated until the queue is empty, and the unselected nodes added to the queue are used as the target nodes.

[0125] For ease of understanding, please refer to Figure 4The following example illustrates the process, but does not limit the scope of this application. As an example, the processing of node D1 is described below. After adding overlapping nodes to the queue, assume the head node of the popped queue is D1. Since D1 is within the overlapping area, it does not meet the preset condition. Therefore, the unselected node connected to D1 (D2) needs to be added to the queue (because overlapping nodes are selected, D4 and D5 connected to D1 cannot be added to the queue). D2 is then marked as the selected node. The process continues, popping the head node from the queue sequentially. When D2 is popped, since it is not a first-confidence node, it does not meet the preset condition. Therefore, the unselected node connected to D2 (D3) needs to be added to the queue, and D3 is marked as the selected node. When D3 is popped, since it is not within the overlapping area and is a first-confidence node, no operation is performed. It is evident that D2 and D3 can be marked as selected nodes through the above operations.

[0126] In this embodiment, overlapping region nodes are first added to a queue, and the head node of the queue is popped out in turn. Then, the target node is determined by cyclic judgment and adjacent search, thereby improving the efficiency of breadth-first search.

[0127] Reference Figure 13 , Figure 13 This is a flowchart illustrating the third embodiment of the pose graph pruning method of this application. Based on the above embodiments, a third embodiment of the pose graph pruning method of this application is proposed.

[0128] In the third embodiment, step S10 includes:

[0129] Step S101: Obtain the positioning signal quality of each node in the initial pose graph;

[0130] The positioning signal quality refers to the performance index of the positioning signal at the location of the node.

[0131] It should be understood that the better the quality of the node's positioning signal, the higher the confidence level of the node's pose. The smaller the positional error between the node's algorithm-estimated pose and the positioning measurement value, the higher the confidence level of the node's pose. Therefore, in order to improve the accuracy of the pose confidence level calculation, this embodiment calculates the pose confidence level of each node based on the quality of the positioning signal of each node and the positional error between the algorithm-estimated pose and the positioning measurement value.

[0132] It should be noted that the positioning signal quality can be the GPS signal quality. Obtaining the positioning signal quality of each node in the initial pose map can be achieved by analyzing the GPS signals of each node in the initial pose map to obtain the GPS signal quality of each node.

[0133] Furthermore, in order to improve the accuracy of the positioning signal quality, in this embodiment, the positioning signal quality of each node is obtained based on the solution identifier of each node in the initial pose diagram and the number of satellites corresponding to the node's location. Step S101 includes: obtaining the solution identifier of each node in the initial pose diagram and the number of satellites corresponding to the node's location, wherein the solution identifier is used to represent the data processing index in the process of parsing the positioning signal; and analyzing the positioning signal quality of each node based on the solution identifier and the number of satellites.

[0134] Understandably, in GPS positioning, resolution metrics typically refer to indicators related to data processing and algorithms. These indicators reflect the stability and reliability of the resolution process, thus indirectly reflecting the quality of the GPS signal. For example, in baseline resolution, the following resolution metrics can be used to evaluate GPS signal quality: RATIO value: The RATIO value is the ratio of the second smallest unit weight variance to the smallest unit weight variance when using a search algorithm to determine the integer values ​​of integer unknown parameters. It reflects the reliability of the determined integer unknown parameters. A higher RATIO value indicates more reliable determination of the integer unknown parameters, indirectly indicating better GPS signal quality. RDOP value: The RDOP (Dilution of Precision) value is the square root of the trace of the cofactor matrix of the undetermined parameters during baseline resolution. Its magnitude is related to the baseline position and the geometric distribution and trajectory of satellites in space (i.e., observation conditions). A smaller RDOP value indicates better observation conditions and higher GPS signal quality. Data deletion rate: During baseline calculation, if the correction for an observation exceeds a certain threshold, the observation is considered to contain gross errors and needs to be deleted. The data deletion rate reflects the quality of the raw GPS observations. A higher data deletion rate indicates more gross errors in the observations and poorer GPS signal quality.

[0135] The number of satellites corresponding to a node's location is also related to the positioning signal quality. The more satellites a node's location has, the better the positioning signal quality; conversely, the fewer satellites a node's location has, the worse the positioning signal quality.

[0136] Step S102: Calculate the algorithm-estimated pose of each node and obtain the positioning measurement value of each node.

[0137] It should be understood that the algorithmic estimated pose of each node can be calculated by using a preset pose estimation algorithm. The preset pose estimation algorithm can be set in advance, for example, the preset pose estimation algorithm can be the SLAM algorithm.

[0138] Furthermore, in order to improve the accuracy of the algorithm's pose estimation, in this embodiment, the algorithm-estimated pose of each node is calculated through feature extraction, feature matching, and pose estimation. Step S102 includes: acquiring multiple types of sensor data and extracting significant feature points from the sensor data; matching feature points corresponding to the significant feature points in consecutive frames or different sensor data; forming feature point pairs based on the significant feature points and the matching feature points, and calculating the algorithm-estimated pose of each node based on the feature point pairs.

[0139] For ease of understanding, the following example is provided, but it does not limit the scope of this application. As an example, suppose a vehicle is traveling on a road, and a camera is mounted on the vehicle. A high-precision map is constructed using a series of keyframes captured by the camera. The algorithmic steps for estimating the pose of each node are as follows:

[0140] 1. Sensor data acquisition: The camera captured a series of keyframes;

[0141] 2. Feature extraction: The ORB algorithm is used to extract significant feature points and their corresponding feature descriptors from each keyframe.

[0142] 3. Feature matching: Match feature points in two adjacent keyframes to find matching point pairs. This is usually achieved by calculating the Hamming distance between feature descriptors and selecting the point pair with the smallest distance as the matching point pair.

[0143] 4. Pose estimation: Based on the matched feature point pairs, epipolar geometry or PnP algorithm (if the 3D coordinates of some feature points are known) is used to estimate the relative pose of the camera at different times.

[0144] Step S103: Calculate the position error between the algorithm-estimated pose and the positioning measurement value.

[0145] Step S104: Calculate the pose confidence of each node in the initial pose graph based on the positioning signal quality and the position error.

[0146] Step S105: Determine whether the pose confidence is greater than the preset confidence, and divide each node into a first confidence node and a second confidence node according to the determination result.

[0147] For ease of understanding, the following example is provided, but it does not limit this application. As an example, the specific steps for dividing the node into a first confidence node and a second confidence node are as follows:

[0148] 1. Obtain the GPS signal quality of each node in the initial pose graph:

[0149] During the initial pose graph construction process, GPS signal quality parameters for each node are recorded simultaneously, such as the number of satellites, signal strength, and signal-to-noise ratio.

[0150] Example:

[0151] Node A: Number of satellites = 8, signal strength = 30dBm, signal-to-noise ratio = 20dB;

[0152] Node B: Number of satellites = 6, signal strength = 25dBm, signal-to-noise ratio = 18dB;

[0153] 2. Calculate the algorithm-estimated pose of each node and obtain GPS measurements:

[0154] Algorithm estimation: Using SLAM (Simultaneous Localization and Mapping) or other pose estimation algorithms, the estimated pose of each node is calculated based on sensor data (such as camera, LiDAR, etc.);

[0155] GPS measurement: At the same time, the precise location information (latitude, longitude, altitude, etc.) of each node is obtained through a GPS receiver.

[0156] Example:

[0157] Node A: Algorithm-estimated pose = (x1, y1, z1, θ1), GPS measurement value = (X... 1, Y1, Z1);

[0158] Node B: Algorithm estimated pose = (x2, y2, z2, θ2), GPS measurement value = (X2, Y2, Z2);

[0159] 3. Calculate the position error between the estimated pose and the GPS measurement using the algorithm.

[0160] Position error calculation: The algorithm calculates the Euclidean distance between the estimated pose position (x, y, z) and the GPS measurement (X, Y, Z).

[0161] Example:

[0162] Node A:

[0163] Node B:

[0164] 4. Calculate the pose confidence level based on the positioning signal quality and position error:

[0165] Define the confidence model: Based on GPS signal quality and position error, define a mathematical model for calculating pose confidence. For example, GPS signal quality can be used as a weight to weight the position error.

[0166] Example:

[0167] Assuming the confidence model is: Pose confidence = Signal quality weight * (1 - Position error / Maximum possible error);

[0168] Node A: Signal quality weight = 0.8 (calculated based on the number of satellites and signal-to-noise ratio), pose confidence = 0.8 * (1 - position error / 100 meters)

[0169] Node B: Signal quality weight = 0.7, pose confidence = 0.7 * (1 - position error / 100 meters)

[0170] 5. Determine pose confidence and divide nodes:

[0171] Set a preset reliability threshold: Based on the application scenario requirements, set a preset reliability threshold.

[0172] Node segmentation: The pose confidence of each node is compared with the preset confidence. Nodes with a confidence greater than the preset confidence are segmented as first-confidence nodes, and nodes with a confidence less than or equal to the preset confidence are segmented as second-confidence nodes.

[0173] Example:

[0174] Preset reliability = 0.75;

[0175] Node A: Confidence level = 0.8 > 0.75, therefore it is classified as a node with the first confidence level;

[0176] Node B: Confidence level = 0.65 < 0.75, classified as a second confidence node;

[0177] This embodiment calculates the pose confidence of each node based on the positioning signal quality of each node and the position error between the algorithm-estimated pose and the positioning measurement value, thereby improving the accuracy of pose confidence calculation.

[0178] Reference Figure 14 , Figure 14 This is a flowchart illustrating the fourth embodiment of the pose graph pruning method of this application. Based on the above embodiments, the fourth embodiment of the pose graph pruning method of this application is proposed.

[0179] In the fourth embodiment, step S20 includes:

[0180] Step S201: Construct a tree data structure based on the position information of each node;

[0181] The tree-shaped data structure is a hierarchical structure in a multidimensional space.

[0182] It should be understood that, in order to improve the search efficiency of overlapping region nodes, in this embodiment, a tree data structure is constructed based on the position information of each node, and then the overlapping region nodes between multiple pose maps to be processed are searched through the tree data structure. The tree data structure is a hierarchical structure in a multi-dimensional space, such as an R-tree or a KD-tree. This embodiment does not limit this.

[0183] It is understandable that constructing a tree data structure based on the location information of each node can be done by calculating the distance between each pair of nodes based on the location information of each node, randomly selecting a node or selecting the most central (or most peripheral) node as the root node, finding the node with the smallest distance from the root node, adding the node with the smallest distance from the root node as the child node of the root node to the tree, continuing this process to find the node with the smallest distance from the child node, and adding the node with the smallest distance from the child node as the child node of the child node to the tree, and so on, until all nodes have been added to the tree.

[0184] Furthermore, to improve the accuracy of the tree data structure, this embodiment constructs the tree data structure through data segmentation and recursive construction. Step S201 includes: selecting one dimension of the multidimensional space as the segmentation axis to sort the position information of each node, and selecting the middle point as the root node according to the sorting result; dividing the remaining nodes except the root node into a left subset and a right subset according to the sorting result; recursively performing the step of selecting one dimension of the multidimensional space as the segmentation axis to sort the position information of each node, and dividing the remaining nodes except the root node into a left subset and a right subset according to the sorting result, until the remaining leaf nodes in the left subset are obtained, thus obtaining the left subtree; recursively performing the step of selecting one dimension of the multidimensional space as the segmentation axis to sort the position information of each node, and dividing the remaining nodes except the root node into a left subset and a right subset according to the sorting result, until the remaining leaf nodes in the right subset are obtained, thus obtaining the right subtree; and constructing a tree data structure based on the root node, the left subtree, and the right subtree.

[0185] For ease of understanding, the following example is provided, but it does not limit this application. As an example, assuming the multidimensional space is a three-dimensional space, and the position information of each node is represented by coordinates (x, y, z), the steps to construct a tree data structure may include: 1. Selecting a partition axis: starting with the x-axis, then sequentially selecting the y-axis and z-axis, or using other strategies (such as based on the axis with the largest variance); 2. Data sorting and partitioning: sorting all nodes according to the currently selected partition axis; selecting the middle point after sorting as the root node; dividing the remaining points into left and right subsets, located on either side of the middle point; 3. Recursive construction: recursively performing the above steps on the points in the left subset to construct the left subtree; recursively performing the above steps on the points in the right subset to construct the right subtree; 4. Continuing recursion: changing the partition axis in each level of recursion (e.g., xyz loop). Repeating sorting, partitioning, and recursive construction until only one point (i.e., a leaf node) remains in each subset.

[0186] For ease of understanding, the following examples are provided, but are not intended to limit this application. As an example, assuming the position information of each node is P1(2,3,5), P2(5,4,9), P3(9,6,12), P4(4,0,3), P5(8,1,6), and P6(7,2,11), the steps for constructing a tree data structure may include:

[0187] 1. Selecting the dividing axis: First, calculate the variances of all nodes in the x, y, and z dimensions as 5.83, 3.74, and 10.92, respectively. Since the variance in the z dimension is the largest, we choose the z dimension as the dividing axis.

[0188] 2. Data Sorting and Partitioning: Sort the coordinates of the nodes according to the partition axis (i.e., the z-axis). The sorted coordinates are P4(4,0,3), P1(2,3,5), P5(8,1,6), P2(5,4,9), P6(7,2,11), and P3(9,6,12). Take the median of the sorted coordinates. For an even number of nodes, the larger median is usually chosen. That is, take P2(5,4,9) as the root node and divide the remaining nodes into two subsets. Specifically, nodes with a z-axis value less than 9 are divided into the left subset, and nodes with a z-axis value greater than 9 are divided into the right subset. Left subset: P4(4,0,3), P1(2,3,5), P5(8,1,6); Right subset: P6(7,2,11), P3(9,6,12).

[0189] 3. Recursive Construction: Taking the left subset as an example, the variances of the nodes in the left subset along the x, y, and z dimensions are calculated to be 5.88, 1.64, and 1.33, respectively. Since the x-dimensional variance is the largest, the x-dimensional is chosen as the dividing axis of the left subset. The nodes of the left subset are sorted according to the dividing axis (i.e., the x-dimensional), and the sorted coordinates are P1(2,3,5), P4(4,0,3), and P5(8,1,6). The point at the median of the sorted coordinates (P4(4,0,3)) is taken as the root node of the left subset to construct the left and right subtrees of the left subset. Specifically, nodes with x-dimensional values ​​less than 4 are divided into the left subtree; nodes with x-dimensional values ​​greater than 4 are divided into the right subtree: Left subtree of the left subset: P1(2,3,5); Right subtree of the left subset: P5(8,1,6).

[0190] Similarly, we can determine that the root node of the right subtree is P3(9,6,12) (segmented according to the y-dimensional): the left subtree of the right subset is P6(7,2,11); the right subtree of the right subset is empty;

[0191] The obtained KD tree is constructed as follows Figure 15 As shown, Figure 15 This is a schematic diagram of a KD tree representing an embodiment of the pose graph pruning method of this application.

[0192] Step S202: Search for overlapping region nodes among the multiple pose graphs to be processed using the tree data structure, and mark the overlapping region nodes as selected nodes.

[0193] It should be understood that the tree data structure is traversed using algorithms such as depth-first search, breadth-first search, or nearest neighbor search to search for overlapping nodes between multiple pose graphs to be processed. This embodiment does not impose any restrictions on this.

[0194] To further improve the search efficiency of overlapping region nodes, this embodiment performs nearest neighbor search on all nodes. The step of searching for overlapping region nodes among the multiple pose graphs to be processed through the tree data structure includes: traversing each node and taking the traversed node as the node to be searched; comparing the value of the node to be searched with the root node of the tree data structure on the split axis, and recursively searching for neighboring nodes on the tree data structure based on the comparison result, wherein the neighboring nodes are nodes within a preset neighborhood range of the node to be searched; when the node to be searched and the neighboring nodes do not belong to the same pose graph to be processed, the node to be searched is determined to be an overlapping region node.

[0195] In a specific implementation, the nearest neighbor search in a KD-tree can start from the root node. The size relationship between the node to be searched and the root node on the split axis determines whether to enter the left or right subtree. Then, the tree is searched recursively until a neighboring node within the preset neighborhood range of the node to be searched is found. Then, the node to be searched is determined to be an overlapping region node based on whether the node to be searched and the neighboring node belong to the same pose graph to be processed. If the node to be searched and the neighboring node do not belong to the same pose graph to be processed, the node to be searched is determined to be an overlapping region node.

[0196] This embodiment constructs a tree data structure based on the location information of each node, and then searches for overlapping region nodes between multiple pose graphs to be processed through the tree data structure, thereby improving the search efficiency of overlapping region nodes.

[0197] It should be noted that the above examples are only for understanding this application and do not constitute a limitation on the pose graph pruning method of this application. Any simple transformations based on this technical concept are within the protection scope of this application.

[0198] This application also provides a pose graph pruning device, please refer to... Figure 16 The pose graph pruning device includes:

[0199] The node partitioning module 10 is used to calculate the pose confidence of each node in the initial pose graph, and to partition each node into a first confidence node and a second confidence node according to the pose confidence. The initial pose graph is a set of multiple pose graphs to be processed. The initial pose graph includes multiple nodes and edges between nodes. The nodes are used to represent the pose of keyframes, and the edges are used to represent the relative constraints between poses. The pose confidence of the first confidence node is greater than a preset confidence, and the pose confidence of the second confidence node is not greater than the preset confidence.

[0200] The overlap search module 20 is used to search for overlapping region nodes among the multiple pose maps to be processed, and to mark the overlapping region nodes as selected nodes.

[0201] The breadth-first search module 30 is used to search for target nodes using the overlapping region nodes as seed nodes in a breadth-first search manner, and to mark the target nodes as selected nodes. The breadth-first search method involves searching for second confidence nodes adjacent to the seed node with the seed node as the search center, until all nodes have been searched or a preset node has been found. The preset node is a first confidence node that is not in the overlapping region. The target node includes the preset node and the second confidence nodes that were passed during the search of the preset node.

[0202] The pruning module 40 is used to prune the initial pose graph based on the edges between the selected nodes to obtain a pruned pose graph.

[0203] The pose graph pruning device provided in this application, employing the pose graph pruning method in the above embodiments, can solve the technical problem of low optimization efficiency caused by the requirement to optimize all nodes, including redundant nodes, in related pose graph optimization methods. Compared with related technologies, the beneficial effects of the pose graph pruning device provided in this application are the same as those of the pose graph pruning method provided in the above embodiments, and other technical features in the pose graph pruning device are the same as those disclosed in the methods of the above embodiments, and will not be repeated here.

[0204] This application provides a pose graph pruning device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the pose graph pruning method in the first embodiment described above.

[0205] The following is for reference. Figure 17 This document illustrates a structural schematic diagram of a pose graph pruning device suitable for implementing embodiments of this application. The pose graph pruning device in this application may include, but is not limited to, mobile terminals such as mobile phones, laptops, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Description), PMPs (Portable Media Players), and in-vehicle terminals (e.g., in-vehicle navigation terminals), as well as fixed terminals such as digital TVs and desktop computers. Figure 17 The pose graph pruning device shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of this application.

[0206] like Figure 17As shown, the pose graph pruning device may include a processing unit 1001 (e.g., a central processing unit, a graphics processing unit, etc.) that can perform various appropriate actions and processes according to a program stored in ROM (Read Only Memory) 1002 or a program loaded from storage device 1003 into RAM (Random Access Memory) 1004. RAM 1004 also stores various programs and data required for the operation of the pose graph pruning device. The processing unit 1001, ROM 1002, and RAM 1004 are interconnected via bus 1005. Input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems can be connected to I / O interface 1006: input devices 1007 including, for example, touchscreens, touchpads, keyboards, mice, image sensors, microphones, accelerometers, gyroscopes, etc.; output devices 1008 including, for example, LCDs (Liquid Crystal Displays), speakers, vibrators, etc.; storage devices 1003 including, for example, magnetic tapes, hard disks, etc.; and communication devices 1009. Communication device 1009 allows the pose graph pruning device to communicate wirelessly or wiredly with other devices to exchange data. Although pose graph pruning devices with various systems are shown in the figures, it should be understood that it is not required to implement or possess all the systems shown. More or fewer systems can be implemented alternatively.

[0207] Specifically, according to the embodiments disclosed in this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments disclosed in this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device, or installed from storage device 1003, or installed from ROM 1002. When the computer program is executed by processing device 1001, it performs the functions defined in the methods of the embodiments disclosed in this application.

[0208] The pose graph pruning device provided in this application, employing the pose graph pruning method in the above embodiments, can solve the technical problem of low optimization efficiency caused by the requirement to optimize all nodes, including redundant nodes, in related pose graph optimization methods. Compared with related technologies, the beneficial effects of the pose graph pruning device provided in this application are the same as those of the pose graph pruning method provided in the above embodiments, and other technical features in this pose graph pruning device are the same as those disclosed in the previous embodiment method, and will not be repeated here.

[0209] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples.

[0210] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0211] This application provides a computer-readable storage medium having computer-readable program instructions (i.e., a computer program) stored thereon, the computer-readable program instructions being used to execute the pose graph pruning method in the above embodiments.

[0212] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared systems, devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections with one or more wires, portable computer disks, hard disks, RAM (Random Access Memory), ROM (Read Only Memory), EPROM (Erasable Programmable Read Only Memory), optical fibers, CD-ROM (CD-Read Only Memory), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, system, or device. The program code contained on the computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0213] The aforementioned computer-readable storage medium may be included in the pose graph pruning device; or it may exist independently and not assembled into the pose graph pruning device.

[0214] The aforementioned computer-readable storage medium carries one or more programs. When the aforementioned one or more programs are executed by the pose graph pruning device, the pose graph pruning device performs the following actions: calculates the pose confidence of each node in the initial pose graph, divides each node into a first confidence node and a second confidence node based on the pose confidence, searches for overlapping region nodes among multiple pose graphs to be processed, marks the overlapping region nodes as selected nodes, searches for target nodes using the overlapping region nodes as seed nodes through a breadth-first search, marks the target nodes as selected nodes, and prunes the initial pose graph based on the edges between the selected nodes to obtain a pruned pose graph.

[0215] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, and C++, as well as conventional procedural programming languages ​​such as "C" or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including LAN (Local Area Network) or WAN (Wide Area Network)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0216] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0217] The modules described in the embodiments of this application can be implemented in software or hardware. The names of the modules do not necessarily limit the functionality of the unit itself.

[0218] The readable storage medium provided in this application is a computer-readable storage medium that stores computer-readable program instructions (i.e., a computer program) for executing the above-described pose graph pruning method. This solves the technical problem that related pose graph optimization methods require optimizing all nodes, including redundant nodes, resulting in low optimization efficiency. Compared with related technologies, the beneficial effects of the computer-readable storage medium provided in this application are the same as those of the pose graph pruning method provided in the above embodiments, and will not be elaborated upon here.

[0219] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the pose graph pruning method as described above.

[0220] The computer program product provided in this application can solve the technical problem that related pose graph optimization methods require optimization of all nodes, including redundant nodes, resulting in low optimization efficiency. Compared with related technologies, the beneficial effects of the computer program product provided in this application are the same as those of the pose graph pruning method provided in the above embodiments, and will not be repeated here.

[0221] The above description is only a part of the embodiments of this application and does not limit the patent scope of this application. All equivalent structural transformations made under the technical concept of this application and using the contents of the specification and drawings of this application, or direct / indirect applications in other related technical fields, are included in the patent protection scope of this application.

Claims

1. A pose graph pruning method, characterized in that, The pose graph pruning method includes: Calculate the pose confidence of each node in the initial pose graph, and divide each node into a first confidence node and a second confidence node based on the pose confidence. The initial pose graph is a set of multiple pose graphs to be processed. The initial pose graph includes multiple nodes and edges between nodes. The nodes are used to represent the pose of keyframes, and the edges are used to represent the relative constraints between poses. The pose confidence of the first confidence node is greater than a preset confidence, and the pose confidence of the second confidence node is not greater than the preset confidence. Search for overlapping region nodes among the multiple pose graphs to be processed, and mark the overlapping region nodes as selected nodes; Using the overlapping region nodes as seed nodes, a breadth-first search is used to search for target nodes, and the target nodes are marked as selected nodes. The breadth-first search method involves searching for second confidence nodes adjacent to the seed node with the seed node as the search center, until all nodes have been searched or a preset node is found. The preset node is a first confidence node that is not in the overlapping region. The target node includes the preset node and the second confidence nodes passed during the search of the preset node. The initial pose graph is pruned based on the edges between the selected nodes to obtain a pruned pose graph.

2. The pose graph pruning method as described in claim 1, characterized in that, The step of using the overlapping region nodes as seed nodes to search for target nodes using a breadth-first search method includes: Add the overlapping region nodes to the queue, and pop the head node of the queue in sequence; When the head node does not meet the preset conditions, the unselected nodes connected to the head node are added to the queue, and the unselected nodes added to the queue are marked as selected nodes. The process returns to the step of popping the head node from the queue in turn until the queue is empty. The preset conditions are that the head node is not in the overlapping area and the head node is a first confidence node.

3. The pose graph pruning method as described in claim 1, characterized in that, The calculation of the pose confidence of each node in the initial pose graph, and the division of each node into a first confidence node and a second confidence node based on the pose confidence, includes: The positioning signal quality of each node in the initial pose graph is obtained, wherein the positioning signal quality is the performance index of the positioning signal at the location of the node. Calculate the algorithmic estimated pose of each node and obtain the localization measurement value of each node; Calculate the position error between the pose estimated by the algorithm and the positioning measurement value; The pose confidence of each node in the initial pose graph is calculated based on the positioning signal quality and the position error. Determine whether the pose confidence level is greater than a preset confidence level, and divide each node into a first confidence level node and a second confidence level node based on the determination result.

4. The pose graph pruning method as described in claim 3, characterized in that, The process of obtaining the positioning signal quality of each node in the initial pose graph includes: Obtain the solution identifier of each node in the initial pose graph and the number of satellites corresponding to the node's location, wherein the solution identifier is used to represent the data processing index during the parsing of the positioning signal; The positioning signal quality of each node is analyzed based on the solution identifier and the number of satellites.

5. The pose graph pruning method as described in claim 3, characterized in that, The algorithm for calculating the pose estimate of each node includes: Acquire data from multiple sensors and extract significant feature points from the sensor data; Matching feature points corresponding to the salient feature points between consecutive frames or different sensor data; Feature point pairs are formed based on the salient feature points and the matching feature points, and the algorithmic estimated pose of each node is calculated based on the feature point pairs.

6. The pose graph pruning method as described in claim 1, characterized in that, The search for overlapping region nodes among the plurality of pose maps to be processed includes: A tree-shaped data structure is constructed based on the location information of each node, wherein the tree-shaped data structure is a hierarchical structure in a multi-dimensional space; The overlapping region nodes between the multiple pose graphs to be processed are searched using the tree data structure.

7. The pose graph pruning method as described in claim 6, characterized in that, The step of constructing a tree data structure based on the position information of each node includes: Select one dimension of the multidimensional space as the dividing axis to sort the position information of each node, and select the middle point as the root node according to the sorting result. Then, divide the remaining nodes except the root node into a left subset and a right subset according to the sorting result. The left subtree is obtained by recursively performing the step of selecting one dimension of the multidimensional space as the dividing axis to sort the position information of each node, and dividing the remaining nodes except the root node into the left and right subtrees according to the sorting result, until the remaining leaf nodes in the left subtree are obtained. The steps of selecting one dimension of the multidimensional space as the dividing axis to sort the position information of each node are recursively executed for the nodes in the right subset. Based on the sorting result, the remaining nodes except the root node are divided into the left subset and the right subset until the remaining leaf nodes in the right subset are obtained, thus obtaining the right subtree. A tree data structure is constructed based on the root node, the left subtree, and the right subtree.

8. The pose graph pruning method as described in claim 7, characterized in that, The step of searching for overlapping region nodes among the multiple pose graphs to be processed using the tree data structure includes: Traverse each node and use the traversed node as the node to be searched; The values ​​of the node to be searched and the root node of the tree data structure on the split axis are compared, and the neighboring nodes on the tree data structure are recursively searched according to the comparison results, wherein the neighboring nodes are nodes within a preset neighborhood range of the node to be searched. When the node to be searched and the neighboring nodes do not belong to the same pose graph to be processed, the node to be searched is determined to be an overlapping region node.

9. The pose graph pruning method according to any one of claims 1 to 8, characterized in that, Before calculating the pose confidence of each node in the initial pose graph and dividing each node into a first confidence node and a second confidence node based on the pose confidence, the method further includes: When simultaneously fusing and pruning multiple acquisition task pose maps corresponding to multiple acquisition tasks, multiple acquisition task pose maps corresponding to multiple acquisition tasks are obtained. An initial pose graph is constructed based on the multiple acquisition task pose graphs, wherein the initial pose graph is a set of the multiple acquisition task pose graphs.

10. The pose graph pruning method according to any one of claims 1 to 8, characterized in that, Before calculating the pose confidence of each node in the initial pose graph and dividing each node into a first confidence node and a second confidence node based on the pose confidence, the method further includes: When performing incremental fusion pruning on multiple acquisition task pose maps of multiple acquisition tasks, the fused pose map and the pose map to be fused are obtained. The fused pose map is the pruned pose map obtained from the previous fusion pruning, and the pose map to be fused is the acquisition task pose map corresponding to the acquisition task to be fused and pruned at the current time. An initial pose graph is constructed based on the fused pose graph and the pose graph to be fused, wherein the initial pose graph is a set of the fused pose graph and the pose graph to be fused; Accordingly, after pruning the initial pose graph based on the edges between the selected nodes to obtain the pruned pose graph, the process further includes: The pose graph loops between the fused pose graph and the pose graph to be fused are adjusted to pose graph intraloops within the pruned pose graph. The adjusted pruned pose graph is then used as the new fused pose graph. The process of obtaining the fused pose graph and the pose graph to be fused is repeated until the pose graphs of all acquisition tasks are fused and pruned. Here, the pose graph loops represent the relative constraints between nodes in the pose graphs, and the pose graph intraloops represent the relative constraints between nodes within the pose graph.

11. A pose graph pruning device, characterized in that, The pose graph pruning device includes: a memory, a processor, and a pose graph pruning program stored in the memory and executable on the processor, wherein the pose graph pruning program, when executed by the processor, implements the pose graph pruning method as described in any one of claims 1 to 10.

12. A storage medium, characterized in that, The storage medium stores a pose graph pruning program, which, when executed by a processor, implements the pose graph pruning method as described in any one of claims 1 to 10.

Images