A distributed pose optimization method and system based on graph algorithm
By constructing a connection graph and using a subgraph partitioning algorithm to decompose the trajectory graph into independent subgraphs, and combining iterative methods for pose optimization, the distributed solution problem of pose graph optimization in crowdsourced graph construction systems is solved, achieving efficient pose optimization results.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- WUHAN ZHONGHAITING DATA TECH CO LTD
- Filing Date
- 2022-11-03
- Publication Date
- 2026-06-05
AI Technical Summary
In crowdsourced graphing systems, existing technologies struggle to effectively process large amounts of data for pose graph optimization within an acceptable timeframe, leading to difficulties in distributed solution.
By constructing a connectivity graph, the connectivity graph between trajectories is divided into independent subgraphs using a subgraph partitioning algorithm in graph theory. Pose optimization is then performed on each subgraph, and the optimized pose estimate is obtained by combining iterative methods.
It enables efficient pose optimization in a distributed computing environment, improving solution speed and efficiency, and can approach the global optimal solution under large-scale data.
Smart Images
Figure CN115795082B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of high-precision map production, and more specifically, to a distributed pose optimization method and system based on graph algorithms. Background Technology
[0002] In crowdsourced graphing systems, crowdsourced vehicles travel on roads and collect road-related information, including their current location, ground features, and traffic signs. After processing, the data needed for graphing is uploaded to the cloud for graph creation. To fuse data collected by multiple vehicles, trajectories need to be optimized to ensure consistency between them; this is pose graph optimization in SLAM. The basic idea is to treat the pose at each time step as a node in the pose graph, and the observations (constraints) between poses as edges. Given reasonable initial values for each pose node, an iterative method is used to optimize the constructed pose graph until convergence. However, pose graph optimization in crowdsourced scenarios faces the problem of being unable to solve within an acceptable timeframe due to the large amount of data. Therefore, it is necessary to decompose the optimization problem for distributed solution. Summary of the Invention
[0003] This invention addresses the technical problems existing in the prior art by providing a distributed pose optimization method and system based on graph algorithms, which can realize distributed pose optimization.
[0004] According to a first aspect of the present invention, a distributed pose optimization method based on graph algorithms is provided, comprising:
[0005] Obtain multiple trajectories collected by vehicles mapping within a geographic area;
[0006] Define the connection vector between each pair of trajectories based on whether there is a matching relationship between them;
[0007] Based on the connection vector between every two trajectories, construct a connection graph between all trajectories;
[0008] The connected graph is divided into multiple completely independent subgraphs using a subgraph partitioning algorithm in graph theory.
[0009] For each subgraph, pose optimization is performed to obtain the optimized pose estimate.
[0010] Based on the above technical solution, the present invention can also be improved as follows.
[0011] Optionally, defining the connection vector between each pair of trajectories based on whether a matching relationship exists between them includes:
[0012] Determine if there are any identical line segments on two tracks. If so, there is a matching relationship between the two tracks, and the connection vector between the two tracks is set to 1. If not, there is no matching relationship between the two tracks, and the connection vector between the two tracks is set to 0.
[0013] Optionally, determining whether there are identical line segments on the two trajectories, and if so, indicating a matching relationship between the two trajectories, includes:
[0014] Determine if there are any identical line segments on two tracks. If so, determine if there is a matching relationship between the two tracks based on the length of the identical line segments. If the length of the identical line segments is greater than a set length threshold, then there is a matching relationship between the two tracks; otherwise, there is no matching relationship between the two tracks.
[0015] Optionally, the step of performing pose optimization on each sub-graph to obtain the optimized pose trajectory includes:
[0016] Based on each subgraph, construct a description of the pose optimization problem:
[0017]
[0018] Among them, the mapping vehicle performs N pose estimations in one data collection trip, which are P i For i ∈ [1, 2, ..., N], there are M GPS signals in a single trajectory, denoted as g. j For j∈[1,2,…,M], the pose measurements between discontinuous poses are obtained through loop closure detection or matching, denoted as L. ij If abs(ij) > 1, the odometry measurement between consecutive poses is recorded as O. ij i+
[0019] 1 = j, let G be the denoted G. k P is the plane projection coordinate of latitude and longitude. j，loc These are the Cartesian coordinate components in the pose, indicated by the superscript l. g lh, g, h∈[1,2,…,K] to distinguish trajectories from different trips, I is the set of all trip trajectories;
[0020] Based on the description of the pose optimization problem, an iterative method is used to obtain the optimized pose estimate.
[0021] Optionally, the step of performing pose optimization on each sub-graph to obtain the optimized pose estimate includes:
[0022] The information of each subgraph is stored in a distributed optimizer. The pose of each subgraph is optimized based on the distributed optimizer to obtain the optimized pose estimate.
[0023] According to a second aspect of the present invention, a distributed pose optimization system based on graph algorithms is provided, comprising:
[0024] The acquisition module is used to acquire multiple trajectories collected by mapping vehicles within a geographical area;
[0025] Define the module to define the connection vector between each pair of trajectories based on whether there is a matching relationship between them;
[0026] The building module is used to construct a connection graph between all trajectories based on the connection vector between every two trajectories;
[0027] A partitioning module is used to divide the connected graph into multiple completely independent subgraphs using a subgraph partitioning algorithm in graph theory.
[0028] The optimization module is used to optimize the pose of each subgraph to obtain the optimized pose estimate.
[0029] Optionally, the optimization module is a distributed optimizer, used to distribute the storage of each subgraph information and perform pose optimization on each subgraph information to obtain the optimized pose estimate.
[0030] According to a third aspect of the present invention, an electronic device is provided, including a memory and a processor, wherein the processor is configured to implement a distributed pose optimization method based on a graph algorithm when executing a computer management program stored in the memory.
[0031] According to a fourth aspect of the present invention, a computer-readable storage medium is provided, on which a computer management class program is stored, wherein when executed by a processor, the computer management class program implements the steps of a distributed pose optimization method based on a graph algorithm.
[0032] This invention provides a pose optimization method and system based on graph algorithms. It constructs a connection graph based on the matching relationship between trajectories, divides the entire connection graph into multiple subgraphs according to the connection relationship between each trajectory, performs pose optimization based on each subgraph, and then obtains global pose optimization. This can realize distributed pose optimization and improve the speed and efficiency of pose optimization. Attached Figure Description
[0033] Figure 1 A flowchart of a distributed pose optimization method based on graph algorithms provided by this invention;
[0034] Figure 2 This is a schematic diagram showing the connections between multiple trajectories.
[0035] Figure 3 This is a schematic diagram of pose optimization for a single trajectory.
[0036] Figure 4This is a schematic diagram of pose optimization for multiple trajectories.
[0037] Figure 5 This is a schematic diagram of the structure of a distributed pose optimization system based on graph algorithms provided by the present invention.
[0038] Figure 6 A schematic diagram of the hardware structure of a possible electronic device provided by the present invention;
[0039] Figure 7 This is a schematic diagram of the hardware structure of a possible computer-readable storage medium provided by the present invention. Detailed Implementation
[0040] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. In addition, the technical features of the various embodiments or individual embodiments provided by the present invention can be arbitrarily combined with each other to form feasible technical solutions. Such combinations are not constrained by the order of steps and / or structural composition patterns, but must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.
[0041] Pose graph optimization in crowdsourcing scenarios involves a large number of nodes and edges, forming a complex graph. Current pose graph algorithms typically optimize the entire network or use inter-process communication (IPC) for information synchronization between nodes. The former limits optimization to a single computer and cannot be improved by expanding computing resources; the latter requires complex design, and information synchronization between servers consumes a significant amount of time. Based on these problems and the unique characteristics of crowdsourcing in open-access environments, this invention proposes a distributed optimization method for pose graph optimization, primarily focusing on the task partitioning problem.
[0042] Figure 1 A flowchart of a distributed pose optimization method based on graph algorithms provided by this invention is shown below. Figure 1 As shown, the method mainly includes the following steps:
[0043] S1: Obtain multiple trajectories collected by mapping vehicles within the geographic area.
[0044] S2, define the connection vector between each pair of trajectories based on whether there is a matching relationship between them.
[0045] Understandably, the set T of N trajectories within a geographical region i For i∈[1,2,…,N], we can define a connectivity C based on whether there is a matching relationship between two trajectories. i,j If there is a matching relationship between two trajectories, the connection value is 1; otherwise, the connection value is 0.
[0046] As an example, defining the connection vector between each pair of trajectories based on whether there is a matching relationship between them includes: determining whether there are identical line segments on the two trajectories; if so, there is a matching relationship between the two trajectories, and the connection vector between the two trajectories is set to 1; if not, there is no matching relationship between the two trajectories, and the connection vector between the two trajectories is set to 0.
[0047] Specifically, the method for determining whether two trajectories have a matching connection is as follows: based on the positions of the shape points on the two trajectories, it can be determined whether the positions of these shape points on the two trajectories overlap, that is, whether there are shape points in the same position. If there is an overlap, it means that there is a matching relationship between the two trajectories; otherwise, there is no matching relationship between the two trajectories.
[0048] S3, based on the connection vector between every two trajectories, constructs a connection graph between all trajectories.
[0049] S4. The connected graph is divided into multiple completely independent subgraphs using a subgraph partitioning algorithm in graph theory.
[0050] Based on this connectivity, a connectivity graph G can be constructed. This graph can be represented using an adjacency matrix or a coefficient representation. Then, using a subgraph partitioning algorithm from graph theory, it can be divided into multiple completely independent subgraphs. These subgraphs are pose graphs that can be optimized independently. A detailed diagram of the connectivity relationships can be found in [reference needed]. Figure 2 .
[0051] Should Figure 2 Each circle in the diagram represents a complete trajectory collected through crowdsourcing. From the connectivity relationships, it can be seen that trajectories 1, 2, 3, 4, and 5 are matched and connected. Trajectories 6, 7, and 8 are also connected, but they are unreachable from the blue nodes. Therefore, this trajectory pose optimization problem formed by 8 trajectories can be divided into two parts: the blue part and the green part. There are no constraints (residual terms) between the two parts, so they can be optimized completely independently.
[0052] It should be noted that if Figure 2 The matching connection graph in the model is a dense graph, meaning it cannot be further divided into subgraphs, thus hindering distributed pose optimization. As an example, determining whether two trajectories have identical line segments, and if so, establishing a matching relationship, includes: determining whether two trajectories have identical line segments; if so, determining whether a matching relationship exists based on the length of the identical line segments; if the length of the identical line segments is greater than a set length threshold, then a matching relationship exists between the two trajectories; otherwise, no matching relationship exists between the two trajectories.
[0053] Understandably, in situations where subgraph partitioning is not possible, a threshold for the length of overlapping lines between two trajectories can be set. Only when the overlapping line segments between two trajectories reach the set threshold are they considered to have a matching relationship. If there are no overlapping line segments or the length of the overlapping line segments is less than the set threshold, then the two trajectories are considered not to have a matching relationship. Finally, a connection graph between trajectories is constructed based on the matching relationships, and the overall connection graph is then partitioned. Although the subgraphs created in this way are not strictly independent, they are acceptable for approximate solutions. Theoretically, the smaller this threshold, the closer the obtained solution is to the global solution.
[0054] S5. Perform pose optimization on each sub-image to obtain the optimized pose estimate.
[0055] Understandably, for each decomposed subgraph, pose optimization can be performed on each subgraph based on distributed resources. Specifically, after partitioning using the matching network described above, the information of these independent subgraphs can be stored in a database and given a unique ID, thus completing the distributed task partitioning. Next, a suitable optimizer can be created in the cluster based on computing resources. The optimizer can consume tasks from the database, ultimately solving the entire trajectory optimization problem.
[0056] For the pose optimization problem based on each subgraph, we first introduce pose graph optimization. Vehicles equipped with Localization and Mapping (SLAM) systems typically collect sensor information at a certain frequency while driving on the road. These sensors include, but are not limited to, inertial measurement units (IMUs), cameras, GPS receivers, and wheel speed sensors. If the vehicle uses this information to build a map, this sensor information needs to be fused for state estimation. Only when the vehicle pose estimation is accurate can the constructed map have high accuracy, because the position of all map elements in a crowdsourced mapping system is based on the vehicle's pose. However, the vehicle's pose estimation cannot achieve high accuracy using only onboard sensors due to large GPS errors or signal loss, as well as accumulated errors. To increase the accuracy of pose estimation, loop closure detection is needed, and the pose optimization should be performed using this loop closure information and the relative pose relationships between the preceding and following poses.
[0057] After a mapping vehicle completes a full data acquisition, it typically obtains relative trajectories, GPS trajectories, traffic elements observed along the route, and dynamic objects. The relative trajectory is complete and continuous, with position coordinates starting from the origin; it records the relative pose (odometer pose) between preceding and subsequent poses. The GPS trajectory records the GPS pose at multiple time points along the route, including latitude, longitude, and altitude information—these are absolute coordinates. Observations record multiple traffic elements relative to the vehicle's coordinate system. With the relative trajectory and observations, matching algorithms can be used to obtain the relative pose relationships between some poses. Adding the odometer pose and GPS global coordinate information from the relative trajectory, a pose optimization map can be constructed, such as... Figure 3 As shown in the figure, triangles represent poses at various times, circles represent GPS positions, solid connecting lines between triangles represent odometry constraints, dashed lines represent loop closure constraints, and triangles represent poses that we need to optimize. By using this pose graph optimization model, we can optimize the pose.
[0058] The pose graph optimization problem is described formally below. For example... Figure 3 As shown, assume that the vehicle performs N pose estimations in one data collection trip, denoted as P. i For i ∈ [1, 2, ..., N], there is also a GPS signal. The sampling time point of the GPS signal may be different from the pose estimation time point. Due to the frequency limitation of the single-point GPS, it usually cannot correspond one-to-one with the pose of the relative trajectory. Here, we can interpolate it, denoted as g. j j∈[1, 2, ..., M]. Furthermore, discontinuous poses (e.g., ...) can be obtained through loop closure detection or matching. Figure 1 The pose measurement between p4 and p8 in the diagram is denoted as L. ij If abs(ij) > 1, the odometry measurement between consecutive poses is recorded as O.ij i+1 = j, and let G be an integer. k Let be the latitude and longitude projected coordinates (UTM coordinates). Based on these notations, the pose optimization problem can be represented as follows:
[0059]
[0060] In the above formula, the first term is the odometer error term, the second term is the lap time error term, and the third term is the GPS prior term. In this term, P... j，loc These are the Cartesian coordinate components in the pose. An optimized pose estimate can be obtained through an iterative method.
[0061] in, Figure 3 This involves optimizing the pose graph for a single-vehicle, single-trip trajectory. In crowdsourcing scenarios, there are multiple vehicles and multiple trips. Within each trajectory, there are odometry constraints between preceding and following poses, as well as loop closure constraints within or between trajectories. These, combined with GPS constraints, create a more complex pose graph. For details, please refer to [reference needed]. Figure 4 .
[0062] The formal description of this problem is as shown in the formula above. However, unlike the formula, crowdsourcing involves multiple trajectories. Besides the odometry and lap-loop constraints within each trajectory, there are also lap-loop constraints between trajectories. Therefore, we can still use the formula (1) above, but add a superscript l to each quantity, where the superscript l... g , l h Let g and h ∈ [1, 2, ..., K] to distinguish trajectories from different runs, and I be the set of all runs. Therefore, the problem can be described as:
[0063]
[0064] Here, the first and third terms are residual terms within the trajectory, while the second term (loop term) may exist between different trajectories. This means that this optimization problem cannot be simply divided according to the number of trajectories. Instead, the loop (or matching) relationships between trajectories need to be considered.
[0065] For each subgraph after partitioning, its pose optimization problem can be described as Equation (2) above. Based on Equation (2), the optimized pose estimate can be obtained by using an iterative method.
[0066] See Figure 5 This invention provides a distributed pose optimization system based on graph algorithms. The system includes an acquisition module 501, a definition module 502, a construction module 503, a partitioning module 504, and an optimization module 505, wherein:
[0067] The acquisition module 501 is used to acquire multiple trajectories collected by mapping vehicles within a geographical area;
[0068] Define module 502, which is used to define the connection vector between each pair of trajectories based on whether there is a matching relationship between them;
[0069] Module 503 is used to construct a connection graph between all trajectories based on the connection vector between every two trajectories;
[0070] The partitioning module 504 is used to partition the connected graph into multiple completely independent subgraphs using a subgraph partitioning algorithm in graph theory.
[0071] The optimization module 505 is used to optimize the pose of each sub-graph to obtain the optimized pose estimate.
[0072] It is understood that the distributed pose optimization system based on graph algorithms provided by this invention corresponds to the distributed pose optimization methods based on graph algorithms provided in the foregoing embodiments. The relevant technical features of the distributed pose optimization system based on graph algorithms can be referred to the relevant technical features of the distributed pose optimization methods based on graph algorithms, and will not be repeated here.
[0073] Please see Figure 6 , Figure 6 This is a schematic diagram illustrating an embodiment of the electronic device provided in this invention. For example... Figure 6 As shown, an embodiment of the present invention provides an electronic device 600, including a memory 610, a processor 620, and a computer program 611 stored in the memory 610 and executable on the processor 620. When the processor 620 executes the computer program 611, it implements the steps of a distributed pose optimization method based on graph algorithms.
[0074] Please see Figure 7 The figure is a schematic diagram of an embodiment of a computer-readable storage medium provided by the present invention. Figure 7 As shown, this embodiment provides a computer-readable storage medium 700, on which a computer program 711 is stored. When the computer program 711 is executed by a processor, it implements the steps of a distributed pose optimization method based on graph algorithms.
[0075] This invention provides a distributed pose optimization method and system based on graph algorithms. The distributed solution method for pose graph optimization in large-scale graph construction scenarios can divide the optimization problem based on matching (or loop closure) information, thereby making full use of computing resources to obtain a solution equivalent to (or close to) global optimization. This method does not require modification of the optimizer and is very easy to implement. It can also be well extended when there is a lot of data and a lot of trajectories.
[0076] It should be noted that the descriptions of each embodiment in the above embodiments have different focuses. For parts that are not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.
[0077] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.
[0078] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded computer, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.
[0079] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.
[0080] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.
[0081] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.
[0082] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.
Claims
1. A distributed pose optimization method based on graph algorithms, characterized in that, include: Obtain multiple trajectories collected by vehicles mapping within a geographic area; Define the connection vector between each pair of trajectories based on whether there is a matching relationship between them; Based on the connection vector between every two trajectories, construct a connection graph between all trajectories; The connected graph is divided into multiple completely independent subgraphs using a subgraph partitioning algorithm in graph theory. Pose optimization is performed on each subgraph to obtain the optimized pose estimate; The step of defining a connection vector between every two trajectories based on whether a matching relationship exists between them includes: Determine if there are any identical line segments on two tracks. If there are, there is a matching relationship between the two tracks, and the connection vector between the two tracks is set to 1. If there are no identical segments, there is no matching relationship between the two tracks, and the connection vector between the two tracks is set to 0. The determination of whether there are identical line segments on two trajectories, and if so, the existence of a matching relationship between the two trajectories, includes: Determine if there are any identical line segments on two tracks. If so, determine if there is a matching relationship between the two tracks based on the length of the identical line segments. If the length of the identical line segments is greater than a set length threshold, then there is a matching relationship between the two tracks; otherwise, there is no matching relationship between the two tracks.
2. The distributed pose optimization method according to claim 1, characterized in that, The step of optimizing the pose of each sub-graph to obtain the optimized pose estimate includes: The information of each subgraph is stored in a distributed optimizer. The pose of each subgraph is optimized based on the distributed optimizer to obtain the optimized pose estimate.
3. A distributed pose optimization system based on graph algorithms, characterized in that, include: The acquisition module is used to acquire multiple trajectories collected by mapping vehicles within a geographical area; Define the module to define the connection vector between each pair of trajectories based on whether there is a matching relationship between them; The building module is used to construct a connection graph between all trajectories based on the connection vector between every two trajectories; A partitioning module is used to divide the connected graph into multiple completely independent subgraphs using a subgraph partitioning algorithm in graph theory. The optimization module is used to optimize the pose of each sub-graph to obtain the optimized pose estimate; The step of defining a connection vector between every two trajectories based on whether a matching relationship exists between them includes: Determine if there are any identical line segments on two tracks. If there are, there is a matching relationship between the two tracks, and the connection vector between the two tracks is set to 1. If there are no identical segments, there is no matching relationship between the two tracks, and the connection vector between the two tracks is set to 0. The determination of whether there are identical line segments on two trajectories, and if so, the existence of a matching relationship between the two trajectories, includes: Determine if there are any identical line segments on two tracks. If so, determine if there is a matching relationship between the two tracks based on the length of the identical line segments. If the length of the identical line segments is greater than a set length threshold, then there is a matching relationship between the two tracks; otherwise, there is no matching relationship between the two tracks.
4. The distributed pose optimization system according to claim 3, characterized in that, The optimization module is a distributed optimizer, which is used to distribute the information of each subgraph and perform pose optimization on each subgraph to obtain the optimized pose estimate.
5. An electronic device, characterized in that, The system includes a memory and a processor, wherein the processor is used to implement the steps of the distributed pose optimization method based on graph algorithms as described in any one of claims 1-2 when executing a computer management program stored in the memory.
6. A computer-readable storage medium, characterized in that, It stores a computer management program, which, when executed by a processor, implements the steps of the distributed pose optimization method based on graph algorithms as described in any one of claims 1-2.