Pose map optimization method and apparatus, and XR device

Abstract

The invention provides a pose map optimization method and device and XR equipment, and relates to the technical field of image processing, and the method comprises the following steps: calculating speedometer constraints between adjacent key frames; calculating a speedometer covariance matrix corresponding to speedometer constraints through an uncertainty propagation model; wherein the uncertainty propagation model is obtained by deducing an odometer constraint formula through an analytical method; calculating a loopback constraint between the current frame and the closed-loop candidate frame and a loopback covariance matrix corresponding to the loopback constraint; according to the speedometer constraint, the speedometer covariance matrix, the loopback constraint and the loopback covariance matrix, constructing a pose map optimization problem; and establishing a global information matrix, carrying out optimization solution on the global information matrix, and updating the pose of the key frame. The method can be operated in real time at an XR equipment end with limited computing power, low-delay and high-precision pose estimation is realized, and stronger robustness can be shown in a complex environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of image processing, and in particular to a pose graph optimization method and device and an XR device. BACKGROUND

[0002] With the rise of the concept of the metaverse, 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 attitude) in the environment, thereby realizing seamless superposition and interaction between the virtual world and the real environment. Pose graph optimization (PGO) is a core technology of the SLAM backend, which optimizes the estimated trajectory and map by constructing a graph model composed of pose nodes and edges connecting these nodes. The key to optimization lies in how to assign appropriate weights to each edge (i.e., the constraint between pose nodes) in the pose graph, which is determined by the uncertainty of the edge.

[0003] In the prior art, the method of setting the uncertainty of the edge often cannot balance real-time performance, accuracy, and computational overhead, which makes them unable to be directly applied to XR devices with strict limitations on hardware computing power and power consumption.

[0004] Therefore, how to accurately and adaptively model the uncertainty of the edge in the pose graph online without increasing the computational overhead, so as to realize real-time pose optimization estimation with high accuracy and robustness in XR devices with limited computing power, is a problem that needs to be solved at present. SUMMARY

[0005] The present application provides a pose graph optimization method and device and an XR device to solve the problem that the existing uncertainty setting method often cannot balance real-time performance, accuracy, and computational overhead, making it impossible to be directly applied to XR devices with strict limitations on hardware computing power and power consumption.

[0006] The present application provides a pose graph optimization method, comprising: calculating an odometry constraint between adjacent key frames; calculating an odometry covariance matrix corresponding to the odometry constraint through an uncertainty propagation model, wherein the uncertainty propagation model is obtained by deducing the odometry constraint formula through an analytical method; calculating a loop constraint between the current frame and the loop candidate frame and a loop covariance matrix corresponding thereto; constructing a pose graph optimization problem according to the odometry constraint, the odometry covariance matrix, the loop constraint, and the loop covariance matrix; Assemble a global information matrix of the pose graph optimization problem, optimize and solve the global information matrix, and update the pose of the key frame.

[0007] According to the pose graph optimization method provided by the application, the uncertainty propagation model is obtained by simplifying the Lie algebra property, the adjoint matrix property and the Beck-Cambridge-Hausdorff (BCH) formula linearization of the odometer constraint formula, the odometer covariance matrix corresponding to the odometer constraint is calculated through the uncertainty propagation model, and the odometer covariance matrix corresponding to the odometer constraint is calculated through the uncertainty propagation model. Obtain a first nominal pose transformation matrix and a second nominal pose transformation matrix of adjacent key frames. Obtain a first covariance matrix of a first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtain a second covariance matrix of a 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 modeled as zero-mean Gaussian distribution. According to the first covariance matrix, the second covariance matrix, and the adjoint matrix of the inverse of the first nominal pose transformation matrix, the odometer covariance matrix corresponding to the odometer constraint is calculated.

[0008] According to the pose graph optimization method provided by the application, the odometer covariance matrix corresponding to the odometer constraint is calculated according to the first covariance matrix, the second covariance matrix, and the adjoint matrix of the inverse of the first nominal pose transformation matrix. Obtain a cross-covariance matrix of the first Lie algebra error and the second Lie algebra error. According to 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 odometer covariance matrix corresponding to the odometer constraint is calculated.

[0009] According to the pose graph optimization method provided by the application, the odometer constraint between adjacent key frames is calculated, including: Obtain a first pose transformation matrix and a second pose transformation matrix of adjacent key frames. Calculate the inverse matrix of the first pose transformation matrix. Multiply the inverse matrix and the second pose transformation matrix to obtain the odometer constraint between adjacent key frames.

[0010] According to the pose graph optimization method provided by the application, the odometer constraint, the odometer covariance matrix, the loop constraint and the loop covariance matrix are used to construct a pose graph optimization problem, including: The pose of the key frame is defined as a state variable. construct a nonlinear least squares objective function aiming at minimizing a weighted sum of squared residuals according to the state variable, the odometry constraint, the odometry covariance matrix, the loop constraint and the loop covariance matrix.

[0011] According to the pose graph optimization method provided by the application, the global information matrix of the pose graph optimization problem is constructed, comprising: linearize the nonlinear least squares objective function to calculate a first Jacobian matrix corresponding to the odometry constraint, a second Jacobian matrix corresponding to the loop constraint, and a 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 added to the first address corresponding to the global information matrix; wherein the first local information matrix is obtained by inverting the odometry 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 added to the second address corresponding to the global information matrix; wherein the second local information matrix is obtained by inverting the loop covariance matrix.

[0012] According to the pose graph optimization method provided by the application, the global information matrix is optimized and solved, and the pose of the key frame is updated, comprising: construct a linear equation according to the global information matrix and the global error vector; perform matrix square root decomposition on the global information matrix to solve the linear equation and obtain an optimal increment of the key frame pose; update the pose of the key frame according to the optimal increment.

[0013] According to the pose graph optimization method provided by the application, the loop constraint between the current frame and the loop candidate frame and the corresponding loop covariance matrix are calculated, comprising: obtain first sensor data of the current frame and second sensor data of the loop candidate frame; match the first sensor data and the second sensor data to calculate the loop constraint between the current frame and the loop candidate frame, and determine the loop covariance matrix by evaluating the matching quality; If the first sensor data and the second sensor data are global navigation satellite system (GNSS) data, the loop constraint is a relative transformation between GNSS poses calculated according to the GNSS data, and the loop covariance matrix is determined according to a GNSS measurement variance; If the first sensor data and the second sensor data are point cloud data, the loop constraint is an optimal transformation matrix calculated by a point cloud registration algorithm according to the point cloud data, and the loop covariance matrix is determined according to a point cloud matching score of the point cloud registration algorithm. If the first sensor data and the second sensor data are image data, the loop constraint is a relative camera pose calculated according to two-dimensional feature points matched in the image data and corresponding three-dimensional landmark points, and the loop covariance matrix is determined according to image re-projection errors.

[0014] The application further provides a pose graph optimization device, comprising: a first calculation module configured to calculate an odometry constraint between adjacent key frames; a second calculation module configured to calculate an odometry covariance matrix corresponding to the odometry constraint by an uncertainty propagation model, wherein the uncertainty propagation model is obtained by deducing an odometry constraint formula by an analytical method; a third calculation module configured to calculate a loop constraint between a current frame and a loop candidate frame and a loop covariance matrix corresponding to the loop constraint; a problem construction module configured to construct a pose graph optimization problem according to the odometry constraint, the odometry covariance matrix, the loop constraint and the loop covariance matrix; a pose update module configured to assemble a global information matrix of the pose graph optimization problem, to optimize and solve the global information matrix, and to update a pose of a key frame.

[0015] The application further provides an XR device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the pose graph optimization method according to any one of the above when executing the computer program.

[0016] The pose graph optimization method, device and XR equipment provided by the application, by calculating the odometer constraint between adjacent key frames, and by the uncertainty propagation model, the odometer covariance matrix corresponding to the odometer constraint is calculated; wherein the uncertainty propagation model is obtained by deducing the odometer constraint formula by an analytical method. The uncertainty propagation model constructed based on the analytical method is used to calculate the odometer covariance matrix corresponding to the odometer constraint. Compared with the existing empirical setting and numerical approximation method, the application realizes the adaptive dynamic adjustment and online accurate estimation of the odometer constraint weight, and has higher precision. At the same time, the above-mentioned analytical method can greatly reduce the calculation overhead, so that it can be run in real time on the XR equipment side with limited computing power, realizing low delay and high precision pose estimation, and improving the immersive experience. At the same time, the loop constraint between the current frame and the loop candidate frame and the loop covariance matrix corresponding thereto are calculated, then, according to the odometer constraint, the odometer covariance matrix, the loop constraint and the loop covariance matrix, the pose graph optimization problem is constructed, the global information matrix of the pose graph optimization problem is constructed, the global information matrix is optimized and solved, and the pose of the key frame is updated. Since the odometer constraint and the loop constraint both use adaptive covariance matrix, the weight of the bad constraint will be naturally reduced during global optimization, so that it can show stronger generalization ability and robustness in complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings in the following description are some embodiments of the application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0018] Figure 1 is the system architecture diagram of the pose graph optimization system provided by the application; Figure 2 is one of the flowcharts of the pose graph optimization method provided by the application; Figure 3 is the second flowchart of the pose graph optimization method provided by the application; Figure 4 is the third flowchart of the pose graph optimization method provided by the application; Figure 5 is the structural diagram of the pose graph optimization device provided by the application; Figure 6 is the structural diagram of the XR equipment provided by the application. DETAILED DESCRIPTION

[0019] In order to make the objects, technical solutions and advantages of the present application clearer, the technical solutions in the present application will be described clearly and completely below in conjunction with the drawings in the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0020] With the rise of the concept of metaverse, 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 attitude) in the environment, thereby realizing seamless superposition and interaction between the virtual world and the real environment. Pose graph optimization (PGO) is a core technology of the SLAM backend, which optimizes the estimated trajectory and map by constructing a graph model composed of pose nodes and edges connecting these nodes. The key to optimization is how to assign appropriate weights to each edge (i.e., the constraint between pose nodes) in the pose graph, which is determined by the uncertainty of the edge.

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

[0022] In summary, the prior art often cannot balance real-time performance, accuracy and computational overhead in uncertainty modeling, which makes them unable to be directly applied to XR devices with strict restrictions on hardware computing power and power consumption.

[0023] Therefore, how to accurately and adaptively model the uncertainty of the edge in the pose graph online without increasing the computational overhead, so as to realize real-time pose optimization estimation with high accuracy and robustness in XR devices with limited computing power, is a problem that needs to be solved at present.

[0024] Based on the above problems, the present application provides a pose graph optimization method, device and XR equipment, which will be described below. Figures 1-6

[0025] Figure 1 is the system architecture diagram of the pose graph optimization system provided by the present application, as Figure 1 shown, the pose graph optimization system includes an XR equipment 01 and a server 02.

[0026] Among them, the XR equipment 01 refers to a wearable or portable device that realizes man-machine interaction by fusing virtual and real environment through hardware and software technology, which can include but not limited to: VR (Virtual Reality, virtual reality) equipment, AR (Augmented Reality, augmented reality) equipment and MR (Mixed Reality, mixed reality) equipment. The VR equipment is to simulate a three-dimensional virtual space by using computer technology, so that the user can immerse in it and interact with it to obtain an immersive experience; the AR equipment is to fuse virtual information with the real world through technology, and to superimpose it to the real scene in real time to improve the sensory experience; the MR equipment is to mix the real world and the virtual world together to produce a new visual environment, which contains physical entities and virtual information at the same time, and the user can interact with these physical entities and virtual information in real time. Specifically, the type of XR equipment can be smart glasses or head-mounted display.

[0027] The server 02 can be a remote server that provides various services, such as analyzing and processing the data collected by the sensors (such as image sensors, inertial measurement units and wheel odometry, etc.) of the XR equipment, and can also be a local computing device of the XR equipment configured by the target carrier.

[0028] Figure 2 is one of the flowcharts of the pose graph optimization method provided by the present application, as Figure 2 shown, the pose graph optimization method includes steps S110, S120, S130, S140 and S150.

[0029] Step S110, calculate the odometry constraint between adjacent key frames.

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

[0031] ​When the XR device moves in the environment, its built-in sensors collect relevant data in real time, and the system selects a specific time point as a key frame. The sensors include but are not limited to: image sensors (such as cameras), IMUs (Inertial Measurement Units), wheel odometry, lidar, RTK (Real-time kinematic), etc. The selection criteria for key frames can be based on time intervals, motion distances, or scene change degrees, etc., which are not limited here.

[0032] The odometry constraint between adjacent key frames is also referred to as the odometry edge between adjacent key frames, or the relative pose transformation between adjacent key frames.

[0033] Specifically, the first pose transformation matrix and the second pose transformation matrix of the adjacent key frames are obtained, then the inverse matrix of the first pose transformation matrix is calculated, and then the inverse matrix is multiplied by the second pose transformation matrix to obtain the odometry constraint between the adjacent key frames. The specific execution process can be referred to in the following embodiments, which will not be repeated here.

[0034] The first pose transformation matrix and the second pose transformation matrix are output by 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 a camera, an inertial measurement unit).

[0035] In step S120, the odometry covariance matrix corresponding to the odometry constraint is calculated by the uncertainty propagation model, wherein the uncertainty propagation model is obtained by deducing the odometry constraint formula by an analytical method.

[0036] The odometry covariance matrix refers to the covariance matrix corresponding to the odometry constraint, which is denoted as the odometry covariance matrix for distinguishing from other covariance matrices, and is used to represent the uncertainty between the odometry edges corresponding to the adjacent key frames.

[0037] The odometry covariance matrix corresponding to the odometry constraint is calculated by the uncertainty propagation model.

[0038] The uncertainty propagation model is obtained by deducing the odometry constraint formula by an analytical method. Specifically, the uncertainty propagation model is obtained by using the Lie algebra property simplification, the adjoint matrix property and the BCH (Baker-Campbell-Hausdorff) linearization on the odometry constraint formula. The specific derivation process is as follows: The pose is represented in the form of Lie group SE(3), and the error term is represented in the form of Lie algebra se(3). The error model of the pose is: wherein T represents the true pose, denotes the error term, is the Lie algebra error, denotes the nominal pose, is the nominal pose transformation matrix. Wherein, the Lie algebra error is modeled as a zero-mean Gaussian distribution, i.e. wherein, is the position vector, is the SO(3) Lie algebra.

[0039] For two adjacent key frames, denoted as the first key frame j and the second key frame k respectively, the first pose transformation matrix and the second pose transformation matrix of the adjacent key frames are obtained. Assuming that the first key frame j and the second key frame k are both in the i coordinate system, the pose transformation matrix corresponding to the first key frame j is denoted as the first pose transformation matrix , and the pose transformation matrix corresponding to the second key frame k is denoted as the second pose transformation matrix .

[0040] The inverse matrix of the first pose transformation matrix is multiplied by the second pose transformation matrix to obtain the odometry constraint between the adjacent key frames. The specific formula is as follows: .

[0041] Considering the pose error, i.e. substituting the error model of the pose into the above odometry constraint formula, we can obtain: ; Further, by using the Lie algebra property, we can obtain: ; Further, by using the adjoint matrix property, we can obtain: ; Further, for small errors, by using the BCH formula for first-order linearization approximation, we can obtain: .

[0042] Based on the above uncertainty propagation model, in an embodiment, the calculation process of the odometry covariance matrix is as follows: obtaining a first nominal pose transformation matrix and a second nominal pose transformation matrix of adjacent key frames; obtaining a first covariance matrix of a first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtaining a second covariance matrix of a 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 distribution; obtaining a cross-covariance matrix of the first Lie algebra error and the second Lie algebra error; and calculating the odometry covariance matrix corresponding to the odometry constraint according to the first covariance matrix, the second covariance matrix, the adjugate matrix of the inverse of the first nominal pose transformation matrix, and the cross-covariance matrix. The specific execution process can be referred to in the following embodiments, and will not be repeated here.

[0043] In another embodiment, the calculation process of the odometry covariance matrix is as follows: obtaining a first nominal pose transformation matrix and a second nominal pose transformation matrix of adjacent key frames; obtaining a first covariance matrix of a first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtaining a second covariance matrix of a 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 distribution; when the first Lie algebra error and the second Lie algebra error are not correlated, i.e. the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error is a zero matrix, calculating the odometry covariance matrix corresponding to the odometry constraint according to the first covariance matrix, the second covariance matrix, and the adjugate 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 repeated here.

[0044] It should be noted that the trigger of the embodiment of the present application can be determined according to the execution result of the closed loop detection module. Specifically, the closed loop detection module of the system background continuously matches the sensor information of the current frame with the historical key frame information stored in the map database to perform loop detection, and when a loop is detected, the pose graph optimization process of the embodiment of the present application is triggered

[0045] It should be understood that the odometry constraint and its corresponding odometry covariance matrix can be calculated synchronously and stored in the map database when a new key frame is generated. When the closed loop detection module detects a loop, the stored odometry constraint and its corresponding odometry covariance matrix are directly called from the map database. In this way, repeated calculation can be avoided, and the overall operation efficiency of the system can be improved.

[0046] Step S130, calculating a loop constraint between the current frame and the loop candidate frame and its corresponding loop covariance matrix.

[0047] When the loop closure is detected, a loop closure candidate frame corresponding to the current frame is acquired, and a loop closure constraint between the current frame and the loop closure candidate frame and a corresponding covariance matrix thereof are calculated. The loop closure constraint is a relative pose transformation between the current frame and the loop closure candidate frame, and is also referred to as a loop edge between the current frame and the loop closure candidate frame. For the sake of distinction, the covariance matrix of the loop closure constraint is referred to as a loop closure covariance matrix, which is used to represent the uncertainty of the loop edge between the current frame and the loop closure candidate frame.

[0048] Specifically, first sensor data of the current frame and second sensor data of the loop closure candidate frame are acquired, and then the first sensor data and the second sensor data are matched to calculate the loop closure constraint between the current frame and the loop closure candidate frame, and the loop closure covariance matrix is determined by evaluating the matching quality. The types of the first sensor data and the second sensor data include, but are not limited to, Global Mobile Satellite Service (GMSS) data, point cloud data and image data. The GMSS data can be acquired by a GNSS module, the point cloud data can be acquired by a Time of Flight (ToF) sensor or a laser radar, and the image data can be acquired by an image sensor. The specific execution process can be referred to the following embodiments, which will not be described here.

[0049] In step S140, a pose graph optimization problem is constructed according to the odometry constraint, the odometry covariance matrix, the loop closure constraint and the loop closure covariance matrix.

[0050] The poses of all key frames are defined as state variables to be optimized, and all odometry constraints and loop closure constraints are defined as edges to construct a pose graph. The goal of optimization is to adjust the poses of all key frames so that the weighted sum of squared errors of all constraints is minimized.

[0051] Specifically, a nonlinear least squares objective function with the goal of minimizing the weighted sum of squared residuals is constructed according to the state variables, the odometry constraints, the odometry covariance matrix, the loop closure constraints and the loop closure covariance matrix. The specific execution process can be referred to the following embodiments, which will not be described here.

[0052] In step S150, a global information matrix of the pose graph optimization problem is constructed, the global information matrix is optimized and solved, and the poses of the key frames are updated.

[0053] After the pose graph optimization problem is constructed, a sparse data structure is used to construct a global information matrix of the pose graph optimization problem.

[0054] The global information matrix, also referred to as a Hessian matrix, is a huge sparse matrix.

[0055] In an embodiment, the process of building the global information matrix is as follows: linearizing the nonlinear least squares objective function to calculate the first Jacobian matrix corresponding to the odometry constraints, the second Jacobian matrix corresponding to the loop constraints, and the global error vector; then, according to the pre-established address mapping table of the global information matrix, directly adding the first matrix block calculated from the first Jacobian matrix and the first local information matrix to the first address corresponding to the global information matrix; wherein the first local information matrix is obtained by inverting the odometry covariance matrix; at the same time, according to the pre-established address mapping table of the global information matrix, directly adding the second matrix block calculated from the second Jacobian matrix and the second local information matrix to the second address corresponding to the global information matrix; wherein the second local information matrix is obtained by inverting the loop covariance matrix.

[0056] In another embodiment, the process of building the global information matrix is as follows: linearizing the nonlinear least squares objective function to calculate the first Jacobian matrix corresponding to the odometry constraints, the second Jacobian matrix corresponding to the loop constraints, and the global error vector; inverting the odometry covariance matrix to obtain the first local information matrix, and inverting the loop covariance matrix to obtain the second local information matrix; calculating the matrix block of the odometry constraints according to the first Jacobian matrix and the first local information matrix, and calculating the matrix block of the loop constraints according to the second Jacobian matrix and the second local information matrix; and building the global information matrix according to the matrix block of the odometry constraints and the matrix block of the loop constraints.

[0057] The specific execution process can refer to the following embodiments, which will not be repeated here.

[0058] It should be noted that, compared with the second embodiment, the first embodiment can avoid invalid operations and repeated memory applications of large matrices in the optimization iteration process, can greatly reduce the memory management overhead, and can improve the construction speed of the global information matrix by combining the address mapping table for direct memory address accumulation.

[0059] After building the global information matrix of the pose graph optimization problem, the global information matrix is optimized and solved, and the pose of the key frame is updated.

[0060] Specifically, a linear equation is constructed according to the global information matrix and the global error vector; the global information matrix is subjected to matrix square root decomposition to solve the linear equation, so as to obtain the optimal increment of the key frame pose; and the pose of the key frame is updated according to the optimal increment. The specific execution process can refer to the following embodiments, which will not be repeated here. By subjecting the global information matrix to matrix square root decomposition, compared with directly inverting the global information matrix, the solving speed and numerical stability can be improved while ensuring the solving accuracy.

[0061] The pose graph optimization method provided by the embodiment of the present application calculates the odometer constraint between adjacent key frames, and calculates the odometer covariance matrix corresponding to the odometer constraint through an uncertainty propagation model. The uncertainty propagation model is obtained by deducing the odometer constraint formula through an analytical method. The uncertainty propagation model constructed based on the analytical method is used to calculate the odometer covariance matrix corresponding to the odometer constraint. Compared with the existing empirical setting and numerical approximation method, the embodiment of the present application realizes adaptive dynamic adjustment and online accurate estimation of the odometer constraint weight, and has higher precision. At the same time, the above-mentioned analytical method can greatly reduce the calculation overhead, so that real-time operation can be realized on the XR device side with limited computing power, realizing low-delay and high-precision pose estimation and improving the immersive experience. At the same time, the loop constraint between the current frame and the loop candidate frame and the loop covariance matrix corresponding thereto are calculated. Then, the pose graph optimization problem is constructed according to the odometer constraint, the odometer covariance matrix, the loop constraint and the loop covariance matrix, the global information matrix of the pose graph optimization problem is established, the global information matrix is optimized and solved, and the pose of the key frame is updated. Since the odometer constraint and the loop constraint both use adaptive covariance matrices, the weight of the bad constraint will be naturally reduced during global optimization, so that the generalization ability and robustness can be better in complex environments.

[0062] Based on any of the above embodiments, Figure 3 is a flowchart of the pose graph optimization method provided by the present application, as Figure 3 shown, the step S120 includes a step S121, a step S122 and a step S123.

[0063] Step S121, obtaining a first nominal pose transformation matrix and a second nominal pose transformation matrix of adjacent key frames.

[0064] For two adjacent key frames, denoted as a first key frame j and a second key frame k, a first nominal pose transformation matrix and a second nominal pose transformation matrix of adjacent key frames are obtained. Assuming that the first key frame j and the second key frame k are both in the i coordinate system, the first nominal pose transformation matrix corresponding to the first key frame j is denoted as , and the second nominal pose transformation matrix corresponding to the second key frame k is denoted as .

[0065] Step S122, obtaining a first covariance matrix of a first Lie algebra error corresponding to the first nominal pose transformation matrix, and obtaining a second covariance matrix of a 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 distribution.

[0066] 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 .

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

[0068] 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.

[0069] 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: .

[0070] 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 .

[0071] 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: .

[0072] At this time, the odometer covariance matrix corresponding to the odometer constraint can be calculated directly according to the first covariance matrix , the second covariance matrix , and the adjugate matrix of the inverse of the first nominal pose transformation matrix. Specifically, the first covariance matrix, the second covariance matrix, and the adjugate matrix of the inverse of the first nominal pose transformation matrix are directly substituted into the above simplified preset uncertainty propagation formula to calculate the odometer covariance matrix corresponding to the odometer constraint.

[0073] It should be noted that in the first embodiment, each error source and its correlation is considered in its entirety, and an accurate relative pose uncertainty estimation method is provided. Compared with the second embodiment, the real estimated uncertainty can be more accurately reflected, reliable constraint weights are provided for the subsequent optimization process, and the overall accuracy of the SLAM system is significantly improved. While in the second embodiment, the computational complexity is reduced by about 50% under the premise of ensuring sufficient accuracy. This simplification is particularly suitable for embedded systems with more limited computing resources, or in scenarios where the pose estimation indeed satisfies the independence assumption, achieving a balance between accuracy and efficiency.

[0074] It should be further noted that the preset uncertainty propagation formula is obtained by simplifying the Lie algebra properties, using the adjugate matrix properties and linearizing the BCH formula for the odometer constraint. The specific derivation process can refer to the above embodiments, and will not be repeated here.

[0075] The pose graph optimization method provided by the embodiments of the present application models the Lie algebra error as a zero-mean Gaussian distribution, and uses the adjugate matrix to propagate the covariance, establishing a probabilistic uncertainty evaluation framework strictly based on Lie group theory. The traditional deterministic odometer constraint is upgraded to a probabilistic constraint, providing accurate weight information for the back-end pose graph optimization and improving the estimation accuracy and theoretical rigor of the SLAM system.

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

[0077] Step S1231 acquires the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error; Step S1232 calculates the odometer covariance matrix corresponding to the odometer constraint according to the first covariance matrix, the second covariance matrix, the adjugate matrix of the inverse of the first nominal pose transformation matrix, and the cross-covariance matrix.

[0078] First, the cross-covariance matrix of the first Lie algebra error and the second Lie algebra error is acquired, including and , both have transpose symmetry, i.e., .

[0079] Then, the first covariance matrix , the second covariance matrix , the adjugate matrix of the inverse of the first nominal pose transformation matrix and the cross covariance matrix are substituted into the preset uncertainty propagation formula to obtain the odometer covariance matrix corresponding to the odometer constraint. Wherein, the preset uncertainty propagation formula is as follows:

[0080] Wherein, represents the odometer covariance matrix corresponding to the odometer constraint between the first key frame j and the second key frame k, represents the adjugate matrix of the inverse of the first nominal pose transformation matrix, represents the transpose of . The pose graph optimization method provided by the embodiment of the application solves the uncertainty estimation deviation problem caused by ignoring error correlation by further introducing the cross covariance matrix to evaluate the correlation between the pose errors, so that the uncertainty evaluation of the odometer constraint is more complete and accurate, and the robustness and reliability of the SLAM system in complex scenes are significantly enhanced.

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

[0082] Step S111, obtaining a first pose transformation matrix and a second pose transformation matrix of adjacent key frames.

[0083] For two adjacent key frames, denoted as a first key frame j and a second key frame k, a first pose transformation matrix and a second pose transformation matrix of adjacent key frames are obtained. Assuming that the first key frame j and the second key frame k are both in the i coordinate system, the pose transformation matrix corresponding to the first key frame j is denoted as the first pose transformation matrix , and the pose transformation matrix corresponding to the second key frame k is denoted as the second pose transformation matrix .

[0084] Step S112, calculating the inverse matrix of the first pose transformation matrix.

[0085] Then, the inverse matrix of the first pose transformation matrix is calculated, that is, .

[0086] Step S113, multiplying the inverse matrix with the second pose transformation matrix to obtain the odometer constraint between the adjacent key frames.

[0087] The inverse matrix is multiplied with the second pose transformation matrix to obtain the odometry constraint between adjacent key frames. Specifically, the following is obtained: ; wherein, represents the relative pose between the first key frame j and the second key frame k (i.e. the odometry constraint), represents the first pose transformation matrix of the first key frame j in the i coordinate system, represents the inverse matrix of , and represents the second pose transformation matrix of the second key frame k in the i coordinate system.

[0088] The inverse matrix of the first pose transformation matrix is multiplied with the second pose transformation matrix to obtain the odometry constraint between adjacent key frames in the pose graph optimization method provided by the embodiments of the present application. The relative pose relationship between adjacent key frames is represented by the inverse matrix and the multiplication operation, which can accurately construct the relative constraint of the odometry edge (i.e. the odometry constraint), and at the same time, conforms to the operation rules of Lie algebra SE(3), which is convenient for subsequent error modeling combined with Lie algebra, and further accurately constructs the uncertainty of the odometry constraint. Based on the accurately constructed odometry constraint and its uncertainty, the accuracy and robustness of the pose graph optimization can be improved.

[0089] Based on any of the above embodiments, step S140 comprises step S141 and step S142.

[0090] In step S141, the pose of the key frame is defined as a state variable.

[0091] The poses of all key frames are defined as state variables to be optimized. For example, assuming that there are n key frames, the state variable set X can be obtained as follows: X={T i1 , T i2 , …, T ij , T ik , …, T in}; wherein, T i1 , T i2 , T ij , T ik , T in represent the pose transformation matrices of the 1st, 2nd, jth, kth and nth key frames in the i coordinate system, respectively.

[0092] In step S142, a nonlinear least squares objective function with the objective of minimizing the weighted residual sum of squares is constructed according to the state variables, the odometry constraint, the odometry covariance matrix, the loop constraint and the loop covariance matrix.

[0093] An optimization objective of the pose graph optimization problem is to minimize a weighted residual sum of squares of all constraints, and a nonlinear least squares objective function can be constructed according to state variables, odometry constraints, an odometry covariance matrix, loop constraints and a loop covariance matrix As follows: ; Wherein, argmin() represents a minimum position function; represents a summation symbol; R represents an odometry constraint set, and an element (j, k) in the odometry constraint set represents an adjacent key frame pair that has an odometry constraint; L represents a loop constraint set, and an element (m, n) in the loop constraint set represents a non-continuous key frame pair that has a constraint established through loop detection; represents an odometry constraint residual vector, and is determined according to the odometry constraint; represents an odometry information matrix, and is obtained by performing an inverse operation on the odometry covariance matrix; is a square of Mahalanobis distance, and represents a weighted error of the odometry constraint; represents a loop constraint residual vector, and is determined according to the loop constraint; represents a loop information matrix, and is obtained by performing an inverse operation on the loop covariance matrix; is a square of Mahalanobis distance, and represents a weighted error of the odometry constraint.

[0094] In the above nonlinear least squares objective function, the information matrix of different constraints is introduced as a weight, so that the optimization process can intelligently distinguish different quality measurements, wherein a high-precision constraint will play a leading role in optimization, and a low-precision constraint has a smaller effect. Through the above manner, it can be ensured that the constraint with small uncertainty occupies a larger weight in the optimization, so as to solve in a more reliable direction in the optimization process, and the precision and accuracy of the final pose estimation can be improved.

[0095] The pose graph optimization method provided by the embodiment of the application integrates the odometry constraint and the loop constraint into one objective function in the above manner, so as to convert a physical pose graph optimization problem into a mathematically defined nonlinear least squares problem, and facilitate subsequent automatic solution.

[0096] Based on any one of the above embodiments, Figure 4 is a third flowchart of the pose graph optimization method provided by the application, as shown in Figure 4 The step of "constructing a global information matrix of the pose graph optimization problem" includes: In step S151, the nonlinear least squares objective function is linearized to calculate a first Jacobian matrix corresponding to the odometry constraint, a second Jacobian matrix corresponding to the loop constraint, and a global error vector.

[0097] Step S152, 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 added to the first address corresponding to the global information matrix; wherein the first local information matrix is obtained by inverting the odometry covariance matrix.

[0098] The global information matrix, also known as the Hessian matrix, is a huge sparse matrix with a dimension of (nd) x (nd), wherein n is the number of key frames, and d is the degree of freedom of each pose, and d is usually 6.

[0099] Before the optimization starts, the structure of the pose graph is analyzed in advance, and the memory address of each non-zero matrix block (each block is a 6x6 matrix) in the global information matrix H is calculated and stored. The address mapping table is a lookup table that pre-stores which specific memory address of the global information matrix H the calculation result of the Jacobian matrix of each constraint should be added to.

[0100] The odometry covariance matrix is inverted to obtain the information matrix of the odometry, denoted as the first local information matrix .

[0101] According to the first Jacobian matrix J jk and the first local information matrix , the first matrix block is calculated , and the matrix block of the odometry constraint is obtained, denoted as the first matrix block. The first matrix block is 12x12 in size and can be divided into four 6x6 sub-blocks. According to the pre-established address mapping table, these sub-blocks are directly added to the first address of the global information matrix H, wherein the first address is the 6x6 memory location corresponding to the key frames j and k calculated in advance.

[0102] In the above manner, the filling and addition operations can be directly performed on the pre-allocated sparse matrix structure, avoiding the dynamic allocation and release of a large number of small matrix blocks.

[0103] 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 added to the second address corresponding to the global information matrix; wherein the second local information matrix is obtained by inverting the loop covariance matrix.

[0104] Similarly, the loop covariance matrix is inverted to obtain the information matrix of the loop, denoted as the second local information matrix .

[0105] According to the second Jacobian matrix J mnand a second local information matrix , calculate , to obtain a matrix block of odometry constraints, denoted as a second matrix block. The second matrix block is 12x12 in size, which can be divided into four 6x6 sub-blocks. According to a pre-established address mapping table, these sub-blocks are directly accumulated to a second address of the global information matrix H, where the second address is a 6x6 memory location corresponding to the key frames m and n that is pre-calculated.

[0106] The pose graph optimization method provided by the embodiments of the present application stores the global information matrix by using a sparse data structure, which can significantly reduce memory occupation, so as to be able to run on an XR device with limited memory resources. Moreover, by utilizing the sparsity of the global information matrix and combining the direct memory address accumulation by using the address mapping table, invalid operations on large matrices and repeated memory applications in the optimization iteration process can be avoided, the memory management overhead can be greatly reduced, and the construction speed of the global information matrix can be improved, which is a key to realizing real-time pose graph optimization on an XR device.

[0107] Based on any of the above embodiments, the step of "optimizing and solving the global information matrix, and updating the poses of the key frames" includes steps S154, S155 and S156.

[0108] In step S154, a linear equation is constructed according to the global information matrix and the global error vector.

[0109] After the global information matrix H is constructed and the global error vector b is obtained, a linear equation is constructed according to the global information matrix H and the global error vector b. Specifically, the linear equation is as follows: H·ΔX=-b; Where ΔX represents the pose increment.

[0110] In step S155, the global information matrix is subjected to matrix square root decomposition to solve the linear equation, so as to obtain the optimal increment of the key frame pose.

[0111] It is considered that the direct inverse operation on the large global information matrix has a high calculation cost. It is analyzed that the global information matrix has good properties of sparsity, symmetry and positive definiteness, and therefore, a more efficient matrix square root decomposition method, for example, Cholesky decomposition, is adopted in the embodiments of the present application. By using the matrix square root method such as Cholesky decomposition, the symmetric positive definite property of the information matrix is fully utilized, and compared with the direct inverse operation, the solving speed and numerical stability can be improved while ensuring the solving accuracy.

[0112] Specifically, the global information matrix H is subjected to matrix square root decomposition, and H is decomposed into a product of a lower triangular matrix L and a transpose matrix of the lower triangular matrix L, specifically as follows: .

[0113] Then, the optimal increment ΔX is obtained by twice efficient triangular matrix solving, the optimal increment ΔX is a vector, ΔX={Δ , Δ , …, Δ }, wherein i represents the i coordinate system, 1, 2, …, n represent the first, second, …, n key frames, and each Δ corresponds to the Lie algebra error increment of a key frame pose.

[0114] In step S156, the pose of the key frame is updated according to the optimal increment.

[0115] The pose of the key frame is updated according to the optimal increment. That is, a small transformation composed of the increment Δ is right multiplied by the original pose to obtain the updated pose. For example, for the pose of the key frame j , the updated pose is = .

[0116] By calculating the increment in the Lie algebra space and using the exponential mapping update method, it can be ensured that the updated pose still satisfies the inherent manifold constraint, avoids the generation of illegal states caused by improper parameterization, and ensures the accuracy and correctness of the optimization process.

[0117] The pose graph optimization method provided by the embodiment of the application solves the linear equation by using the matrix square root decomposition method, compared with directly inverting the global information matrix, has better numerical stability while ensuring the solving accuracy, and can improve the solving speed.

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

[0119] In step S131, the first sensor data of the current frame and the second sensor data of the loop candidate frame are obtained.

[0120] The sensor data of the current frame is obtained and denoted as the first sensor data, and the sensor data of the loop candidate frame is obtained and denoted as the second sensor data.

[0121] The types of the first sensor data and the second sensor data include but are not limited to: GNSS data, point cloud data and image data. The GNSS data can be obtained by a GNSS module, the point cloud data can be obtained by a ToF sensor or a laser radar, and the image data can be obtained by an image sensor.

[0122] Further, the types of the first sensor data and the second sensor data can be determined according to the current scene. For example, if the current scene is an outdoor scene, it is determined that the first sensor data and the second sensor data are GNSS data; if the current scene is an indoor scene, it is determined that the first sensor data and the second sensor data are point cloud data or image data. It should be noted that in the indoor scene, the point cloud data is preferred, and if the point cloud data is sparse, the image data is used.

[0123] By intelligently selecting the most reliable sensor data according to the actual environment to construct the loop constraint and the loop covariance matrix corresponding thereto, the system can effectively detect and utilize the loop information in any scene, greatly enhancing the robustness and adaptability of the system to complex and variable environments.

[0124] In step S132, the loop constraint between the current frame and the loop candidate frame is calculated by matching the first sensor data and the second sensor data, and the loop covariance matrix is determined by evaluating the matching quality.

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

[0126] Further, if the first sensor data and the second sensor data are GNSS data, the loop constraint is a relative transformation between GNSS poses calculated according to the GNSS data, and the loop covariance matrix is determined according to the GNSS measurement variance.

[0127] If the first sensor data and the second sensor data are GNSS data, the GNSS data includes longitude, latitude, and height coordinates and GNSS accuracy information.

[0128] The longitude, latitude, and height coordinates of the current frame and the longitude, latitude, and height coordinates of the loop candidate frame are converted to the same local Cartesian coordinate system to obtain GNSS poses in the world coordinate system. The GNSS pose of the current frame is denoted as a first GNSS pose, and the GNSS pose of the loop candidate frame is denoted as a second GNSS pose.

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

[0130] Meanwhile, according to the GNSS accuracy information of the current frame, the GNSS measurement variance of the current frame is calculated, denoted as a first GNSS measurement variance, and the GNSS measurement variance is the square of the GNSS accuracy information. Similarly, according to the GNSS accuracy information of the loop candidate frame, the GNSS measurement variance of the loop candidate frame is calculated, denoted as a second GNSS measurement variance.

[0131] Then, according to the first GNSS measurement variance and the second GNSS measurement variance, a loop covariance matrix is constructed. The diagonal elements of the translation part of the loop covariance matrix are obtained according to the sum of the first GNSS measurement variance and the second GNSS measurement variance, the diagonal elements of the rotation part of the loop covariance matrix are set to a very large number, for example, 10 6 , 10 8 , and the covariance part (i.e. non-diagonal line) of the loop covariance matrix is set to 0.

[0132] Further, if the first sensor data and the second sensor data are point cloud data, the loop constraint is an optimal transformation matrix calculated according to the point cloud data by a point cloud registration algorithm, and the loop covariance matrix is determined according to a point cloud matching score of the point cloud registration algorithm.

[0133] If the first sensor data and the second sensor data are point cloud data, the point cloud data of the current frame is obtained, denoted as first point cloud data, and the point cloud data of the loop candidate frame is obtained, denoted as second point cloud data, and the first point cloud data and the second point cloud data are aligned by a point cloud matching algorithm to obtain an optimal transformation matrix as the loop constraint. Wherein, the point cloud matching algorithm can adopt ICP (Iterative Closest Point) algorithm.

[0134] After the ICP registration is completed, the point cloud matching score of the ICP algorithm is used to construct the loop covariance matrix. Wherein, the point cloud matching score is usually the root mean square error of the distance between all corresponding point pairs after registration. When constructing the loop covariance matrix, a simple mapping relationship can be established: the lower the point cloud matching score, that is, the better the matching, the smaller the variance term in the corresponding loop covariance matrix, that is, the more reliable the constraint. For example, the diagonal elements (representing the variance term) of the loop covariance matrix are set to be proportional to the square of the root mean square error.

[0135] Further, if the first sensor data and the second sensor data are image data, the loop constraint is a camera relative pose calculated according to the two-dimensional feature points matched from the image data and the corresponding three-dimensional landmark points, and the loop covariance matrix is determined according to the image re-projection error.

[0136] If the first sensor data and the second sensor data are image data, image data of the current frame is obtained and denoted as first image data, image data of the loop closure candidate frame is obtained and denoted as second image data, feature point matching is performed on the first image data and the second image data to obtain matched feature point pairs. In the feature point matching, an ORB (Oriented FAST and Rotated BRIEF, a feature extraction algorithm combining fast feature point detection and binary robust independent basic feature description), a SIFT (Scale-Invariant Feature Transform, scale invariant feature transformation) or the like can be used.

[0137] Then, for the two-dimensional feature point corresponding to the first image data in the feature point pair, a three-dimensional landmark point corresponding to the two-dimensional feature point is found from the map, a camera relative pose is calculated by a PnP (Perspective-n-Point, n-point perspective problem) solver, the camera relative pose is a relative pose of a camera of the current frame relative to a camera of the loop closure candidate frame, and the camera relative pose is taken as a loop closure constraint.

[0138] According to an image re-projection error generated by the PnP solver, a loop closure covariance matrix is determined. Specifically, after the camera relative pose is calculated, all three-dimensional landmark points participating in the PnP calculation are re-projected onto an image plane of the current frame according to the camera relative pose to obtain a set of theoretical two-dimensional projection points, denoted as theoretical two-dimensional projection points, a pixel distance between the theoretical two-dimensional projection points and actually observed two-dimensional feature points, i.e., an image re-projection error, is calculated. Then, diagonal elements of the loop closure covariance matrix are set to be proportional to squares of the image re-projection errors. The smaller the image re-projection error is, the smaller the element value of the loop closure covariance matrix is, and the higher the confidence of the constraint is.

[0139] The pose graph optimization method provided by the embodiments of the present application provides corresponding loop closure constraints and determination manners of loop closure covariance matrices for different types of sensor data, such as GNSS data, point cloud data and image data, and uses the above-mentioned accurately calculated loop closure constraints and loop closure covariance matrices for optimization of a global pose graph, so that accumulated drift errors of an XR device caused by long-time operation can be effectively eliminated, and the optimization result of the pose graph is improved.

[0140] The pose graph optimization device provided by the present application is described below, and the pose graph optimization device described below can be correspondingly referred to the pose graph optimization method described above.

[0141] Figure 5 is a structural schematic diagram of the pose graph optimization device provided by the present application, like Figure 5As shown, the device comprises a first calculation module 510, a second calculation module 520, a third calculation module 530, a problem construction module 540, and a pose updating module 550; wherein: The first calculation module 510 is configured to calculate the odometry constraint between adjacent key frames; The second calculation module 520 is configured to calculate the odometry covariance matrix corresponding to the odometry constraint through an uncertainty propagation model; wherein the uncertainty propagation model is obtained by deducing the odometry constraint formula through an analytical method; The third calculation module 530 is configured to calculate the loop constraint between the current frame and the loop candidate frame and the loop covariance matrix corresponding thereto; The problem construction module 540 is configured to construct a pose graph optimization problem according to the odometry constraint, the odometry covariance matrix, the loop constraint, and the loop covariance matrix; The pose updating module 550 is configured to assemble a global information matrix of the pose graph optimization problem, perform optimization solving on the global information matrix, and update the pose of the key frame.

[0142] The pose graph optimization device provided by the embodiment of the present application calculates the odometry constraint between adjacent key frames, and calculates the odometry covariance matrix corresponding to the odometry constraint through an uncertainty propagation model; wherein the uncertainty propagation model is obtained by deducing the odometry constraint formula through an analytical method. The uncertainty propagation model constructed based on the analytical method is used to calculate the odometry covariance matrix corresponding to the odometry constraint. Compared with the existing empirical setting and numerical approximation method, the embodiment of the present application realizes adaptive dynamic adjustment and online accurate estimation of the odometry constraint weight, and has higher precision. At the same time, the above-mentioned analytical method can greatly reduce the calculation overhead, so that it can be run in real time on the XR device side with limited computing power, realize low-delay and high-precision pose estimation, and improve the immersive experience. At the same time, the loop constraint between the current frame and the loop candidate frame and the loop covariance matrix corresponding thereto are calculated, then a pose graph optimization problem is constructed according to the odometry constraint, the odometry covariance matrix, the loop constraint, and the loop covariance matrix, a global information matrix of the pose graph optimization problem is assembled, optimization solving is performed on the global information matrix, and the pose of the key frame is updated. Since the odometry constraint and the loop constraint both use adaptive covariance matrices, the weight of the bad constraint will be naturally reduced during global optimization, so that it can show stronger generalization ability and robustness in complex environments.

[0143] It should be noted that the above-mentioned pose graph optimization device provided by the embodiment of the present application can realize all the method steps realized by the above-mentioned pose graph optimization method embodiment, and can achieve the same technical effects. The same parts and beneficial effects in this embodiment as the method embodiment will not be described in detail.

[0144] Figure 6 An example is a schematic diagram of the physical structure of an XR device, such as... Figure 6 As 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.

[0145] 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.

[0146] In another aspect, the present application also provides a computer program product comprising a computer program, which can be stored on a non-transitory computer readable storage medium, and the computer program can be executed by a processor to enable a computer to perform the pose graph optimization method provided by any of the above embodiments, which comprises: calculating a odometry constraint between adjacent keyframes; calculating a odometry covariance matrix corresponding to the odometry constraint by an uncertainty propagation model, wherein the uncertainty propagation model is derived by an analytical method on the odometry constraint formula; calculating a loop constraint between a current frame and a loop candidate frame and a loop covariance matrix corresponding to the loop constraint; constructing a pose graph optimization problem according to the odometry constraint, the odometry covariance matrix, the loop constraint and the loop covariance matrix; assembling a global information matrix of the pose graph optimization problem, optimizing and solving the global information matrix, and updating the pose of the keyframe.

[0147] In another aspect, the present application also provides a non-transitory computer readable storage medium having a computer program stored thereon, which can be executed by a processor to implement the pose graph optimization method provided by any of the above embodiments, which comprises: calculating a odometry constraint between adjacent keyframes; calculating a odometry covariance matrix corresponding to the odometry constraint by an uncertainty propagation model, wherein the uncertainty propagation model is derived by an analytical method on the odometry constraint formula; calculating a loop constraint between a current frame and a loop candidate frame and a loop covariance matrix corresponding to the loop constraint; constructing a pose graph optimization problem according to the odometry constraint, the odometry covariance matrix, the loop constraint and the loop covariance matrix; assembling a global information matrix of the pose graph optimization problem, optimizing and solving the global information matrix, and updating the pose of the keyframe.

[0148] The device embodiments described above are only schematic, wherein the units shown as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, i.e., can be located in one place or distributed on a plurality of network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the present embodiment. Those skilled in the art can understand and implement without creative labor.

[0149] Those skilled in the art can clearly understand the technical solutions of the various embodiments from the above description of the embodiments, and the various embodiments can be implemented by means of software with the necessary general hardware platforms, and of course, can also be implemented by hardware. Based on such understanding, the above technical solutions, essentially or in other words, the part of the prior art that makes a contribution, can be embodied in the form of a software product, which can be stored in a computer readable storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, and the like, and includes a number of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0150] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, rather than limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for some technical features therein; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

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 derived from the odometer constraint formula using analytical methods. 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. 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.

2. The pose graph optimization method according to claim 1, characterized in that, The uncertainty propagation model is obtained by simplifying the odometer constraint formula using Lie algebra properties, adjoint matrix properties, and linearizing it using the Beck-Campbell-Hausdorff (BCH) formula. The calculation of 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; 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.

3. The pose graph optimization method according to claim 2, characterized in that, 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: 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.

4. 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.

5. 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.

6. The pose graph optimization method according to claim 5, 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.

7. The pose graph optimization method according to claim 6, 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.

8. The pose graph optimization method according to any one of claims 1 to 7, 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.

9. 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 derived from the odometer constraint formula using an analytical method. 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, optimize and solve the global information matrix, and update the pose of the keyframes.

10. 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 8.

Images