Pose graph optimization method and apparatus, and XR device

Abstract

This invention provides a pose graph optimization method, apparatus, and XR device, relating to the field of image processing technology. The method includes: calculating odometry constraints between adjacent keyframes; calculating the odometry covariance matrix corresponding to the odometry constraints using an uncertainty propagation model; wherein the uncertainty propagation model is derived analytically from the odometry constraint formula; calculating the closure constraints and their corresponding closure covariance matrices between the current frame and closed-loop candidate frames; constructing a pose graph optimization problem based on the odometry constraints, odometry covariance matrix, closure constraints, and closure covariance matrix; constructing a global information matrix, optimizing and solving the global information matrix, and updating the pose of the keyframes. This invention can run in real-time on XR devices with limited computing power, achieving low-latency, high-precision pose estimation, and exhibiting stronger robustness in complex environments.

Description

Technical Field

[0001] This invention relates to the field of image processing technology, and in particular to a pose graph optimization method, apparatus, and XR device. Background Technology

[0002] With the rise of the metaverse concept, XR (Extended Reality) devices have experienced rapid development. XR devices rely on SLAM (Simultaneous Localization and Mapping) technology to perceive their own pose (position and orientation) in the environment, thereby achieving seamless overlay and interaction between the virtual world and the real environment. Pose Graph Optimization (PGO) is a core technology in the SLAM backend. It optimizes the estimated trajectory and map by constructing a graph model consisting of pose nodes and edges connecting these nodes. The key to optimization lies in how to assign reasonable weights to each edge in the pose graph (i.e., the constraints between pose nodes), and these weights are determined by the uncertainty of the edges.

[0003] In existing technologies, the methods for setting the uncertainty of edges often cannot balance real-time performance, accuracy, and computational overhead, which makes them unsuitable for direct application to XR devices where hardware computing power and power consumption are strictly limited.

[0004] Therefore, how to accurately and adaptively model the uncertainty of edges in the pose graph without increasing computational overhead, so as to achieve high-precision and robust real-time pose optimization estimation in XR devices with limited computing power, is an urgent problem to be solved. Summary of the Invention

[0005] This invention provides a pose graph optimization method, apparatus, and XR device to address the problem that existing uncertainty settings often fail to balance real-time performance, accuracy, and computational overhead, making them unsuitable for direct application to XR devices where hardware computing power and power consumption are strictly limited.

[0006] This invention provides a pose graph optimization method, comprising:

[0007] Calculate the odometry constraints between adjacent keyframes;

[0008] The odometer covariance matrix corresponding to the odometer constraint is calculated using an uncertainty propagation model; wherein, the uncertainty propagation model is derived from the odometer constraint formula using analytical methods.

[0009] Calculate the closure constraints between the current frame and the candidate closed-loop frames and their corresponding closure covariance matrix;

[0010] Based on the odometry constraints, the odometry covariance matrix, the loop closure constraints, and the loop closure covariance matrix, a pose graph optimization problem is constructed.

[0011] Construct a global information matrix for the pose graph optimization problem, optimize and solve the global information matrix, and update the pose of the keyframes.

[0012] According to the pose graph optimization method provided by the present invention, the uncertainty propagation model is obtained by simplifying the odometry constraint formula using Lie algebra properties, adjoint matrix properties, and linearizing it using the Beck-Campbell-Hausdorff (BCH) formula. The step of calculating the odometry covariance matrix corresponding to the odometry constraint using the uncertainty propagation model includes:

[0013] Obtain the first and second nominal pose transformation matrices of adjacent keyframes;

[0014] Obtain the first covariance matrix of the first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtain the second covariance matrix of the second Lie algebra error corresponding to the second nominal pose transformation matrix; wherein, the first Lie algebra error and the second Lie algebra error are both modeled as zero-mean Gaussian distributions;

[0015] The odometer covariance matrix corresponding to the odometer constraint is calculated based on the first covariance matrix, the second covariance matrix, and the adjoint matrix of the inverse of the first nominal pose transformation matrix.

[0016] According to a pose graph optimization method provided by the present invention, the step of calculating the odometer covariance matrix corresponding to the odometer constraint based on the first covariance matrix, the second covariance matrix, and the adjoint matrix of the inverse of the first nominal pose transformation matrix includes:

[0017] Obtain the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error;

[0018] The odometer covariance matrix corresponding to the odometer constraint is calculated based on the first covariance matrix, the second covariance matrix, the adjoint matrix of the inverse of the first nominal pose transformation matrix, and the cross covariance matrix.

[0019] According to a pose graph optimization method provided by the present invention, the calculation of odometry constraints between adjacent keyframes includes:

[0020] Obtain the first pose transformation matrix and the second pose transformation matrix of adjacent keyframes;

[0021] Calculate the inverse of the first pose transformation matrix;

[0022] Multiplying the inverse matrix by the second pose transformation matrix yields the odometry constraints between adjacent keyframes.

[0023] According to a pose graph optimization method provided by the present invention, the step of constructing a pose graph optimization problem based on the odometry constraints, the odometry covariance matrix, the loop closure constraints, and the loop closure covariance matrix includes:

[0024] Define the pose of the keyframe as a state variable;

[0025] Based on the state variables, the odometer constraints, the odometer covariance matrix, the laparosional constraints, and the laparosional covariance matrix, a nonlinear least squares objective function is constructed with the goal of minimizing the weighted sum of squared residuals.

[0026] According to a pose graph optimization method provided by the present invention, constructing the global information matrix of the pose graph optimization problem includes:

[0027] The nonlinear least squares objective function is linearized to calculate the first Jacobian matrix corresponding to the odometer constraint, the second Jacobian matrix corresponding to the loop closure constraint, and the global error vector.

[0028] According to the pre-established address mapping table of the global information matrix, the first matrix block calculated by the first Jacobian matrix and the first local information matrix is ​​directly accumulated into the first address corresponding to the global information matrix; wherein, the first local information matrix is ​​obtained by inverting the odometer covariance matrix;

[0029] According to the pre-established address mapping table of the global information matrix, the second matrix block calculated by the second Jacobian matrix and the second local information matrix is ​​directly accumulated into the second address corresponding to the global information matrix; wherein, the second local information matrix is ​​obtained by inverting the loop covariance matrix.

[0030] According to a pose graph optimization method provided by the present invention, the step of optimizing and solving the global information matrix and updating the pose of keyframes includes:

[0031] Based on the global information matrix and the global error vector, construct a linear equation;

[0032] The global information matrix is ​​decomposed by the square root of the matrix to solve the linear equation and obtain the optimal increment of the keyframe pose.

[0033] The pose of the keyframe is updated based on the optimal increment.

[0034] According to a pose graph optimization method provided by the present invention, the step of calculating the loop closure constraints and their corresponding loop closure covariance matrix between the current frame and the loop closure candidate frame includes:

[0035] Acquire the first sensor data of the current frame and the second sensor data of the closed-loop candidate frame;

[0036] By matching the first sensor data and the second sensor data, the loop closure constraint between the current frame and the closed-loop candidate frame is calculated, and the loop closure covariance matrix is ​​determined by evaluating the matching quality.

[0037] Wherein, if the first sensor data and the second sensor data are Global Mobile Satellite Service (GNSS) data, the loop closure constraint is calculated based on the relative transformation between GNSS poses, and the loop closure covariance matrix is ​​determined based on the GNSS measurement variance;

[0038] If the first sensor data and the second sensor data are point cloud data, then the loop closure constraint is the optimal transformation matrix calculated by the point cloud registration algorithm based on the point cloud data, and the loop closure covariance matrix is ​​determined based on the point cloud matching score of the point cloud registration algorithm.

[0039] If the first sensor data and the second sensor data are image data, then the loop closure constraint is the camera relative pose calculated based on the two-dimensional feature points matched with the image data and their corresponding three-dimensional landmark points, and the loop closure covariance matrix is ​​determined based on the image reprojection error.

[0040] The present invention also provides a pose graph optimization device, comprising:

[0041] The first calculation module is used to calculate the odometry constraints between adjacent keyframes;

[0042] The second calculation module is used to calculate the odometer covariance matrix corresponding to the odometer constraint using an uncertainty propagation model; wherein the uncertainty propagation model is derived from the odometer constraint formula using an analytical method.

[0043] The third calculation module is used to calculate the closure constraints between the current frame and the closure candidate frame and their corresponding closure covariance matrix.

[0044] The problem construction module is used to construct a pose graph optimization problem based on the odometry constraints, the odometry covariance matrix, the loop closure constraints, and the loop closure covariance matrix.

[0045] The pose update module is used to construct a global information matrix for the pose graph optimization problem, optimize and solve the global information matrix, and update the pose of the keyframes.

[0046] The present invention also provides an XR 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 pose graph optimization method as described above.

[0047] The pose graph optimization method, apparatus, and XR device provided by this invention calculate odometry constraints between adjacent keyframes and, through an uncertainty propagation model, calculate the odometry covariance matrix corresponding to the odometry constraints. The uncertainty propagation model is derived analytically from the odometry constraint formula. This analytically derived uncertainty propagation model is used to calculate the odometry covariance matrix corresponding to the odometry constraints. Compared to existing empirical settings and numerical approximation methods, this invention achieves adaptive dynamic adjustment and online accurate estimation of odometry constraint weights, resulting in higher precision. Simultaneously, the analytical method significantly reduces computational overhead, enabling real-time operation on XR devices with limited computing power, achieving low-latency, high-precision pose estimation and enhancing the immersive experience. Furthermore, the method calculates the closure constraints and their corresponding closure covariance matrices between the current frame and closed-loop candidate frames. Then, based on the odometry constraints, odometry covariance matrix, closure constraints, and closure covariance matrix, a pose graph optimization problem is constructed, a global information matrix for the pose graph optimization problem is built, the global information matrix is ​​optimized, and the pose of the keyframes is updated. Since both the odometer constraint and the loop closure constraint use an adaptive covariance matrix, the weight of undesirable constraints will be naturally reduced during global optimization, thus exhibiting stronger generalization ability and robustness when facing complex environments. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0049] Figure 1 This is a system architecture diagram of the pose graph optimization system provided by the present invention;

[0050] Figure 2 This is one of the flowcharts illustrating the pose graph optimization method provided by the present invention;

[0051] Figure 3 This is the second flowchart of the pose graph optimization method provided by the present invention;

[0052] Figure 4 This is the third flowchart illustrating the pose graph optimization method provided by the present invention;

[0053] Figure 5 This is a schematic diagram of the pose graph optimization device provided by the present invention;

[0054] Figure 6 This is a schematic diagram of the structure of the XR device provided by the present invention. Detailed Implementation

[0055] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this 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 this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0056] With the rise of the metaverse concept, XR (Extended Reality) devices have experienced rapid development. XR devices rely on SLAM (Simultaneous Localization and Mapping) technology to perceive their own pose (position and orientation) in the environment, thereby achieving seamless overlay and interaction between the virtual world and the real environment. Pose Graph Optimization (PGO) is a core technology in the SLAM backend. It optimizes the estimated trajectory and map by constructing a graph model consisting of pose nodes and edges connecting these nodes. The key to optimization lies in how to assign reasonable weights to each edge in the pose graph (i.e., the constraints between pose nodes), and these weights are determined by the uncertainty of the edges.

[0057] In the prior art, the uncertainty of the edge is usually set in the following ways: (1) empirical setting, that is, directly setting a fixed confidence level based on experience for all similar constraints (e.g., odometry edge); (2) numerical approximation, that is, estimating the uncertainty through Monte Carlo or numerical integration; (3) heuristic method, that is, setting according to the task experience rules. The above (1) method lacks modeling of the real noise propagation of the sensor, resulting in rigid and distorted weight allocation, which ultimately leads to poor accuracy of the pose graph optimization result; the above (2) method has high computational cost and the approximation error fluctuates with sampling, which is not feasible for XR devices that operate under extremely low latency and have strict power consumption budget; the above (3) method may be effective in specific scenarios, but it is often prone to failure when the user performs complex, fast or irregular movements, that is, it is difficult to maintain robustness in complex environments.

[0058] In summary, existing technologies often fail to balance real-time performance, accuracy, and computational overhead in uncertainty modeling, making them unsuitable for direct application to XR devices where hardware computing power and power consumption are strictly limited.

[0059] Therefore, how to accurately and adaptively model the uncertainty of edges in the pose graph without increasing computational overhead, so as to achieve high-precision and robust real-time pose optimization estimation in XR devices with limited computing power, is an urgent problem to be solved.

[0060] To address the aforementioned problems, this invention proposes a pose graph optimization method, apparatus, and XR device, which are described below in conjunction with... Figures 1-6 Describe it.

[0061] Figure 1 This is a system architecture diagram of the pose graph optimization system provided by the present invention, such as... Figure 1 As shown, the pose graph optimization system includes an XR device 01 and a server 02.

[0062] XR device 01 refers to wearable or portable devices that integrate virtual and real environments through hardware and software technologies to achieve human-computer interaction. These can include, but are not limited to, VR (Virtual Reality) devices, AR (Augmented Reality) devices, and MR (Mixed Reality) devices. VR devices use computer technology to simulate and generate a three-dimensional virtual space, allowing users to immerse themselves and interact with it, gaining a truly immersive experience. AR devices use technology to merge virtual information with the real world, overlaying it onto real scenes in real time to enhance sensory experience. MR devices mix the real and virtual worlds to create a new visual environment that simultaneously contains physical entities and virtual information, allowing users to interact with these physical entities and virtual information in real time. Specifically, XR devices can be smart glasses or head-mounted displays.

[0063] Server 02 can be a remote server that provides various services, such as a remote server that analyzes and processes data collected by the sensors of XR devices (e.g., image sensors, inertial measurement units, and wheel odometers), or it can be a local computing device of the XR device configured on the target carrier.

[0064] Figure 2 This is one of the flowcharts illustrating the pose graph optimization method provided by the present invention, such as... Figure 2 As shown, the pose graph optimization method includes steps S110, S120, S130, S140 and S150.

[0065] Step S110: Calculate the odometry constraints between adjacent keyframes.

[0066] In this embodiment, the pose graph optimization method is applied to an XR device. It should be understood that the pose graph optimization method can also be applied to mobile terminals with virtual reality or augmented reality functions, such as mobile robots, autonomous vehicles, and drones.

[0067] As the XR device moves through its environment, its built-in sensors collect relevant data in real time, and the system selects specific points in time as keyframes. These sensors include, but are not limited to, image sensors (such as cameras), IMUs (Inertial Measurement Units), wheeled odometry, LiDAR, and RTK (Real-time Kinematic) systems. The selection criteria for keyframes can be based on time intervals, distance traveled, or the degree of scene change, and are not specifically limited here.

[0068] Odometry constraints between adjacent keyframes are odometry edges between adjacent keyframes, and also refer to the relative pose transformation between adjacent keyframes.

[0069] Specifically, the first and second pose transformation matrices of adjacent keyframes are first obtained. Then, the inverse of the first pose transformation matrix is ​​calculated, and the inverse matrix is ​​multiplied by the second pose transformation matrix to obtain the odometry constraints between adjacent keyframes. The specific execution process can be found in the following embodiment, which will not be elaborated here.

[0070] The first pose transformation matrix and the second pose transformation matrix are output through the front-end odometry. Specifically, the pose transformation matrix of the key frame is obtained by processing data from one or more sensors (such as cameras and inertial measurement units).

[0071] Step S120: Calculate the odometer covariance matrix corresponding to the odometer constraint using the uncertainty propagation model; wherein the uncertainty propagation model is derived from the odometer constraint formula using an analytical method.

[0072] The odometer covariance matrix is ​​the covariance matrix corresponding to the odometer constraint. To distinguish it from other covariance matrices, it is denoted as the odometer covariance matrix and is used to characterize the uncertainty between odometer edges corresponding to adjacent keyframes.

[0073] The odometer covariance matrix corresponding to the odometer constraint is calculated using an uncertainty propagation model.

[0074] The uncertainty propagation model is derived analytically from the odometer constraint formula. Specifically, the uncertainty propagation model is obtained by simplifying the odometer constraint formula using Lie algebra properties, adjoint matrix properties, and BCH (Baker-Campbell-Hausdorff) linearization. The detailed derivation process is as follows:

[0075] The pose is represented by a Lie group SE(3), and the error term is represented by a Lie algebra se(3). The pose error model is as follows: Where T represents the true pose. Indicates the error term. For Lie algebra error, Let represent the nominal pose, and be the nominal pose transformation matrix. The Lie algebra error is modeled as a zero-mean Gaussian distribution, i.e. ,in, For position vectors, Let SO(3) be the Lie algebra.

[0076] For two adjacent keyframes, denoted as the first keyframe j and the second keyframe k, obtain the first pose transformation matrix and the second pose transformation matrix of the adjacent keyframes. Assuming that both the first keyframe j and the second keyframe k are in the i-coordinate system, the pose transformation matrix corresponding to the first keyframe j is denoted as the first pose transformation matrix. Let the pose transformation matrix corresponding to the second keyframe k be denoted as the second pose transformation matrix. .

[0077] Multiplying the inverse of the first pose transformation matrix by the second pose transformation matrix yields the odometry constraints between adjacent keyframes. The specific formula is as follows:

[0078] .

[0079] After considering the pose error, that is, substituting the pose error model into the above odometry constraint formula, we can obtain:

[0080] ;

[0081] Furthermore, by simplifying using the properties of Lie algebras, we can obtain:

[0082] ;

[0083] Furthermore, using the properties of the adjoint matrix, we can obtain:

[0084] ;

[0085] Furthermore, for small errors, a first-order linearization approximation is performed using the BCH formula, and the result is:

[0086] .

[0087] Based on the aforementioned uncertainty propagation model, in one embodiment, the calculation process of the odometer covariance matrix is ​​as follows: Obtain the first and second nominal pose transformation matrices of adjacent keyframes; obtain the first covariance matrix of the first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtain the second covariance matrix of the second Lie algebra error corresponding to the second nominal pose transformation matrix; wherein, both the first and second Lie algebra errors are modeled as zero-mean Gaussian distributions; obtain the cross-covariance matrix of the first and second Lie algebra errors; calculate the odometer covariance matrix corresponding to the odometer constraint based on the first covariance matrix, the second covariance matrix, the adjoint matrix of the inverse of the first nominal pose transformation matrix, and the cross-covariance matrix. The specific execution process can be found in the following embodiments, which will not be elaborated here.

[0088] In another embodiment, the calculation process of the odometer covariance matrix is ​​as follows: Obtain the first and second nominal pose transformation matrices of adjacent keyframes; obtain the first covariance matrix of the first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtain the second covariance matrix of the second Lie algebra error corresponding to the second nominal pose transformation matrix; wherein, both the first and second Lie algebra errors are modeled as zero-mean Gaussian distributions; when the first and second Lie algebra errors are uncorrelated, i.e., when the cross-covariance matrices of the first and second Lie algebra errors are both zero matrices, calculate the odometer covariance matrix corresponding to the odometer constraint based on the first covariance matrix, the second covariance matrix, and the adjoint matrix of the inverse of the first nominal pose transformation matrix. The specific execution process can be referred to in the following embodiments, and will not be elaborated here.

[0089] It should be noted that the triggering of this embodiment of the invention can be determined based on the execution result of the loop closure detection module. Specifically, the loop closure detection module in the system background continuously matches the sensor information of the current frame with the historical keyframe information stored in the map database to perform loop closure detection. When a loop closure is detected, the pose graph optimization process of this embodiment of the invention is triggered.

[0090] It should be understood that odometer constraints and their corresponding odometer covariance matrices can be calculated synchronously and stored in the map database when new keyframes are generated. When the loop closure detection module detects a loop closure, it directly retrieves the stored odometer constraints and their corresponding odometer covariance matrices from the map database. This method avoids redundant calculations and improves the overall operating efficiency of the system.

[0091] Step S130: Calculate the closure constraints between the current frame and the closed-loop candidate frames and their corresponding closure covariance matrix.

[0092] Upon detecting a loop closure, the corresponding loop closure candidate frame for the current frame is obtained, and the loop closure constraint and its corresponding covariance matrix between the current frame and the loop closure candidate frame are calculated. The loop closure constraint refers to the loop closure edge between the current frame and the loop closure candidate frame, and also to the relative pose transformation between them. For ease of distinction, the covariance matrix of the loop closure constraint is denoted as the loop closure covariance matrix, which characterizes the uncertainty of the loop closure edge between the current frame and the loop closure candidate frame.

[0093] Specifically, the process first acquires the first sensor data of the current frame and the second sensor data of the candidate closed-loop frame. Then, by matching the first and second sensor data, the loop closure constraint between the current frame and the candidate closed-loop frame is calculated, and the loop closure covariance matrix is ​​determined by evaluating the matching quality. The types of the first and second sensor data include, but are not limited to, GMSS (Global Mobile Satellite Service) data, point cloud data, and image data. GMSS data can be acquired through a GNSS module, point cloud data can be acquired through a ToF (Time of Flight) sensor or LiDAR, and image data can be acquired through an image sensor. The specific execution process can be found in the following embodiment, which will not be elaborated here.

[0094] Step S140: Construct a pose graph optimization problem based on the odometer constraint, the odometer covariance matrix, the loop closure constraint, and the loop closure covariance matrix.

[0095] The poses of all keyframes are defined as state variables to be optimized, and all odometry constraints and loop closure constraints are used as edges to construct a pose graph. The optimization goal is to adjust the poses of all keyframes to minimize the weighted sum of squared errors of all constraints.

[0096] Specifically, based on the state variables, odometer constraints, odometer covariance matrix, laparoscopy constraints, and laparoscopy covariance matrix, a nonlinear least squares objective function is constructed to minimize the weighted sum of squared residuals. The specific execution process can be found in the following embodiment, and will not be elaborated upon here.

[0097] Step S150: Construct a global information matrix for the pose graph optimization problem, optimize and solve the global information matrix, and update the pose of the keyframes.

[0098] After constructing the pose graph optimization problem, a sparse data structure is used to build a global information matrix for the pose graph optimization problem.

[0099] The global information matrix, also known as the Hessian matrix, is a huge sparse matrix.

[0100] In one embodiment, the process of constructing the global information matrix is ​​as follows: the nonlinear least squares objective function is linearized to calculate the first Jacobian matrix corresponding to the odometer constraint, the second Jacobian matrix corresponding to the loop closure constraint, and the global error vector; then, according to a pre-established address mapping table of the global information matrix, the first matrix block calculated from the first Jacobian matrix and the first local information matrix is ​​directly accumulated into the first address corresponding to the global information matrix; wherein, the first local information matrix is ​​obtained by inverting the odometer covariance matrix; simultaneously, according to a pre-established address mapping table of the global information matrix, the second matrix block calculated from the second Jacobian matrix and the second local information matrix is ​​directly accumulated into the second address corresponding to the global information matrix; wherein, the second local information matrix is ​​obtained by inverting the loop closure covariance matrix.

[0101] In another embodiment, the process of constructing the global information matrix is ​​as follows: the nonlinear least squares objective function is linearized to calculate the first Jacobian matrix corresponding to the odometer constraint, the second Jacobian matrix corresponding to the loop closure constraint, and the global error vector; the odometer covariance matrix is ​​inverted to obtain the first local information matrix, and the loop closure covariance matrix is ​​inverted to obtain the second local information matrix; based on the first Jacobian matrix and the first local information matrix, the matrix block of the odometer constraint is calculated, and based on the second Jacobian matrix and the second local information matrix, the matrix block of the loop closure constraint is calculated; the global information matrix is ​​constructed based on the matrix blocks of the odometer constraint and the matrix blocks of the loop closure constraint.

[0102] The specific execution process can be found in the following examples, which will not be elaborated here.

[0103] It should be noted that the first implementation method described above, by combining the address mapping table for direct memory address accumulation, avoids invalid operations and repeated memory allocations on large matrices during the optimization iteration process compared to the second implementation method. This can greatly reduce memory management overhead and improve the construction speed of the global information matrix.

[0104] After constructing the global information matrix for the pose graph optimization problem, the global information matrix is ​​optimized and solved, and the pose of the keyframes is updated.

[0105] Specifically, a linear equation is constructed based on the global information matrix and the global error vector; the global information matrix is ​​then decomposed using the square root of the matrix to solve the linear equation, yielding the optimal increment for the keyframe pose; and the keyframe pose is updated based on this optimal increment. The specific execution process can be found in the following embodiment, which will not be elaborated upon here. By performing the square root decomposition on the global information matrix, compared to directly inverting the global information matrix, the solution speed and numerical stability can be improved while maintaining the accuracy of the solution.

[0106] The pose graph optimization method provided in this invention calculates the odometry constraints between adjacent keyframes and uses an uncertainty propagation model to calculate the odometry covariance matrix corresponding to the odometry constraints. The uncertainty propagation model is derived analytically from the odometry constraint formula. This analytically derived uncertainty propagation model is used to calculate the odometry covariance matrix corresponding to the odometry constraints. Compared to existing empirical settings and numerical approximations, this invention achieves adaptive dynamic adjustment and online accurate estimation of odometry constraint weights, resulting in higher precision. Simultaneously, the analytical method significantly reduces computational overhead, enabling real-time operation on XR devices with limited computing power, achieving low-latency, high-precision pose estimation and enhancing the immersive experience. Furthermore, the method calculates the closure constraints and their corresponding closure covariance matrices between the current frame and closed-loop candidate frames. Then, based on the odometry constraints, odometry covariance matrix, closure constraints, and closure covariance matrix, a pose graph optimization problem is constructed. A global information matrix for the pose graph optimization problem is built, and the global information matrix is ​​optimized and solved to update the pose of the keyframes. Since both the odometer constraint and the loop closure constraint use an adaptive covariance matrix, the weight of undesirable constraints will be naturally reduced during global optimization, thus exhibiting stronger generalization ability and robustness when facing complex environments.

[0107] Based on any of the above embodiments Figure 3 This is the second flowchart illustrating the pose graph optimization method provided by this invention, as shown below. Figure 3 As shown, step S120 includes: step S121, step S122 and step S123.

[0108] Step S121: Obtain the first nominal pose transformation matrix and the second nominal pose transformation matrix of adjacent keyframes.

[0109] For two adjacent keyframes, denoted as first keyframe j and second keyframe k respectively, obtain the first nominal pose transformation matrix and the second nominal pose transformation matrix of the adjacent keyframes. Assuming that both first keyframe j and second keyframe k are in the i-coordinate system, the first nominal pose transformation matrix corresponding to first keyframe j is denoted as... Let the second nominal pose transformation matrix corresponding to the second keyframe k be denoted as .

[0110] Step S122: Obtain the first covariance matrix of the first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtain the second covariance matrix of the second Lie algebra error corresponding to the second nominal pose transformation matrix; wherein, the first Lie algebra error and the second Lie algebra error are both modeled as zero-mean Gaussian distributions.

[0111] Obtain the first nominal pose transformation matrix The corresponding first Lie algebra error (denoted as ) The first covariance matrix of is denoted as And obtain the second nominal pose transformation matrix. The corresponding second Lie algebra error (denoted as ) The second covariance matrix of is denoted as .

[0112] The first and second Lie algebra errors are both modeled as zero-mean Gaussian distributions, i.e. .

[0113] Step S123: Calculate the odometer covariance matrix corresponding to the odometer constraint based on the first covariance matrix, the second covariance matrix, and the adjoint matrix of the inverse of the first nominal pose transformation matrix.

[0114] In one embodiment, the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error is first obtained, including... and Then, based on the first covariance matrix Second covariance matrix The odometry covariance matrix corresponding to the odometry constraint is calculated using the adjoint matrix of the inverse of the first nominal pose transformation matrix and the cross covariance matrix. Specifically, the odometry covariance matrix corresponding to the odometry constraint is calculated by substituting the first covariance matrix, the second covariance matrix, the adjoint matrix of the inverse of the first nominal pose transformation matrix, and the cross covariance matrix into a preset uncertainty propagation formula. The preset uncertainty propagation formula is as follows:

[0115] .

[0116] in, This represents the odometer covariance matrix corresponding to the odometer constraints between the first keyframe j and the second keyframe k. Let represent the adjoint matrix of the inverse of the first nominal pose transformation matrix. express The transpose of .

[0117] In another implementation, when the first Lie algebra error Error with the second Lie algebra When uncorrelated, the corresponding cross-covariance matrix of the first Lie algebra error and the second Lie algebra error is... and Since both are zero matrices, the above-mentioned pre-defined uncertainty propagation formula can be simplified to:

[0118] .

[0119] At this point, we can directly use the first covariance matrix. Second covariance matrix The odometry covariance matrix corresponding to the odometry constraint is calculated by substituting the first covariance matrix, the second covariance matrix, and the adjoint matrix of the inverse of the first nominal pose transformation matrix into the simplified preset uncertainty propagation formula.

[0120] It should be noted that the first implementation method described above fully considers all error sources and their correlations, providing an accurate method for estimating relative pose uncertainty. Compared to the second implementation method, it more accurately reflects the true estimation uncertainty, providing reliable constraint weights for subsequent optimization processes and significantly improving the overall accuracy of the SLAM system. The second implementation method, on the other hand, can reduce computational complexity by approximately 50% while maintaining sufficient accuracy. This simplification is particularly suitable for embedded systems with limited computing resources, or for scenarios where pose estimation truly satisfies the independence assumption, achieving a balance between accuracy and efficiency.

[0121] It should also be noted that the above-mentioned pre-defined uncertainty propagation formula is obtained by simplifying the odometry using the properties of Lie algebra, the properties of the adjoint matrix, and the BCH formula. The specific derivation process can be referred to the above-mentioned embodiment, and will not be repeated here.

[0122] The pose graph optimization method provided in this invention establishes a probabilistic uncertainty assessment framework based on Lie group theory by modeling the Lie algebra error as a zero-mean Gaussian distribution and using the adjoint matrix to propagate the covariance. This upgrades the traditional deterministic odometry constraint to a probabilistic constraint, providing accurate weight information for backend pose graph optimization and improving the estimation accuracy and theoretical rigor of the SLAM system.

[0123] Based on any of the above embodiments, step S123 includes: step S1231 and step S1232.

[0124] Step S1231: Obtain the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error;

[0125] Step S1232: Calculate the odometer covariance matrix corresponding to the odometer constraint based on the first covariance matrix, the second covariance matrix, the adjoint matrix of the inverse of the first nominal pose transformation matrix, and the cross covariance matrix.

[0126] First, obtain the error of the first Lie algebra. Error with the second Lie algebra The cross covariance matrix, including and Both have transpose symmetry, that is, .

[0127] Then, the first covariance matrix Second covariance matrix Substituting the adjoint matrix and cross covariance matrix of the inverse of the first nominal pose transformation matrix into the preset uncertainty propagation formula, the odometer covariance matrix corresponding to the odometer constraint is calculated. The preset uncertainty propagation formula is as follows:

[0128]

[0129] in, This represents the odometer covariance matrix corresponding to the odometer constraints between the first keyframe j and the second keyframe k. Let represent the adjoint matrix of the inverse of the first nominal pose transformation matrix. express The transpose of the transpose. The pose graph optimization method provided in this embodiment of the invention further introduces a cross covariance matrix to evaluate the correlation between pose errors, which solves the problem of uncertainty estimation bias caused by ignoring error correlation, making the uncertainty assessment of odometry constraints more complete and accurate, and significantly enhancing the robustness and reliability of the SLAM system in complex scenarios.

[0130] Based on any of the above embodiments, step S110 includes: step S111, step S112 and step S113.

[0131] Step S111: Obtain the first pose transformation matrix and the second pose transformation matrix of adjacent keyframes.

[0132] For two adjacent keyframes, denoted as the first keyframe j and the second keyframe k, obtain the first pose transformation matrix and the second pose transformation matrix of the adjacent keyframes. Assuming that both the first keyframe j and the second keyframe k are in the i-coordinate system, the pose transformation matrix corresponding to the first keyframe j is denoted as the first pose transformation matrix. Let the pose transformation matrix corresponding to the second keyframe k be denoted as the second pose transformation matrix. .

[0133] Step S112: Calculate the inverse matrix of the first pose transformation matrix.

[0134] Then, calculate the inverse matrix of the first pose transformation matrix, which is... .

[0135] Step S113: Multiply the inverse matrix with the second pose transformation matrix to obtain the odometry constraint between adjacent keyframes.

[0136] Multiplying the inverse matrix by the second pose transformation matrix yields the odometry constraints between adjacent keyframes. Specifically:

[0137] ;

[0138] in, This represents the relative pose (i.e., odometry constraint) between the first keyframe j and the second keyframe k. Let represent the first pose transformation matrix of the first keyframe j in coordinate system i. express The inverse matrix, This represents the second pose transformation matrix in the second keyframe k in coordinate system i.

[0139] The pose graph optimization method provided in this invention multiplies the inverse of the first pose transformation matrix with the second pose transformation matrix to obtain the odometry constraints between adjacent keyframes. The relative pose relationship between adjacent keyframes is represented by the inverse matrix and multiplication operation, which accurately constructs the relative constraints of the odometry edges (i.e., odometry constraints). This method also conforms to the operational rules of Lie algebra SE(3), facilitating subsequent error modeling using Lie algebras and thus accurately constructing the uncertainty of the odometry constraints. Based on the accurately constructed odometry constraints and their uncertainties, the accuracy and robustness of pose graph optimization can be improved.

[0140] Based on any of the above embodiments, step S140 includes: step S141 and step S142.

[0141] Step S141: Define the pose of the keyframe as a state variable.

[0142] Define the poses of all keyframes as state variables to be optimized. For example, assuming there are n keyframes, the set of state variables X can be obtained as follows:

[0143] X={T i1 T i2 , ..., T ij T ik , ..., T in};

[0144] Among them, T i1 T i2 T ij T ik T in These represent the pose transformation matrices for the 1st, 2nd, jth, kth, and nth keyframes in coordinate system i, respectively.

[0145] Step S142: Based on the state variables, the odometer constraints, the odometer covariance matrix, the loop closure constraints, and the loop closure covariance matrix, construct a nonlinear least squares objective function with the goal of minimizing the weighted sum of squared residuals.

[0146] The optimization objective of the pose graph optimization problem is to minimize the weighted sum of squared residuals across all constraints. A nonlinear least-squares objective function can be constructed based on the state variables, odometry constraints, odometry covariance matrix, lap closure constraints, and lap closure covariance matrix. as follows:

[0147] ;

[0148] Where argmin() represents the position function for finding the minimum value; The summation symbol is represented by R; R represents the set of odometer constraints, where the element (j,k) represents adjacent keyframe pairs with odometer constraints; L represents the set of loop closure constraints, where the element (m,n) represents non-contiguous keyframe pairs with constraints established through loop closure detection. The residual vector representing the odometer constraint is determined based on the odometer constraint. The information matrix representing the odometer is obtained by inverting the odometer covariance matrix. It is the square of the Mahalanobis distance, representing the weighted error of the mileage constraint; The residual vector representing the loop closure constraint is determined based on the loop closure constraint. The information matrix representing the loop is obtained by inverting the loop covariance matrix. It is the square of the Mahalanobis distance, representing the weighted error of the mileage constraint.

[0149] In the aforementioned nonlinear least squares objective function, by introducing information matrices of different constraints as weights, the optimization process can intelligently distinguish measurements of different quality. Among them, high-precision constraints will play a dominant role in the optimization, while low-precision constraints will have a smaller effect. In this way, it can be ensured that constraints with low uncertainty occupy a larger weight in the optimization, thereby solving in a more reliable direction during the optimization process, which can improve the accuracy and precision of the final pose estimation.

[0150] The pose graph optimization method provided in this invention integrates odometry constraints and closure constraints into a single objective function, thereby transforming a physical pose graph optimization problem into a mathematically well-defined nonlinear least squares problem, which facilitates subsequent automated solution.

[0151] Based on any of the above embodiments Figure 4 This is the third flowchart of the pose graph optimization method provided by the present invention, as shown below. Figure 4 As shown, the step "Constructing the global information matrix for the pose graph optimization problem" includes:

[0152] Step S151: Linearize the nonlinear least squares objective function to calculate the first Jacobian matrix corresponding to the odometer constraint, the second Jacobian matrix corresponding to the loop closure constraint, and the global error vector.

[0153] Step S152: According to the address mapping table of the pre-established global information matrix, the first matrix block calculated by the first Jacobian matrix and the first local information matrix is ​​directly accumulated into the first address corresponding to the global information matrix; wherein, the first local information matrix is ​​obtained by inverting the odometer covariance matrix.

[0154] The global information matrix, also known as the Hessian matrix, is a huge sparse matrix with dimensions (nd)×(nd), where n is the number of keyframes and d is the degree of freedom for each pose, typically 6.

[0155] Before optimization begins, the structure of the pose graph is analyzed in advance, and the memory address of each non-zero matrix block (each block is a 6×6 matrix) in the global information matrix H is calculated and stored. The address mapping table is a pre-stored lookup table that determines the specific memory address in the global information matrix H where the Jacobian matrix calculation result for each constraint should be accumulated.

[0156] Inverting the odometer covariance matrix yields the odometer information matrix, denoted as the first local information matrix. .

[0157] According to the first Jacobian matrix J jk and the first local information matrix ,calculate The odometry constraint matrix block is obtained and denoted as the first matrix block. This first matrix block is 12×12 in size and can be divided into four 6×6 sub-blocks. According to the pre-established address mapping table, these sub-blocks are directly accumulated to the first address of the global information matrix H, where the first address is the pre-calculated 6×6 memory location corresponding to keyframes j and k.

[0158] Using the above method, filling and accumulation operations can be performed directly on the pre-allocated sparse matrix structure, avoiding the need to dynamically allocate and release memory for a large number of small matrix blocks each time.

[0159] Step S153: According to the pre-established address mapping table of the global information matrix, the second matrix block calculated by the second Jacobian matrix and the second local information matrix is ​​directly accumulated into the second address corresponding to the global information matrix; wherein, the second local information matrix is ​​obtained by inverting the loop covariance matrix.

[0160] Similarly, by inverting the cyclic covariance matrix, we obtain the cyclic information matrix, denoted as the second local information matrix. .

[0161] According to the second Jacobian matrix J mn Second local information matrix ,calculate The odometer-constrained matrix block is obtained and denoted as the second matrix block. This second matrix block is 12×12 in size and can be divided into four 6×6 sub-blocks. According to the pre-established address mapping table, these sub-blocks are directly accumulated to the second address of the global information matrix H, where the second address is the pre-calculated 6×6 memory location corresponding to keyframes m and n.

[0162] The pose graph optimization method provided in this invention uses a sparse data structure to store the global information matrix, which can significantly reduce memory usage and thus enable it to run on XR devices with limited memory resources. Furthermore, by leveraging the sparsity of the global information matrix and combining it with an address mapping table for direct memory address accumulation, invalid operations on large matrices and repeated memory allocations during optimization iterations can be avoided, greatly reducing memory management overhead and improving the construction speed of the global information matrix. This is crucial for achieving real-time pose graph optimization on XR devices.

[0163] Based on any of the above embodiments, the step "optimize and solve the global information matrix and update the pose of the keyframe" includes: steps S154, S155 and S156.

[0164] Step S154: Construct a linear equation based on the global information matrix and the global error vector.

[0165] After constructing the global information matrix H and obtaining the global error vector b, a linear equation is constructed based on the global information matrix H and the global error vector b. The details are as follows:

[0166] H·ΔX=-b;

[0167] Where ΔX represents the pose increment.

[0168] Step S155: Perform matrix square root decomposition on the global information matrix to solve the linear equation and obtain the optimal increment of the keyframe pose.

[0169] Considering that directly inverting a large global information matrix is ​​computationally expensive, and given the favorable properties of the global information matrix—sparseness, symmetry, and positive definiteness—this embodiment of the invention employs a more efficient matrix square root decomposition method, such as Cholesky decomposition. By using Cholesky decomposition and other matrix square root methods, the symmetric and positive definite properties of the information matrix are fully utilized. Compared to direct inversion, this method improves solution speed and numerical stability while maintaining accuracy.

[0170] Specifically, the global information matrix H is decomposed using the square root of the matrix, resulting in a lower triangular matrix L and its transpose. The product is as follows:

[0171] .

[0172] Then, the optimal increment ΔX is obtained through two efficient triangular matrix solutions. The optimal increment ΔX is a vector, ΔX={Δ Δ , ..., Δ }, where i represents the i-th coordinate system, 1, 2, ..., n represent the 1st, 2nd, ..., nth keyframes, and each Δ The Lie algebra error increment corresponding to a keyframe pose.

[0173] Step S156: Update the pose of the keyframe according to the optimal increment.

[0174] Based on the optimal increment mentioned above, the pose of the keyframe is updated. That is, the original pose is multiplied to the right by an increment Δ. The small transformations constitute the updated pose. For example, the pose of keyframe j. Updated pose = .

[0175] By calculating the increment in the Lie algebra space and using an exponential mapping update method, it can be guaranteed that the updated pose still satisfies its inherent manifold constraints, avoiding the generation of illegal states caused by improper parameterization, and ensuring the accuracy and correctness of the optimization process.

[0176] The pose graph optimization method provided in this invention solves linear equations by matrix square root decomposition. Compared with directly inverting the global information matrix, it has better numerical stability while ensuring solution accuracy and improving solution speed.

[0177] Based on any of the above embodiments, step S130 includes: step S131 and step S132.

[0178] Step S131: Obtain the first sensor data of the current frame and the second sensor data of the closed-loop candidate frame.

[0179] Acquire the sensor data of the current frame, denoted as the first sensor data, and acquire the sensor data of the closed-loop candidate frame, denoted as the second sensor data.

[0180] The types of data from the first and second sensors include, but are not limited to, GMSS data, point cloud data, and image data. GMSS data can be acquired through a GNSS module, point cloud data can be acquired through a ToF sensor or lidar, and image data can be acquired through an image sensor.

[0181] Furthermore, the types of the first and second sensor data can be determined based on the current scene. For example, if the current scene is outdoor, the first and second sensor data are determined to be GMSS data; if the current scene is indoor, the first and second sensor data are determined to be point cloud data or image data. It should be noted that in indoor scenes, point cloud data is preferred; if the point cloud data is sparse, image data is used.

[0182] By intelligently selecting the most reliable sensor data based on the actual environment to construct closure constraints and their corresponding closure covariance matrix, the system can effectively detect and utilize closure information in any scenario, greatly enhancing the system's robustness and adaptability to complex and changing environments.

[0183] Step S132: By matching the first sensor data and the second sensor data, the loop closure constraint between the current frame and the closed-loop candidate frame is calculated, and the loop closure covariance matrix is ​​determined by evaluating the matching quality.

[0184] By matching the data from the first sensor and the data from the second sensor, the loop closure constraint between the current frame and the candidate closed-loop frame is calculated. At the same time, the loop closure covariance matrix is ​​determined by evaluating the matching quality.

[0185] Furthermore, if the first sensor data and the second sensor data are Global Mobile Satellite Service (GNSS) data, then the loop closure constraint is calculated based on the relative transformation between GNSS poses, and the loop closure covariance matrix is ​​determined based on the GNSS measurement variance.

[0186] If the data from the first sensor and the data from the second sensor are GNSS data, the GNSS data includes latitude, longitude, and altitude coordinates and GNSS accuracy information.

[0187] Transform the latitude, longitude, and height coordinates of the current frame and the closed-loop candidate frame into the same local Cartesian coordinate system to obtain the GNSS pose in the world coordinate system. Record the GNSS pose of the current frame as the first GNSS pose and the GNSS pose of the closed-loop candidate frame as the second GNSS pose.

[0188] Then, the relative transformation between the first GNSS pose and the second GNSS pose is calculated as the loop closure constraint. It should be noted that since GNSS typically does not provide precise attitude information, the loop closure constraint is a transformation that only includes translation and not rotation.

[0189] Simultaneously, based on the GNSS accuracy information of the current frame, the GNSS measurement variance of the current frame is calculated and denoted as the first GNSS measurement variance, which is the square of the GNSS accuracy information. Similarly, based on the GNSS accuracy information of the closed-loop candidate frames, the GNSS measurement variance of the closed-loop candidate frames is calculated and denoted as the second GNSS measurement variance.

[0190] Then, a cyclic covariance matrix is ​​constructed based on the first and second GNSS measurement variances. The diagonal elements of the translated portion of this cyclic covariance matrix are obtained by summing the first and second GNSS measurement variances. The diagonal elements of the rotated portion of the cyclic covariance matrix are set to very large values, such as 10. 6 10 8 The covariance portion (i.e., the off-diagonal part) of the loop covariance matrix is ​​set to 0.

[0191] Furthermore, if the first sensor data and the second sensor data are point cloud data, then the loop closure constraint is the optimal transformation matrix calculated based on the point cloud data using a point cloud registration algorithm, and the loop closure covariance matrix is ​​determined based on the point cloud matching score of the point cloud registration algorithm.

[0192] If the data from the first sensor and the data from the second sensor are point cloud data, then the point cloud data of the current frame is acquired and denoted as the first point cloud data, and the point cloud data of the candidate closed-loop frame is acquired and denoted as the second point cloud data. The first point cloud data and the second point cloud data are aligned using a point cloud matching algorithm to obtain the optimal transformation matrix, which serves as the loop closure constraint. The point cloud matching algorithm can employ the ICP (Iterative Closest Point) algorithm.

[0193] After ICP registration is completed, the point cloud matching score from the ICP algorithm is used to construct the clonic covariance matrix. The point cloud matching score is typically the root mean square error of the distances between all corresponding point pairs after registration. When constructing the clonic covariance matrix, a simple mapping relationship can be established: the lower the point cloud matching score, i.e., the better the matching, the smaller the variance term in the corresponding clonic covariance matrix, indicating a more reliable constraint. For example, the diagonal elements of the clonic covariance matrix (representing the variance term) can be set to be proportional to the square of the root mean square error.

[0194] Furthermore, if the first sensor data and the second sensor data are image data, then the loop closure constraint is the camera relative pose calculated based on the two-dimensional feature points matched by the image data and their corresponding three-dimensional landmark points, and the loop closure covariance matrix is ​​determined based on the image reprojection error.

[0195] If the first sensor data and the second sensor data are image data, the image data of the current frame is acquired and denoted as the first image data, and the image data of the closed-loop candidate frame is acquired and denoted as the second image data. Feature point matching is performed on the first image data and the second image data to obtain matched feature point pairs. When performing feature point matching, algorithms such as ORB (Oriented FAST and Rotated BRIEF, a feature extraction algorithm combining fast feature point detection and binary robust independent basic feature descriptors) and SIFT (Scale-Invariant Feature Transform) can be used.

[0196] Then, for the two-dimensional feature points corresponding to the first image data in the feature point pair, the corresponding three-dimensional landmark points are found from the map, and the relative pose of the camera is calculated by the PnP (Perspective-n-Point) solver. The relative pose of the camera is the relative pose of the camera in the current frame with respect to the camera in the loop closure candidate frame, and the relative pose of the camera is used as the loop closure constraint.

[0197] The loop closure covariance matrix is ​​determined based on the image reprojection error generated by the PnP solver. Specifically, after calculating the camera's relative pose, all 3D landmarks involved in the PnP calculation are reprojected onto the image plane of the current frame according to the camera's relative pose, resulting in a set of theoretical 2D projection points, denoted as theoretical 2D projection points. The pixel distance between the coordinates of the theoretical 2D projection points and the coordinates of the actually observed 2D feature points is calculated, i.e., the image reprojection error. Then, the diagonal elements of the loop closure covariance matrix are set to be proportional to the square of the image reprojection error. The smaller the image reprojection error, the smaller the element values ​​of the loop closure covariance matrix, indicating a higher confidence level of the constraint.

[0198] The pose graph optimization method provided in this invention provides corresponding loop closure constraints and methods for determining their loop closure covariance matrices for different types of sensor data, such as GNSS data, point cloud data, and image data. By using the precisely calculated loop closure constraints and loop closure covariance matrices for global pose graph optimization, the cumulative drift error caused by long-term operation of XR devices can be effectively eliminated, thereby improving the pose graph optimization results.

[0199] The pose graph optimization apparatus provided by the present invention will be described below. The pose graph optimization apparatus described below can be referred to in correspondence with the pose graph optimization method described above.

[0200] Figure 5 This is a schematic diagram of the pose graph optimization device provided by the present invention, as shown below. Figure 5 As shown, the device includes a first calculation module 510, a second calculation module 520, a third calculation module 530, a problem construction module 540, and a pose update module 550; wherein:

[0201] The first calculation module 510 is used to calculate the odometry constraints between adjacent keyframes;

[0202] The second calculation module 520 is used to calculate the odometer covariance matrix corresponding to the odometer constraint using an uncertainty propagation model; wherein the uncertainty propagation model is derived from the odometer constraint formula using an analytical method.

[0203] The third calculation module 530 is used to calculate the closure constraints between the current frame and the closure candidate frame and their corresponding closure covariance matrix.

[0204] Problem construction module 540 is used to construct a pose graph optimization problem based on the odometry constraints, the odometry covariance matrix, the loop closure constraints, and the loop closure covariance matrix.

[0205] The pose update module 550 is used to construct a global information matrix for the pose graph optimization problem, optimize and solve the global information matrix, and update the pose of the keyframes.

[0206] The pose graph optimization device provided in this invention calculates the odometry constraints between adjacent keyframes and, through an uncertainty propagation model, calculates the odometry covariance matrix corresponding to the odometry constraints. The uncertainty propagation model is derived analytically from the odometry constraint formula. This uncertainty propagation model, derived analytically, is used to calculate the odometry covariance matrix corresponding to the odometry constraints. Compared to existing empirical settings and numerical approximations, this invention achieves adaptive dynamic adjustment and online accurate estimation of odometry constraint weights, resulting in higher precision. Simultaneously, the analytical method significantly reduces computational overhead, enabling real-time operation on XR devices with limited computing power, achieving low-latency, high-precision pose estimation and enhancing the immersive experience. Furthermore, it calculates the closure constraints between the current frame and closed-loop candidate frames and their corresponding closure covariance matrices. Then, based on the odometry constraints, odometry covariance matrix, closure constraints, and closure covariance matrix, a pose graph optimization problem is constructed, a global information matrix for the pose graph optimization problem is built, the global information matrix is ​​optimized, and the pose of the keyframes is updated. Since both the odometer constraint and the loop closure constraint use an adaptive covariance matrix, the weight of undesirable constraints will be naturally reduced during global optimization, thus exhibiting stronger generalization ability and robustness when facing complex environments.

[0207] It should be noted that the pose graph optimization device provided in this embodiment of the invention can implement all the method steps implemented in the above pose graph optimization method embodiment and can achieve the same technical effect. Here, the parts that are the same as those in the method embodiment and the beneficial effects will not be described in detail.

[0208] Figure 6 An example is a schematic diagram of the physical structure of an XR device, such as... Figure 6As shown, the XR device may include a processor 610, a communications interface 620, a memory 630, and a communication bus 640. The processor 610, communications interface 620, and memory 630 communicate with each other via the communication bus 640. The processor 610 can call logical instructions in the memory 630 to execute a pose graph optimization method. This method includes: calculating odometry constraints between adjacent keyframes; calculating the odometry covariance matrix corresponding to the odometry constraints using an uncertainty propagation model; wherein the uncertainty propagation model is derived analytically from the odometry constraint formula; calculating the closure constraints and their corresponding closure covariance matrices between the current frame and the closure candidate frames; constructing a pose graph optimization problem based on the odometry constraints, the odometry covariance matrix, the closure constraints, and the closure covariance matrix; constructing a global information matrix for the pose graph optimization problem; optimizing and solving the global information matrix; and updating the pose of the keyframes.

[0209] Furthermore, the logical instructions in the aforementioned memory 630 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0210] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the pose graph optimization method provided in the above embodiments. The method includes: calculating odometry constraints between adjacent keyframes; calculating the odometry covariance matrix corresponding to the odometry constraints using an uncertainty propagation model; wherein the uncertainty propagation model is derived from the odometry constraint formula using an analytical method; calculating the closure constraints and their corresponding closure covariance matrices between the current frame and the closure candidate frame; constructing a pose graph optimization problem based on the odometry constraints, the odometry covariance matrix, the closure constraints, and the closure covariance matrix; constructing a global information matrix for the pose graph optimization problem; optimizing and solving the global information matrix; and updating the pose of the keyframes.

[0211] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements the pose graph optimization method provided in the above embodiments. The method includes: calculating odometry constraints between adjacent keyframes; calculating the odometry covariance matrix corresponding to the odometry constraints using an uncertainty propagation model; wherein the uncertainty propagation model is derived analytically from the odometry constraint formula; calculating the closure constraints and their corresponding closure covariance matrices between the current frame and the closure candidate frame; constructing a pose graph optimization problem based on the odometry constraints, the odometry covariance matrix, the closure constraints, and the closure covariance matrix; constructing a global information matrix for the pose graph optimization problem; optimizing and solving the global information matrix; and updating the pose of the keyframes.

[0212] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0213] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0214] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A pose graph optimization method, characterized in that, include: Calculate the odometry constraints between adjacent keyframes; The odometer covariance matrix corresponding to the odometer constraint is calculated using an uncertainty propagation model; wherein, the uncertainty propagation model is obtained by simplifying the odometer constraint formula using Lie algebra properties, adjoint matrix properties, and the Beck-Campbell-Hausdorff (BCH) formula. Calculate the closure constraints between the current frame and the candidate closed-loop frames and their corresponding closure covariance matrix; Based on the odometry constraints, the odometry covariance matrix, the loop closure constraints, and the loop closure covariance matrix, a pose graph optimization problem is constructed. A global information matrix for the pose graph optimization problem is constructed using a sparse data structure. The global information matrix is ​​then optimized and solved to update the pose of the keyframes. The step of calculating the odometer covariance matrix corresponding to the odometer constraint using the uncertainty propagation model includes: Obtain the first and second nominal pose transformation matrices of adjacent keyframes; Obtain the first covariance matrix of the first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtain the second covariance matrix of the second Lie algebra error corresponding to the second nominal pose transformation matrix; wherein, the first Lie algebra error and the second Lie algebra error are both modeled as zero-mean Gaussian distributions; Obtain the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error; The odometer covariance matrix corresponding to the odometer constraint is calculated based on the first covariance matrix, the second covariance matrix, the adjoint matrix of the inverse of the first nominal pose transformation matrix, and the cross covariance matrix.

2. The pose graph optimization method according to claim 1, characterized in that, The calculation of odometry constraints between adjacent keyframes includes: Obtain the first pose transformation matrix and the second pose transformation matrix of adjacent keyframes; Calculate the inverse of the first pose transformation matrix; Multiplying the inverse matrix by the second pose transformation matrix yields the odometry constraint between adjacent keyframes.

3. The pose graph optimization method according to claim 1, characterized in that, The step of constructing a pose graph optimization problem based on the odometer constraints, the odometer covariance matrix, the loop closure constraints, and the loop closure covariance matrix includes: Define the pose of the keyframe as a state variable; Based on the state variables, the odometer constraints, the odometer covariance matrix, the laparosional constraints, and the laparosional covariance matrix, a nonlinear least squares objective function is constructed with the goal of minimizing the weighted sum of squared residuals.

4. The pose graph optimization method according to claim 3, characterized in that, The global information matrix for constructing the pose graph optimization problem includes: The nonlinear least squares objective function is linearized to calculate the first Jacobian matrix corresponding to the odometer constraint, the second Jacobian matrix corresponding to the loop closure constraint, and the global error vector. According to the pre-established address mapping table of the global information matrix, the first matrix block calculated by the first Jacobian matrix and the first local information matrix is ​​directly accumulated into the first address corresponding to the global information matrix; wherein, the first local information matrix is ​​obtained by inverting the odometer covariance matrix; According to the pre-established address mapping table of the global information matrix, the second matrix block calculated by the second Jacobian matrix and the second local information matrix is ​​directly accumulated into the second address corresponding to the global information matrix; wherein, the second local information matrix is ​​obtained by inverting the loop covariance matrix.

5. The pose graph optimization method according to claim 4, characterized in that, The optimization and solution of the global information matrix, and the updating of the pose of the keyframes, include... Based on the global information matrix and the global error vector, construct a linear equation; The global information matrix is ​​decomposed by the square root of the matrix to solve the linear equation and obtain the optimal increment of the keyframe pose. The pose of the keyframe is updated based on the optimal increment.

6. The pose graph optimization method according to any one of claims 1 to 5, characterized in that, The calculation of the loop closure constraints and their corresponding loop closure covariance matrix between the current frame and the closed-loop candidate frames includes: Acquire the first sensor data of the current frame and the second sensor data of the closed-loop candidate frame; By matching the first sensor data and the second sensor data, the loop closure constraint between the current frame and the closed-loop candidate frame is calculated, and the loop closure covariance matrix is ​​determined by evaluating the matching quality. Wherein, if the first sensor data and the second sensor data are Global Mobile Satellite Service (GNSS) data, the loop closure constraint is calculated based on the relative transformation between GNSS poses, and the loop closure covariance matrix is ​​determined based on the GNSS measurement variance; If the first sensor data and the second sensor data are point cloud data, then the loop closure constraint is the optimal transformation matrix calculated by the point cloud registration algorithm based on the point cloud data, and the loop closure covariance matrix is ​​determined based on the point cloud matching score of the point cloud registration algorithm. If the first sensor data and the second sensor data are image data, then the loop closure constraint is the camera relative pose calculated based on the two-dimensional feature points matched with the image data and their corresponding three-dimensional landmark points, and the loop closure covariance matrix is ​​determined based on the image reprojection error.

7. A pose graph optimization device, characterized in that, include: The first calculation module is used to calculate the odometry constraints between adjacent keyframes; The second calculation module is used to calculate the odometer covariance matrix corresponding to the odometer constraint using an uncertainty propagation model; wherein, the uncertainty propagation model is obtained by simplifying the odometer constraint formula using Lie algebra properties, adjoint matrix properties, and the Beck-Campbell-Hausdorff BCH formula. The third calculation module is used to calculate the closure constraints between the current frame and the closure candidate frame and their corresponding closure covariance matrix. The problem construction module is used to construct a pose graph optimization problem based on the odometry constraints, the odometry covariance matrix, the loop closure constraints, and the loop closure covariance matrix. The pose update module is used to construct a global information matrix for the pose graph optimization problem using a sparse data structure, optimize and solve the global information matrix, and update the pose of the keyframes. The second calculation module is specifically used for: Obtain the first and second nominal pose transformation matrices of adjacent keyframes; Obtain the first covariance matrix of the first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtain the second covariance matrix of the second Lie algebra error corresponding to the second nominal pose transformation matrix; wherein, the first Lie algebra error and the second Lie algebra error are both modeled as zero-mean Gaussian distributions; Obtain the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error; The odometer covariance matrix corresponding to the odometer constraint is calculated based on the first covariance matrix, the second covariance matrix, the adjoint matrix of the inverse of the first nominal pose transformation matrix, and the cross covariance matrix.

8. An XR device, comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, When the processor executes the computer program, it implements the pose graph optimization method as described in any one of claims 1 to 6.

Images