A method, device, equipment and medium for optimizing a Cartographer backend pose graph based on geometric constraints of a Transformer

Abstract

The application discloses a Cartographer backend pose graph optimization method and device based on geometric constraint Transformer, equipment and medium, the method comprises the following steps: obtaining the initial pose, point cloud data and constraint edge and loop node pair between nodes of each key frame node in the pose graph; the initial pose and the point cloud data are encoded and fused into the node initial feature; the space position coding is generated by the translation coordinates and is injected to obtain the input feature sequence; the input feature sequence is input into the cascade attention network which is trained end to end in advance with the geometric consistency joint loss function, and the constraint edge and the loop node pair are combined to update layer by layer to obtain the node updated feature; the node updated feature is decoded into the pose increment and is superimposed on the initial pose to obtain and output the final optimized pose. The application replaces the iterative solution with the layer-by-layer attention update and the one-time feedforward reasoning, so that the optimization time consumption linearly grows with the number of nodes and constraint edges, and the real-time requirement of large-scale long-time mapping is met.

Description

Technical Field

[0001] This invention relates to the field of data processing technology, and in particular to a method, apparatus, device, and medium for optimizing the Cartographer backend pose graph based on geometrically constrained Transformer. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology uses point cloud data acquired by LiDAR as input to simultaneously estimate the pose of a vehicle in an unknown environment and build an environmental map. It is widely used in large-scale, long-duration navigation scenarios such as industrial Automated Guided Vehicles (AGVs), warehouse robots, and autonomous driving. A typical SLAM system consists of a front-end and a back-end. The front-end is responsible for frame-by-frame scanning and matching and generating keyframe nodes, while the back-end is responsible for global consistency correction of the poses of all keyframe nodes. The core component responsible for global consistency correction is Pose Graph Optimization (PGO). A typical implementation of this system architecture is Google's open-source Cartographer SLAM system, whose back-end uses pose graph optimization based on a Ceres nonlinear least squares solver to complete the global consistency correction of keyframe node poses.

[0003] Existing pose graph optimization methods use pose graphs as a unified data structure. Nodes in the pose graph represent keyframe nodes and their poses, while edges represent constraints between nodes. These constraints include inter-frame constraints generated by inter-frame scanning matching of point cloud data from adjacent keyframe nodes, and loop closure constraints generated by the loop closure detection module when the carrier repeatedly traverses the same region. Pose graph optimization aims to minimize the weighted sum of squared residuals across all constraint edges. It iteratively solves for the poses of each keyframe node using a nonlinear least squares solver. Representative solution frameworks include the Ceres solver used in the Cartographer backend and the g2o general graph optimization framework proposed by Kümmerle et al. The objective function for pose graph optimization is as follows:

[0004] In the formula: To optimize the set of optimal poses of keyframes obtained from the solution; T is the candidate pose to be optimized; C is the set of constraint edges in the pose graph, including inter-frame scan matching constraints and loop closure constraints; To constrain the relative pose residual between node i and node j connected by the edge; Let be the covariance matrix of constraint (i,j), used to characterize the reliability of the observations for this constraint. The smaller the covariance, the higher the weight of this constraint in the optimization. The objective function means: to find the optimal pose configuration in the weighted least squares sense of all constraints, so as to minimize the sum of the Mahalanobis distances of the residuals of each constraint.

[0005] In the process of solving the above objective function, the solver needs to repeatedly linearize the residual terms and construct and solve the corresponding sparse linear equation system until the residual converges; parameters such as covariance matrix, iteration termination threshold and damping factor need to be manually set according to the specific scenario; at the same time, the above solution process only uses the relative pose constraints between nodes, and does not utilize the environmental structure information contained in the point cloud corresponding to the key frame nodes.

[0006] However, as the mapping mileage increases, the number of keyframe nodes in the pose graph continues to accumulate. The number of iterations required for the above iterative numerical solution and the computational cost per iteration both increase with the node size. The time consumption of pose graph optimization grows superlinearly, making it difficult to meet real-time requirements in large-scale, long-term mapping scenarios. Summary of the Invention

[0007] This invention provides a method, apparatus, device, and medium for optimizing the pose graph of the Cartographer backend based on geometrically constrained Transformer, aiming to solve the technical problem that the time consumption of pose graph optimization in the prior art increases superlinearly with the accumulation of the number of keyframe nodes, and it is difficult to meet the real-time requirements in large-scale long-term mapping scenarios.

[0008] In a first aspect, embodiments of the present invention provide a method for optimizing the Cartographer backend pose graph based on geometrically constrained Transformers, including: The initial pose and corresponding point cloud data of each keyframe node in the pose graph to be optimized are obtained, as well as the constraint edges and loop point pairs between keyframe nodes. The initial pose and point cloud data of each keyframe node are encoded into pose feature vectors and point cloud feature vectors, respectively, and then fused to obtain the initial features of the node. A spatial position code is generated based on the translation coordinates in the initial pose and injected into the node initial features of each keyframe node to obtain an input feature sequence; The input feature sequence is input into a cascaded attention network, and attention is updated layer by layer by combining the constraint edges and the loop nodes to obtain node update features. The node update features are decoded into pose increments for each keyframe node, and the pose increments are superimposed with the initial pose to obtain the optimized pose. The optimized pose is used as the final optimized pose output for each keyframe node; The cascaded attention network is a network that has been pre-trained end-to-end using a geometric consistency joint loss function.

[0009] Secondly, embodiments of the present invention provide a Cartographer backend pose graph optimization device based on geometrically constrained Transformer, comprising: The acquisition module is used to acquire the initial pose and corresponding point cloud data of each keyframe node in the pose graph to be optimized, as well as the constraint edges and loop point pairs between keyframe nodes. The feature encoding module is used to encode the initial pose and point cloud data of each key frame node into pose feature vectors and point cloud feature vectors respectively, and fuse them to obtain the initial features of the node. The position encoding module is used to generate spatial position codes based on the translation coordinates in the initial pose, and inject them into the node initial features of each keyframe node to obtain an input feature sequence. The attention update module is used to input the input feature sequence into the cascaded attention network, and perform attention update layer by layer by combining the constraint edge and the loop node pair to obtain the node update feature; The pose decoding module is used to decode the node update features into pose increments for each keyframe node, and to superimpose the pose increments with the initial pose to obtain the optimized pose. An optimized output module is used to output the optimized pose as the final optimized pose for each keyframe node. The cascaded attention network is a network that has been pre-trained end-to-end using a geometric consistency joint loss function.

[0010] Thirdly, embodiments of the present invention provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the Cartographer backend pose graph optimization method based on geometrically constrained Transformer of the first aspect.

[0011] Fourthly, embodiments of the present invention provide a computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, it implements the Cartographer backend pose graph optimization method based on geometrically constrained Transformer of the first aspect.

[0012] This invention provides a method for optimizing the backend pose graph of a Cartographer based on a geometrically constrained Transformer, comprising: acquiring the initial pose and corresponding point cloud data of each keyframe node in the pose graph to be optimized, as well as constraint edges and loopback node pairs between keyframe nodes; encoding the initial pose and point cloud data of each keyframe node into pose feature vectors and point cloud feature vectors respectively, and fusing them to obtain initial node features; generating spatial position codes based on the translation coordinates in the initial poses, and injecting them into the initial node features of each keyframe node to obtain an input feature sequence; inputting the input feature sequence into a cascaded attention network, and performing attention updates layer by layer in combination with the constraint edges and loopback node pairs to obtain node update features; decoding the node update features into the pose increments of each keyframe node, and superimposing the pose increments with the initial poses to obtain the optimized pose; and outputting the optimized poses as the final optimized poses of each keyframe node; wherein, the cascaded attention network is a network pre-trained end-to-end using a geometric consistency joint loss function. This invention encodes the initial pose and point cloud data of each keyframe node into initial node features and injects spatial position codes generated from the translation coordinates in the initial pose. This allows the cascaded attention network to complete constraint modeling layer by layer by combining constraint edges and loopback node pairs, under the condition of having spatial geometric priors. Then, the decoding and superposition of pose increments replaces the repeated linearization and iterative solution of residual terms. After end-to-end training with the geometric consistency joint loss function, the cascaded attention network can output the final optimized pose of all keyframe nodes with only one feedforward inference. The time consumption of pose graph optimization increases linearly with the number of keyframe nodes and the number of constraint edges, thus meeting the real-time requirements in large-scale long-term mapping scenarios.

[0013] This invention also provides a Cartographer backend pose graph optimization device, computer equipment, and storage medium based on geometrically constrained Transformer, which also have the above-mentioned beneficial effects. Attached Figure Description

[0014] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0015] Figure 1 A flowchart illustrating a Cartographer backend pose graph optimization method based on geometrically constrained Transformer provided in an embodiment of the present invention; Figure 2This is a schematic block diagram of a Cartographer backend pose graph optimization device based on geometrically constrained Transformer, provided as an embodiment of the present invention. Detailed Implementation

[0016] 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, not all, of the embodiments of the present invention. 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.

[0017] It should be understood that, when used in this specification and the appended claims, the terms "comprising" and "including" indicate the presence of the described features, integrals, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0018] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the invention. As used in this specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0019] It should also be further understood that the term "and / or" as used in this specification and the appended claims refers to any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0020] Please see below. Figure 1 , Figure 1 The flowchart of a Cartographer backend pose graph optimization method based on geometrically constrained Transformer provided in this embodiment of the invention specifically includes steps S101 to S106.

[0021] S101. Obtain the initial pose and corresponding point cloud data of each key frame node in the pose graph to be optimized, as well as the constraint edges and loop point pairs between key frame nodes. S102. The initial pose and point cloud data of each key frame node are encoded into pose feature vectors and point cloud feature vectors respectively, and fused to obtain the initial features of the node. S103. Generate a spatial position code based on the translation coordinates in the initial pose, and inject it into the node initial features of each key frame node to obtain an input feature sequence; S104. Input the input feature sequence into the cascaded attention network, and perform attention updates layer by layer by combining the constraint edges and the loop nodes to obtain node update features. S105. Decode the node update features into pose increments for each keyframe node, and superimpose the pose increments with the initial pose to obtain the optimized pose. S106. The optimized pose is output as the final optimized pose of each keyframe node.

[0022] In step S101, the initial pose and corresponding point cloud data of each key frame node in the pose graph to be optimized, as well as the constraint edges and loop point pairs between key frame nodes, are obtained respectively.

[0023] In one embodiment, step S101 includes: From the front end of the laser synchronous positioning and mapping system, obtain the key frame nodes generated sequentially during the mapping process; Read the initial pose of each keyframe node, which is composed of translation vectors and rotation matrices, and the corresponding point cloud data; Obtain the constraint edges generated by inter-frame scanning matching of the point cloud data of adjacent key frame nodes; Based on the constraint edges, obtain the loop closure point pairs and corresponding loop closure confidence scores output by the loop closure detection module; The initial pose, point cloud data, constraint edges, loop closure point pairs, and loop closure confidence are summarized to form the pose graph to be optimized.

[0024] In this embodiment, keyframe nodes refer to frames selected and retained from continuous laser scanning frames during the mapping process of the laser synchronous localization and mapping system's front end, according to a set time interval or spatial interval. The keyframe nodes are arranged sequentially according to their generation order, collectively forming the processing object for the backend pose graph optimization. The pose graph is a graph structure composed of nodes and edges, where the nodes are the keyframe nodes and their poses, and the edges are the constraint edges between nodes. Pose graph optimization is the process of globally consistently correcting the poses of all nodes given all constraint edges. The purpose of this step is to read and summarize all the input data required for subsequent steps from the laser synchronous localization and mapping system to form the pose graph to be optimized.

[0025] Specifically, the keyframe nodes generated sequentially during the mapping process are first obtained from the front end of the laser-synchronized localization and mapping (LSTLM) system. In this embodiment, the LTLM system is Google's open-source Cartographer system. Its front end estimates the motion increment of the carrier by matching adjacent laser scanning frames, and generates a new keyframe node when the displacement or rotation of the carrier exceeds a set threshold. Therefore, there is a natural temporal relationship between the keyframe nodes.

[0026] Furthermore, the initial pose of each keyframe node, consisting of a translation vector and a rotation matrix, and the corresponding point cloud data are read. The initial pose of the i-th keyframe node is denoted as... =( , ), which belongs to the three-dimensional special Euclidean group SE(3), in which It is a translation vector, belonging to the three-dimensional real vector space. This is used to describe the position of the keyframe node in three-dimensional space; This is a rotation matrix, belonging to the 3D special orthogonal group SO(3), used to describe the pose of this keyframe node. The corresponding point cloud data is denoted as... This is a set of 3D points acquired by the lidar at the corresponding moment of the keyframe node, carrying the environmental structure information observed at that keyframe node. It should be noted that the initial pose read here is obtained frame by frame by the front-end scanning matching and has not yet undergone global consistency correction, so there is cumulative drift under long-distance operation.

[0027] Furthermore, constraint edges generated by inter-frame scan matching of the point cloud data of adjacent keyframe nodes are obtained. Inter-frame scan matching refers to the process of taking the point cloud data of two keyframe nodes as input and solving the relative pose between them through point cloud registration. The solved relative pose is the observation value carried by the constraint edge. The constraint edge represents the geometrically satisfying relative pose relationship between two keyframe nodes and is the most numerous and densely distributed type of edge in the pose graph.

[0028] Then, based on the constraint edges, the loop closure point pairs and corresponding loop closure confidence scores output by the loop closure detection module are obtained. A loop closure refers to the phenomenon where the carrier repeatedly passes through the same area during operation. The loop closure detection module compares the point cloud data of the current keyframe node with those of historical keyframe nodes to determine whether they observe the same area. If the determination is correct, a loop closure point pair is output, along with a loop closure confidence score characterizing the reliability of the determination. The loop closure point pair connects two keyframe nodes that are temporally far apart in the pose graph, thus constraining the drift accumulated over long distances back to the true position in one go. The loop closure confidence score provides a basis for controlling the strength of this type of constraint in subsequent steps. It should be noted that loop closure detection may result in false matches, where two structurally similar but actually different areas in the environment are identified as the same area. Such false matches typically result in low loop closure confidence scores.

[0029] Finally, the initial pose, point cloud data, constraint edges, loop closure point pairs, and loop closure confidence scores are summarized to form the pose graph to be optimized. Accordingly, the pose graph to be optimized simultaneously includes the pose and observation information of each node, the local constraint relationships between nodes, and the long-range constraint relationships between nodes, providing complete input for subsequent steps.

[0030] For example, in the scenario of 3D laser synchronous positioning and mapping of industrial automated guided vehicles, a 16-line LiDAR is used to collect long-term navigation data covering various environments such as workshops, warehouses and corridors. The operating mileage is about 10km, and the number of key frame nodes generated by the front end is about 12,000. Correspondingly, the pose graph to be optimized contains about 12,000 nodes, as well as all the constraint edges generated by inter-frame scan matching and loop closure detection.

[0031] It is worth noting that the acquisition process in this step can be implemented through interface adaptation, that is, directly receiving the node pose array, constraint edge list, loop closure detection results, and point cloud data corresponding to each keyframe node output by the native pose graph module of the Cartographer system, without making any modifications to the front-end scan matching module, loop closure detection module, and subgraph construction module of the system; at the same time, the original optimization triggering mechanism of the system is retained, for example, when a set number of keyframe nodes are added to the pose graph, the acquisition process and subsequent optimization process described in this embodiment are triggered once.

[0032] In step S102, the initial pose and point cloud data of each key frame node are encoded into pose feature vectors and point cloud feature vectors, respectively, and fused to obtain the initial features of the node.

[0033] In one embodiment, step S102 includes: The initial pose is decomposed into a translation vector and a rotation matrix, and the rotation matrix is ​​converted into a unit quaternion; The translation vector and the unit quaternion are concatenated to form a seven-dimensional vector, which is then linearly projected to obtain the pose feature vector. Voxel downsampling is performed on the point cloud data of the keyframe node to which the pose feature vector belongs to obtain the downsampled point cloud; Extract the global structural feature vector of the downsampled point cloud, and then obtain the point cloud feature vector through linear projection; The pose feature vector and the point cloud feature vector are concatenated and fused to obtain the initial node features of each keyframe node.

[0034] In this embodiment, the pose feature vector and the point cloud feature vector refer to fixed-length vectors that are obtained by mapping the initial pose and point cloud data respectively and can be directly used for attention calculation. In the Transformer model, they are also called pose token and point cloud feature token, where token refers to the basic unit in the input sequence processed by the Transformer model.

[0035] The purpose of this step is to construct a unified feature representation for each keyframe node in the pose graph, so that the feature carries both the pose information of the keyframe node itself and the environmental structure information observed by the keyframe node.

[0036] Specifically, the initial pose is first decomposed into a translation vector and a rotation matrix, and the rotation matrix is ​​then converted into a unit quaternion. A unit quaternion is a four-dimensional vector with a magnitude of 1, consisting of one real part and three imaginary parts, which can represent three-dimensional rotations without singularity. Compared to a rotation matrix that contains nine elements and requires orthogonality constraints, a unit quaternion can completely describe the pose with only four elements, making it more suitable as input to a neural network.

[0037] Furthermore, the translation vector and the unit quaternion are concatenated to form a seven-dimensional vector, which is then linearly projected to obtain the pose feature vector. Linear projection refers to performing matrix multiplication on the input vector using a learnable weight matrix, thereby mapping it to a feature space of a specified dimension. The formula for calculating the pose feature vector is as follows:

[0038] In the formula: Let be the pose feature vector of the i-th keyframe node; is the learnable weight matrix of the pose linear projection layer; Let be the translation vector of the i-th keyframe node; Let be the unit quaternion of the i-th keyframe node; [·;·] denotes vector concatenation; Let be a d-dimensional real vector space; d is the total dimension of the model features. For example, d can be 256.

[0039] Furthermore, voxel downsampling is performed on the point cloud data of the keyframe nodes to which the pose feature vector belongs, resulting in a downsampled point cloud. Voxel downsampling involves dividing the 3D space of the point cloud into cubic voxels with fixed side lengths, and replacing these points with the centroids of all points falling within the same voxel, thereby reducing the number of points while preserving the overall structure of the point cloud. After voxel downsampling, the problem of uneven point cloud density distribution is suppressed, and the computational load required for subsequent feature extraction is correspondingly reduced.

[0040] The global structural feature vector of the downsampled point cloud is then extracted, and linear projection is used to obtain the point cloud feature vector. The global structural feature vector is a fixed-length descriptor obtained by statistically analyzing or encoding the spatial distribution of the entire frame's point cloud, used to characterize the environmental structure of the keyframe node's location. The formula for calculating the point cloud feature vector is as follows:

[0041] In the formula: Let be the point cloud feature vector of the i-th keyframe node; φ is the learnable weight matrix of the linear projection layer of the point cloud features; φ(·) is the global structural feature extraction function of the point cloud, which can be either Scan Context features or point cloud statistical histogram features; Let be the point cloud corresponding to the i-th keyframe node.

[0042] Finally, the pose feature vector and the point cloud feature vector are concatenated and fused to obtain the initial node features of each keyframe node. The calculation formula is as follows:

[0043] In the formula: is the initial node feature obtained by fusing the i-th keyframe node; [·;·] represents the concatenation operation of the pose feature vector and the point cloud feature vector, which enables the initial node feature to carry both pose information and environmental structure information.

[0044] Therefore, compared to the method that only uses node pose information, the initial node features obtained in this step introduce the global structure information of the point cloud, which enables the subsequent attention calculation to distinguish between keyframe nodes with similar poses but different environmental structures, and provides additional criteria for the correction of cumulative drift under long-distance operation.

[0045] In step S103, a spatial position code is generated based on the translation coordinates in the initial pose and injected into the node initial features of each keyframe node to obtain an input feature sequence.

[0046] In one embodiment, step S103 includes: From the initial pose of each keyframe node, the translation coordinates on the three coordinate axes are extracted, and sine and cosine functions of different frequencies are used to encode each coordinate component to obtain three coded components. The three encoded components are concatenated in order of coordinate axes to obtain the spatial position encoding of each keyframe node. The spatial location code is injected into the node initial feature of the corresponding keyframe node in an element-wise additive manner, and the node initial features after being injected with the spatial location code are arranged in the temporal order of the keyframe nodes to obtain the input feature sequence.

[0047] In this embodiment, spatial location encoding refers to a vector generated based on the absolute position of the keyframe node in three-dimensional space, which matches the initial features of the node, and is used to provide spatial geometric priors between nodes to the cascaded attention network.

[0048] It should be noted that attention calculation itself is not sensitive to the arrangement of elements in the input sequence. Without additional positional information, the cascaded attention network will be unable to distinguish the relative distances of nodes in different keyframes in space. The Transformer model natively uses temporal positional encoding generated based on sequence numbers, which describes the sequential relationship of elements in the sequence. However, in the pose graph optimization scenario, the strength of the constraints between nodes is determined by the positional relationship of the nodes in three-dimensional space. Therefore, this step is changed to generating spatial positional encoding based on the translation coordinates in the initial pose.

[0049] Specifically, firstly, the translation coordinates on the three coordinate axes are extracted from the initial pose of each keyframe node. Then, sine and cosine functions of different frequencies are used to encode each coordinate component, resulting in three coded components. Next, the three coded components are concatenated in the order of the coordinate axes to obtain the spatial position code of each keyframe node. Finally, the spatial position code is injected into the initial node features of the corresponding keyframe node using an element-wise addition method. The calculation formula for the above process is as follows:

[0050] In the formula: PE(·) is the spatial location coding function; p is the translation coordinate of the keyframe node, i.e., p=(x,y,z); , , These are the position encoding components along the x, y, and z coordinate axes, respectively. (a) is the encoding of the k-th frequency component to the coordinate component a, which is composed of a pair of sine and cosine functions; Let be the angular frequency of the k-th component, according to = The rule settings allow different components to correspond to different encoding scales; k is the frequency component index; d is the feature dimension; The initial node features of the i-th keyframe node; Let be the translation coordinates of the i-th keyframe node; The features of the i-th keyframe node after spatial location encoding are injected.

[0051] As shown in the above equation, the angular frequency decreases as the index k increases. This means that components with smaller indices encode coordinate components at a higher frequency, which can be used to distinguish keyframe nodes that are spatially close; components with larger indices encode coordinate components at a lower frequency, which can be used to characterize large-scale spatial positional differences. After concatenating the encoded components of the three coordinate axes in the order of x, y, and z, the spatial position encoding dimension is 3d / 2.

[0052] Finally, the initial features of each node after being injected with spatial location encoding are arranged in temporal order according to the keyframe nodes to obtain the input feature sequence. Accordingly, each element in the input feature sequence carries simultaneously the pose information, environmental structure information, and spatial location information of the corresponding keyframe node. When calculating the attention weights between nodes, the cascaded attention network has the ability to perceive the spatial geometric distribution.

[0053] In step S104, the input feature sequence is input into a cascaded attention network, and attention is updated layer by layer by combining the constraint edges and the loop nodes to obtain node update features.

[0054] In one embodiment, the cascaded attention network includes a local window attention layer, a loop-cross attention layer, and a global structure attention layer cascaded sequentially; step S104 includes: The input feature sequence is input into the local window attention layer to compute self-attention between nodes within a temporal sliding window; A geometric bias term is generated based on the translation distance and rotation angle between nodes, and the weights of the self-attention are adjusted to obtain local update features; The local update features are input into the loop-cross attention layer to perform bidirectional cross-attention updates between the loop point pairs. The update amplitude is then controlled by the loop confidence weighting to obtain the loop update features, which are then input into the global structure attention layer. In the global structure attention layer, the node update features are calculated using the constraint edges as attention masks.

[0055] In this embodiment, a cascaded attention network refers to a neural network composed of multiple attention layers connected in series, where the output of the previous layer serves as the input of the next layer, and each layer applies geometric constraints of different granularities. The three attention layers in this embodiment correspond one-to-one with the three types of constraints in pose graph optimization: the local window attention layer corresponds to inter-frame constraints between adjacent keyframe nodes, the loop closure cross-attention layer corresponds to loop closure constraints, and the global structure attention layer corresponds to the overall topological constraints of the pose graph. Therefore, each layer update of the cascaded attention network has a corresponding geometric meaning.

[0056] Specifically, the input feature sequence is first input into the local window attention layer to calculate self-attention between nodes within a temporal sliding window. The temporal sliding window refers to a neighborhood centered on the current keyframe node, extending temporally forward and backward by a predetermined number of keyframe nodes. Self-attention calculation is performed only within this neighborhood and does not involve keyframe nodes outside this neighborhood, thus aligning with the local constraints established by inter-frame scan matching. For example, the size W of the temporal sliding window can be 8, meaning each keyframe node only calculates self-attention with its eight adjacent keyframe nodes.

[0057] Furthermore, a geometric bias term is generated based on the translation distance and rotation angle between nodes, and the weights of the self-attention are adjusted to obtain locally updated features. The geometric bias term is a correction amount calculated based on the difference between the initial poses of two keyframe nodes and directly superimposed on the attention score. The calculation formula for the local window attention layer is as follows:

[0058] In the formula: The output of the local window attention layer is Q, which represents the local updated features. Q, K, and V are the query matrix, key matrix, and value matrix of the attention mechanism, respectively, all of which are obtained by linear projection of the input feature sequence. is the attention head dimension; softmax(·) is the normalization exponential function used to normalize the attention scores to weights that sum to 1; β is the weighting coefficient of the geometric bias term; G is the geometric bias matrix; These are the elements corresponding to nodes i and j in the geometric bias matrix; , Let be the translation vectors of nodes i and j, respectively; ‖·‖ is the L2 norm; α is the weight of the rotation difference term; θ( , Let be the rotation matrix between node i and node j. , The geodesic angle between two attitudes is the minimum angle required to rotate from one attitude to another.

[0059] As shown in the above formula, the geometric bias term is preceded by a negative sign. The smaller the translation distance and rotation angle between two keyframe nodes, the larger the value of the geometric bias term, and the higher the attention weight obtained by that node pair. Accordingly, spatially adjacent node pairs with consistent poses dominate local updates, which is consistent with the physical meaning of local constraints in pose graph optimization.

[0060] Furthermore, the locally updated features are input into the loop-cross attention layer to perform bidirectional cross-attention updates between the loop node pairs. The update magnitude is then controlled by weighting the loop confidence to obtain loop-cross updated features, which are then input into the global structure attention layer. Cross-attention refers to attention calculation using the features of one node as the query and the features of another node as the key and value; bidirectional cross-attention refers to performing the above calculation symmetrically between the two nodes of the loop node pair, causing their features to converge. The calculation formula for the loop-cross attention layer is as follows:

[0061] In the formula: The features of node i after the attention update via loop closure; , , i and j are the features before the update, respectively; σ(·) is the sigmoid function, used to map the loop closure confidence to between 0 and 1; The loop closure confidence score between node i and node j, output by the loop closure detection module, is used as a gating factor to control the influence strength of the loop closure constraint; Attn( , ) represents the bidirectional cross-attention output between node i and node j. Performing a symmetric update on node j completes the bidirectional consistency constraint for this node pair.

[0062] As shown in the above equation, the lower the confidence of the loop closure, the smaller the gating coefficient obtained after mapping by the S-shaped growth function, and the smaller the update amplitude of the node features of the loop closure node pair. Accordingly, the impact of low-confidence loop closure node pairs corresponding to loop closure mismatches is automatically weakened in the feature update stage, and will not cause the optimization results to diverge as if the loop closure constraint were added as a hard constraint to the iterative solution.

[0063] Finally, in the global structure attention layer, the node update features are calculated using the constraint edges as attention masks. The attention mask is a matrix with the same shape as the attention score matrix, taking values ​​of 0 or 1, used to mask out node pairs that do not need to participate in the calculation before normalization. The calculation formula for the global structure attention layer is as follows:

[0064] In the formula: This is the output of the global structure attention layer, i.e., the node update features; Q, K, V and The meaning is the same as in the above formula; M is the pose graph constraint edge mask matrix, whose elements The value is 1 only if there is a constraint edge between node i and node j, i.e. (i,j) belongs to the constraint edge set E; otherwise, it is 0. E is the constraint edge set of the pose graph. ⊙ represents element-wise multiplication, which is used to mask the attention scores of unconstrained node pairs before normalization.

[0065] As shown in the above equation, the global attention layer only calculates attention between node pairs that are actually connected by constraints in the pose graph. Therefore, its computational cost is proportional to the number of constraint edges, with a complexity of O(|E|), and does not increase with the square of the number of nodes. Accordingly, cascaded attention networks can be extended to pose graphs with tens of thousands of nodes without failing due to the expansion of attention computation.

[0066] As an alternative, the three attention layers mentioned above can be combined with a feedforward network and layer normalization to form an encoder block. A predetermined number of encoder blocks can be stacked to further enhance the optimization effect. The feedforward network performs node-by-node nonlinear transformations on the features of each node, and layer normalization stabilizes the numerical distribution of features across layers. For example, the number of stacked encoder blocks, L, can be 4, and the model feature dimension, d, can be 256.

[0067] In step S105, the node update features are decoded into pose increments for each keyframe node, and the pose increments are superimposed with the initial pose to obtain the optimized pose.

[0068] In one embodiment, step S105 includes: The node update features of each keyframe node are input into the translation decoder and the attitude decoder respectively. The translation decoder outputs the translation increment, and the attitude decoder outputs the corresponding quaternion increment. The quaternion increment is normalized to a unit length and then converted into a rotation increment represented by a rotation matrix. The translation increment and the rotation increment are combined to obtain the pose increment of each keyframe node; The pose increment is multiplied by right and added to the corresponding initial pose to obtain the optimized pose of each keyframe node.

[0069] In this embodiment, the pose increment refers to the correction amount applied to the initial pose of the keyframe node, rather than the absolute pose of the keyframe node.

[0070] It should be noted that this step uses incremental decoding instead of direct regression of absolute pose because the absolute pose of keyframe nodes has a wide range of values ​​in a large scene, making direct regression difficult; while the values ​​of pose increments are concentrated around zero, and the cascaded attention network only needs to learn the correction amount of the initial pose, making the training process easier to converge.

[0071] Specifically, the node update features of each keyframe node are first input into the translation decoder and the pose decoder, respectively. The translation decoder outputs the translation increment, and the pose decoder outputs the corresponding quaternion increment. Then, the quaternion increment is normalized to a unit length and converted into a rotation increment represented by a rotation matrix. Next, the translation increment and the rotation increment are combined to obtain the pose increment of each keyframe node. Finally, the pose increments are right-multiplied and added to the corresponding initial pose to obtain the optimized pose of each keyframe node. The calculation formulas for the above process are as follows:

[0072] In the formula: , These are the translation increment and quaternion increment of node i, respectively; , These are the learnable weight matrices for the translation decoder and the attitude decoder, respectively. Update the node features for node i; L2norm(·) is the L2 normalization operation, used to ensure that the modulus of the quaternion increment is 1; Let be the initial pose of node i; Exp(·) is the SE(3) exponential mapping, used to map the increment in Lie algebra form to the pose transformation; Let be the SE(3) Lie algebra pose increment of node i, with the superscript ∧ indicating a Lie algebra element; Δt is the translation increment component of this pose increment; Let be the rotation increment component of the pose increment; se(3) is the Lie algebra space corresponding to SE(3); The optimized pose of node i is obtained by multiplying the initial pose by the incremental transformation.

[0073] In the above formula, the Lie algebra refers to the tangent space of the Lie group at the identity element. The Lie algebra se(3) corresponding to SE(3) is a six-dimensional vector space, where the first three dimensions correspond to translation increments and the last three dimensions correspond to rotation increments. The six-dimensional vector can be restored to the pose transformation in SE(3) through exponential mapping. Using Lie algebra to represent the pose increment allows the increment to participate in the calculation in the form of an unconstrained six-dimensional vector, avoiding the inconvenience caused by directly imposing orthogonality constraints on the rotation matrix.

[0074] Normalizing the quaternion increment to a unit length is to ensure that the rotation it represents is a true three-dimensional rotation; if normalization is not performed, the magnitude of the four-dimensional vector output by the decoder may deviate from 1, and the matrix obtained by its conversion will no longer be an orthogonal matrix, and therefore cannot represent rotation.

[0075] The right-multiplication method of superposition refers to treating the pose increment as a transformation defined in the local coordinate system of the keyframe node itself, and then compounding it onto the initial pose. Since the node update features relied upon by the decoding head are centered on the keyframe node, using the right-multiplication method ensures that the definition of the increment is consistent with the meaning of the features.

[0076] In step S106, the optimized pose is used as the final optimized pose output for each keyframe node. It should be noted that the cascaded attention network is a network pre-trained end-to-end using a geometric consistency joint loss function, and its training process is completed offline. Steps S101 to S106 are the processing steps in the usage phase, where attention updates and pose increment decoding are completed by the trained cascaded attention network through a single feedforward inference step.

[0077] In one embodiment, the cascaded attention network is pre-trained in the following manner: The observed relative pose of the constrained edge is compared with the optimized pose of its two end nodes, and the scan matching residual loss is calculated. The pose continuity loss is calculated based on the second-order difference of the optimized poses of three adjacent frames. The loop overlap loss of the loop closure point pair is calculated by weighting based on the loop closure confidence. The scan matching residual loss, the pose continuity loss and the loop overlap loss are then weighted and superimposed to obtain the geometric consistency joint loss function. Using the geometric consistency joint loss function as the training objective, the cascaded attention network is trained end-to-end to obtain a pre-trained cascaded attention network.

[0078] In this embodiment, the geometric consistency joint loss function refers to the training objective constructed entirely based on the geometric constraints in the pose graph, with each component corresponding to a type of constraint residual in the pose graph optimization.

[0079] It should be noted that by using the geometric consistency joint loss function for end-to-end training, there is no need to manually derive the Jacobian matrix of the residual term for each pose component, nor is there a need to manually set the covariance matrix, iteration termination threshold and damping factor and other solution parameters. The cascaded attention network can learn the pose optimization logic on its own during the training process.

[0080] Specifically, the observed relative pose of the constrained edge is first compared with the optimized poses of its two endpoints, and the scan matching residual loss is calculated. The scan matching residual loss corresponds to the inter-frame constraint residual in pose graph optimization, and its calculation formula is as follows:

[0081] In the formula: C represents the scan matching residual loss; C is the set of inter-frame scan matching constraints. The front-end scans and matches the relative poses between observed nodes i and j; , Let i and j be the optimized poses of nodes i and j, respectively; log(·) is the SE(3) logarithmic mapping, used to map the relative pose transformation to the Lie algebra space to calculate the residual; ||·||² is the squared L2 norm of the Lie algebra residual vector. This loss measures the deviation between the optimized inter-frame relative pose and the front-end scan matching observations. The smaller the deviation, the better the optimization result matches the front-end observations.

[0082] Furthermore, the pose continuity loss is calculated based on the second-order difference of the optimized poses from three adjacent frames. The pose continuity loss penalizes abrupt temporal changes in pose, and its calculation formula is as follows:

[0083] In the formula: This is due to pose continuity loss; , , These represent the optimized poses of the i-1, i, and i+1 keyframe nodes, respectively; the meanings of log(·) and ||·||² are the same as in the above formula. This loss penalizes the second-order difference formed by the three adjacent frames, suppressing drastic changes in pose and ensuring that the optimized trajectory maintains smooth motion, consistent with the prior knowledge of approximately uniform motion of carriers such as industrial automated guided vehicles.

[0084] Then, the loop overlap loss of the loop link pair is calculated based on the loop confidence score. The loop overlap loss corresponds to the loop constraint residual in pose graph optimization, and its calculation formula is as follows:

[0085] In the formula: is the loop overlap loss; L is the set of loop closure constraints; The loop closure confidence score between node i and node j, output by the loop closure detection, is used as the weighting coefficient for this constraint term to reduce the impact of mismatched loops. The relative pose observations between node i and node j are given for loop closure detection; log(·), ‖·‖² and , The meaning is the same as in the above formula. By weighting each loop constraint term with the loop confidence, the loop node pairs with high confidence dominate the training direction, while the influence of loop node pairs with low confidence is automatically weakened. This weighting method, together with the confidence gating in the loop cross-attention layer, constitutes a dual suppression of loop mismatch.

[0086] The weighted summation of the scan matching residual loss, pose continuity loss, and loop closure overlap loss yields the geometric consistency joint loss function, calculated as follows:

[0087] In the formula: ℒ is the total loss function for end-to-end training, i.e., the geometric consistency joint loss function; , , These are, respectively, scan matching residual loss, pose continuity loss, and loop closure overlap loss; , , These are the weighting coefficients for each component of the loss, used to balance the contributions of different geometric priors to the training. For example, You can use 1.0. 0.3 can be taken. You can take 0.5.

[0088] Finally, using the geometric consistency joint loss function as the training objective, the cascaded attention network is trained end-to-end. Feedforward inference is then performed on the trained cascaded attention network to output the final optimized pose of each keyframe node. End-to-end training refers to incorporating all stages from node feature encoding to pose incremental decoding into the same computational graph, where the gradient of the joint loss function uniformly drives the parameter updates of each stage. For example, the training phase can employ an Adaptive Moment Estimation (Adam) optimizer with a learning rate of 1e-4, a batch size of 32, and training for 200 epochs until convergence.

[0089] Once trained, the cascaded attention network requires no further iterations. The features of all keyframe nodes in the pose graph are input into the network all at once. After layer-by-layer attention updates and the decoding and superposition of pose increments, the final optimized pose of all keyframe nodes is output. This process constitutes one feedforward inference. The output final optimized pose is given as a node pose array, with a format consistent with the output of the iterative solver. It can be directly written back to the pose graph data structure of the laser synchronous localization and mapping system for use in subsequent mapping and navigation stages.

[0090] For example, in the aforementioned scenario of 3D laser synchronous localization and mapping (LSMR) of an industrial automated guided vehicle, a computing platform consisting of an Intel i7-12700H CPU and an NVIDIA RTX 3060 GPU was used to perform one optimization on a pose graph containing approximately 12,000 keyframe nodes. The method described in this embodiment took 230ms, while the iterative nonlinear least squares solver took 1280ms, a reduction of approximately 82.0%. The mean absolute trajectory error (ATE) decreased from 0.187m to 0.180m, improving pose accuracy by approximately 3.7%. In cases with loop mismatches, the method described in this embodiment did not exhibit divergence in optimization results, eliminating the need for manual removal of mismatched loop pairs. Furthermore, the method described in this embodiment only requires loading a pre-trained model, without the need to individually fine-tune more than ten solver parameters.

[0091] In summary, the Cartographer backend pose graph optimization method based on geometrically constrained Transformer provided in this invention encodes the initial pose and point cloud data of each keyframe node into pose feature vectors and point cloud feature vectors, and fuses them into initial node features. Then, it injects spatial position encoding generated by translation coordinates. This allows the cascaded attention network to complete constraint modeling layer by layer, combining local window attention layers, loop-cross attention layers, and global structure attention layers with constraint edges and loop-cross point pairs, under the condition of having spatial geometric priors. In this way, the decoding and superposition of pose increments replaces the repeated linearization and iterative solution of residual terms. After end-to-end training with the geometric consistency joint loss function, the cascaded attention network only needs one feedforward inference to output the final optimized pose of all keyframe nodes. The time consumption of pose graph optimization increases linearly with the number of keyframe nodes and the number of constraint edges, thereby solving the technical problem in the prior art where the time consumption of pose graph optimization increases superlinearly with the accumulation of the number of keyframe nodes and is difficult to meet real-time requirements in large-scale long-term mapping scenarios.

[0092] Combination Figure 2 As shown, Figure 2 A schematic block diagram of a Cartographer back-end pose graph optimization device based on geometrically constrained Transformer provided in this embodiment of the invention. The Cartographer back-end pose graph optimization device 200 based on geometrically constrained Transformer includes: The acquisition module 201 is used to acquire the initial pose and corresponding point cloud data of each key frame node in the pose graph to be optimized, as well as the constraint edges and loop point pairs between key frame nodes. The feature encoding module 202 is used to encode the initial pose and point cloud data of each key frame node into pose feature vectors and point cloud feature vectors respectively, and fuse them to obtain the initial features of the node. The position encoding module 203 is used to generate a spatial position code based on the translation coordinates in the initial pose, and inject it into the node initial features of each key frame node to obtain an input feature sequence; Attention update module 204 is used to input the input feature sequence into the cascaded attention network, and perform attention update layer by layer in combination with the constraint edge and the loop node pair to obtain node update features; The pose decoding module 205 is used to decode the node update features into pose increments for each keyframe node, and to superimpose the pose increments with the initial pose to obtain the optimized pose. The optimized output module 206 is used to output the optimized pose as the final optimized pose of each keyframe node.

[0093] The cascaded attention network is a network that has been pre-trained end-to-end using a geometric consistency joint loss function.

[0094] In this embodiment, the Cartographer backend pose graph optimization device 200 based on geometric constraint Transformer includes: an acquisition module 201, used to acquire the initial pose and corresponding point cloud data of each keyframe node in the pose graph to be optimized, as well as constraint edges and loop point pairs between keyframe nodes; a feature encoding module 202, used to encode the initial pose and point cloud data of each keyframe node into pose feature vectors and point cloud feature vectors respectively, and fuse them to obtain the initial features of the nodes; and a position encoding module 203, used to generate spatial position codes based on the translation coordinates in the initial poses and inject them into each keyframe node. In the initial node features, an input feature sequence is obtained; the attention update module 204 is used to input the input feature sequence into the cascaded attention network, and perform attention update layer by layer in combination with the constraint edges and the loop nodes to obtain node update features; the pose decoding module 205 is used to decode the node update features into the pose increment of each keyframe node, and superimpose the pose increment with the initial pose to obtain the optimized pose; the optimization output module 206 is used to output the optimized pose as the final optimized pose of each keyframe node; wherein, the cascaded attention network is a network pre-trained end-to-end with a geometric consistency joint loss function.

[0095] In one embodiment, the acquisition module 201 is specifically used for: From the front end of the laser synchronous positioning and mapping system, obtain the key frame nodes generated sequentially during the mapping process; Read the initial pose of each keyframe node, which is composed of translation vectors and rotation matrices, and the corresponding point cloud data; Obtain the constraint edges generated by inter-frame scanning matching of the point cloud data of adjacent key frame nodes; Based on the constraint edges, obtain the loop closure point pairs and corresponding loop closure confidence scores output by the loop closure detection module; The initial pose, point cloud data, constraint edges, loop closure point pairs, and loop closure confidence are summarized to form the pose graph to be optimized.

[0096] In one embodiment, the feature encoding module 202 is specifically used for: The initial pose is decomposed into a translation vector and a rotation matrix, and the rotation matrix is ​​converted into a unit quaternion; The translation vector and the unit quaternion are concatenated to form a seven-dimensional vector, which is then linearly projected to obtain the pose feature vector. Voxel downsampling is performed on the point cloud data of the keyframe node to which the pose feature vector belongs to obtain the downsampled point cloud; Extract the global structural feature vector of the downsampled point cloud, and then obtain the point cloud feature vector through linear projection; The pose feature vector and the point cloud feature vector are concatenated and fused to obtain the initial node features of each keyframe node.

[0097] In one embodiment, the position encoding module 203 is specifically used for: From the initial pose of each keyframe node, the translation coordinates on the three coordinate axes are extracted, and sine and cosine functions of different frequencies are used to encode each coordinate component to obtain three coded components. The three encoded components are concatenated in order of coordinate axes to obtain the spatial position encoding of each keyframe node. The spatial location code is injected into the node initial feature of the corresponding keyframe node in an element-wise additive manner, and the node initial features after being injected with the spatial location code are arranged in the temporal order of the keyframe nodes to obtain the input feature sequence.

[0098] In one embodiment, the cascaded attention network includes a local window attention layer, a loop-cross attention layer, and a global structure attention layer cascaded sequentially; the attention update module 204 is specifically used for: The input feature sequence is input into the local window attention layer to compute self-attention between nodes within a temporal sliding window; A geometric bias term is generated based on the translation distance and rotation angle between nodes, and the weights of the self-attention are adjusted to obtain local update features; The local update features are input into the loop-cross attention layer to perform bidirectional cross-attention updates between the loop point pairs. The update amplitude is then controlled by the loop confidence weighting to obtain the loop update features, which are then input into the global structure attention layer.

[0099] In the global structure attention layer, the node update features are calculated using the constraint edges as attention masks.

[0100] In one embodiment, the pose decoding module 205 is specifically used for: The node update features of each keyframe node are input into the translation decoder and the attitude decoder respectively. The translation decoder outputs the translation increment, and the attitude decoder outputs the corresponding quaternion increment. The quaternion increment is normalized to a unit length and then converted into a rotation increment represented by a rotation matrix. The translation increment and the rotation increment are combined to obtain the pose increment of each keyframe node; The pose increment is multiplied by right and added to the corresponding initial pose to obtain the optimized pose of each keyframe node.

[0101] In one embodiment, the cascaded attention network is pre-trained in the following manner: The observed relative pose of the constrained edge is compared with the optimized pose of its two end nodes, and the scan matching residual loss is calculated. The pose continuity loss is calculated based on the second-order difference of the optimized poses of three adjacent frames. The loop overlap loss of the loop closure point pair is calculated by weighting based on the loop closure confidence. The scan matching residual loss, the pose continuity loss and the loop overlap loss are then weighted and superimposed to obtain the geometric consistency joint loss function. Using the geometric consistency joint loss function as the training objective, the cascaded attention network is trained end-to-end to obtain a pre-trained cascaded attention network.

[0102] Since the embodiments of the apparatus and the embodiments of the method correspond to each other, please refer to the description of the embodiments of the method for the embodiments of the apparatus, which will not be repeated here.

[0103] This invention also provides a computer-readable storage medium storing a computer program thereon, which, when executed, can perform the steps provided in the above embodiments. The storage medium may include various media capable of storing program code, such as a USB flash drive, a portable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0104] This invention also provides a computer device, which may include a memory and a processor. The memory stores a computer program, and when the processor calls the computer program in the memory, it can implement the steps provided in the above embodiments. Of course, the computer device may also include various network interfaces, a power supply, a graphics card, etc., to utilize the graphics card's performance to operate the model, such as for inference and training.

[0105] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to in the method section. It should be noted that those skilled in the art can make various improvements and modifications to this application without departing from the principles of this application, and these improvements and modifications also fall within the protection scope of the claims of this application.

[0106] It should also be noted that, in this specification, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

Claims

1. A method for optimizing the backend pose graph of Cartographer based on geometrically constrained Transformer, characterized in that, include: The initial pose and corresponding point cloud data of each keyframe node in the pose graph to be optimized are obtained, as well as the constraint edges and loop point pairs between keyframe nodes. The initial pose and point cloud data of each keyframe node are encoded into pose feature vectors and point cloud feature vectors, respectively, and then fused to obtain the initial features of the node. A spatial position code is generated based on the translation coordinates in the initial pose and injected into the node initial features of each keyframe node to obtain an input feature sequence; The input feature sequence is input into a cascaded attention network, and attention is updated layer by layer by combining the constraint edges and the loop nodes to obtain node update features. The node update features are decoded into pose increments for each keyframe node, and the pose increments are superimposed with the initial pose to obtain the optimized pose. The optimized pose is used as the final optimized pose output for each keyframe node; The cascaded attention network is a network that has been pre-trained end-to-end using a geometric consistency joint loss function.

2. The Cartographer backend pose graph optimization method based on geometrically constrained Transformer according to claim 1, characterized in that, The process of acquiring the initial pose and corresponding point cloud data of each keyframe node in the pose graph to be optimized, as well as the constraint edges and loop point pairs between keyframe nodes, includes: From the front end of the laser synchronous positioning and mapping system, obtain the key frame nodes generated sequentially during the mapping process; Read the initial pose of each keyframe node, which is composed of translation vectors and rotation matrices, and the corresponding point cloud data; Obtain the constraint edges generated by inter-frame scanning matching of the point cloud data of adjacent key frame nodes; Based on the constraint edges, obtain the loop closure point pairs and corresponding loop closure confidence scores output by the loop closure detection module; The initial pose, point cloud data, constraint edges, loop closure point pairs, and loop closure confidence are summarized to form the pose graph to be optimized.

3. The method for optimizing the Cartographer backend pose graph based on geometrically constrained Transformer according to claim 1, characterized in that, The initial pose and point cloud data of each keyframe node are encoded into pose feature vectors and point cloud feature vectors, respectively, and fused to obtain the initial features of the node, including: The initial pose is decomposed into a translation vector and a rotation matrix, and the rotation matrix is ​​converted into a unit quaternion; The translation vector and the unit quaternion are concatenated to form a seven-dimensional vector, which is then linearly projected to obtain the pose feature vector. Voxel downsampling is performed on the point cloud data of the keyframe node to which the pose feature vector belongs to obtain the downsampled point cloud; Extract the global structural feature vector of the downsampled point cloud, and then obtain the point cloud feature vector through linear projection; The pose feature vector and the point cloud feature vector are concatenated and fused to obtain the initial node features of each keyframe node.

4. The Cartographer backend pose graph optimization method based on geometrically constrained Transformer according to claim 1, characterized in that, The step of generating a spatial position code based on the translation coordinates in the initial pose and injecting it into the initial node features of each keyframe node to obtain an input feature sequence includes: From the initial pose of each keyframe node, the translation coordinates on the three coordinate axes are extracted, and sine and cosine functions of different frequencies are used to encode each coordinate component to obtain three coded components. The three encoded components are concatenated in order of coordinate axes to obtain the spatial position encoding of each keyframe node. The spatial location code is injected into the node initial feature of the corresponding keyframe node in an element-wise additive manner, and the node initial features after being injected with the spatial location code are arranged in the temporal order of the keyframe nodes to obtain the input feature sequence.

5. The Cartographer backend pose graph optimization method based on geometrically constrained Transformer according to claim 2, characterized in that, The cascaded attention network includes a series of cascaded local window attention layers, loop-crossing attention layers, and global structure attention layers. The process of inputting the input feature sequence into the cascaded attention network and updating the attention layer by layer by combining the constraint edges and the loop-crossing nodes to obtain node update features includes: The input feature sequence is input into the local window attention layer to compute self-attention between nodes within a temporal sliding window; A geometric bias term is generated based on the translation distance and rotation angle between nodes, and the weights of the self-attention are adjusted to obtain local update features; The local update features are input into the loop-cross attention layer to perform bidirectional cross-attention updates between the loop point pairs. The update amplitude is then controlled by the loop confidence weighting to obtain the loop update features, which are then input into the global structure attention layer. In the global structure attention layer, the node update features are calculated using the constraint edges as attention masks.

6. The Cartographer backend pose graph optimization method based on geometrically constrained Transformer according to claim 1, characterized in that, The step of decoding the node update features into pose increments for each keyframe node and superimposing the pose increments with the initial pose to obtain the optimized pose includes: The node update features of each keyframe node are input into the translation decoder and the attitude decoder respectively. The translation decoder outputs the translation increment, and the attitude decoder outputs the corresponding quaternion increment. The quaternion increment is normalized to a unit length and then converted into a rotation increment represented by a rotation matrix. The translation increment and the rotation increment are combined to obtain the pose increment of each keyframe node; The pose increment is multiplied by right and added to the corresponding initial pose to obtain the optimized pose of each keyframe node.

7. The Cartographer backend pose graph optimization method based on geometrically constrained Transformer according to claim 2, characterized in that, The cascaded attention network was pre-trained in the following manner: The observed relative pose of the constrained edge is compared with the optimized pose of its two end nodes, and the scan matching residual loss is calculated. The pose continuity loss is calculated based on the second-order difference of the optimized poses of three adjacent frames. The loop overlap loss of the loop closure point pair is calculated by weighting based on the loop closure confidence. The scan matching residual loss, the pose continuity loss and the loop overlap loss are then weighted and superimposed to obtain the geometric consistency joint loss function. Using the geometric consistency joint loss function as the training objective, the cascaded attention network is trained end-to-end to obtain a pre-trained cascaded attention network.

8. A Cartographer backend pose graph optimization device based on geometrically constrained Transformer, characterized in that, include: The acquisition module is used to acquire the initial pose and corresponding point cloud data of each keyframe node in the pose graph to be optimized, as well as the constraint edges and loop point pairs between keyframe nodes. The feature encoding module is used to encode the initial pose and point cloud data of each key frame node into pose feature vectors and point cloud feature vectors respectively, and fuse them to obtain the initial features of the node. The position encoding module is used to generate spatial position codes based on the translation coordinates in the initial pose, and inject them into the node initial features of each keyframe node to obtain an input feature sequence. The attention update module is used to input the input feature sequence into the cascaded attention network, and perform attention update layer by layer by combining the constraint edge and the loop node pair to obtain the node update feature; The pose decoding module is used to decode the node update features into pose increments for each keyframe node, and to superimpose the pose increments with the initial pose to obtain the optimized pose. An optimized output module is used to output the optimized pose as the final optimized pose for each keyframe node. The cascaded attention network is a network that has been pre-trained end-to-end using a geometric consistency joint loss function.

9. A computer device, characterized in that, The method includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the Cartographer backend pose graph optimization method based on geometrically constrained Transformer as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the Cartographer backend pose graph optimization method based on geometrically constrained Transformer as described in any one of claims 1 to 7.

Images