Visual-Inertial Fusion Pose Estimation Method and Device Based on Composite Manifold Optimization

Abstract

The present invention discloses a visual-inertial fusion pose estimation method and device based on composite manifold optimization. The method includes: acquiring real-time image data through an image acquisition module of a device to be estimated and acquiring inertial measurement data through a sensing module of the device to be estimated; calculating image observation data based on a preset observation projection model corresponding to the image acquisition module according to the real-time image data; calculating inertial navigation pre-integrated data based on a pre-established inertial navigation measurement and dynamic model according to the inertial measurement data; calculating pose data of the device to be estimated based on a pre-established pose estimation framework optimized by a composite fully decoupled manifold <SO(3),R<supgt;3< / supgt;,R<supgt;3< / supgt>> according to the image observation data and the inertial navigation pre-integrated data. It can be seen that implementing the present invention can perform composite manifold optimization on the pose of the device to be estimated, improve the calculation efficiency of the visual-inertial fusion optimization algorithm, and maintain the consistency of inertial navigation pre-integration.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular, to a visual-inertial fusion pose estimation method and device based on composite manifold optimization. Background Art

[0002] Environmental perception and self-motion perception play crucial roles in the navigation process. The motion perception of a robot relies on navigation and positioning technologies. These technologies include inertial reckoning methods (such as inertial sensors, pedometers, wheel encoders, magnetometers, and gyroscopes, etc.) and relative positioning methods (such as Global Navigation Satellite System (GNSS), Ultra-Wideband (UWB), acoustic ranging, lidar, and cameras, etc.). In the field of robot navigation, visual-inertial fusion navigation can provide the current state of the robot and a local map, which is crucial for path planning, obstacle avoidance, and real-time control. As a widely used navigation technology, visual-inertial fusion navigation has made significant contributions to the Guidance, Navigation, and Control System (GNC). In particular, since visual-inertial fusion navigation relies on the use of on-board sensors (cameras and inertial navigation), this perception mode is particularly important in the absence of external positioning means such as GPS, Ultra-Wideband (UWB), and visual motion capture systems. Therefore, visual-inertial fusion navigation is widely applied in indoor positioning (AR and XR), underwater exploration, urban reconstruction, and search and rescue missions.

[0003] Currently, the methods for real-time obtaining the pose state of a robot mainly adopt a velocity-decoupled chained rotation-translation manifold, which can only achieve real-time state estimation in a relatively short fixed lag time window and with fewer feature points. Summary of the Invention

[0004] The present invention provides a visual-inertial fusion pose estimation method and device based on composite manifold optimization, which can achieve longer time window optimization under limited computing resources, reserve more sufficient processing time for the visual front-end, improve the computational efficiency of the visual-inertial fusion optimization algorithm, and maintain the consistency of inertial pre-integration.

[0005] To solve the above technical problems, in a first aspect, the present invention discloses a visual-inertial fusion pose estimation method based on composite manifold optimization, the method comprising:

[0006] Acquiring real-time image data through an image acquisition module of the device to be estimated and acquiring inertial measurement data through a sensing module of the device to be estimated;

[0007] Based on a preset observation projection model corresponding to the image acquisition module, calculating image observation data according to the real-time image data;

[0008] Based on a pre-established inertial navigation measurement and dynamic model, calculate inertial navigation pre-integrated data according to the inertial measurement data;

[0009] Based on a pre-established composite fully decoupled manifold optimization pose estimation framework, calculate the pose data of the device to be estimated according to the image observation data and the inertial navigation pre-integrated data.

[0010] As an alternative implementation, the observation projection model is established through the following steps:

[0011] Represent the surrounding space environment information of the device to be estimated by a three-dimensional point cloud;

[0012] Extract feature points from the real-time image data acquired by the image acquisition module;

[0013] Convert the three-dimensional coordinates of the feature points to the coordinate system of the image acquisition module and project them onto the image pixel coordinates;

[0014] Combined with the Kannala-Brandt camera distortion parameters, substitute the homogeneous coordinates of the coordinate system of the image acquisition module into the Kannala-Brandt camera distortion parameters to obtain the observation projection model.

[0015] As an alternative implementation, the inertial measurement data includes angular velocity data and acceleration data.

[0016] As an alternative implementation, the inertial navigation measurement and dynamic model is established through the following steps:

[0017] The inertial navigation measurement and dynamic model includes measured angular velocity and measured acceleration. Based on the inertial measurement data, the angular velocity in the true inertial navigation coordinate system is superimposed with the angular velocity offset and angular velocity noise to obtain the measured angular velocity;

[0018] Multiply the rotation matrix from the world coordinate system to the inertial navigation coordinate system by the product of the true acceleration from the world coordinate system to the inertial coordinate system and the gravitational acceleration, and superimpose the acceleration offset and acceleration noise to obtain the measured acceleration.

[0019] As an alternative implementation, the composite fully decoupled manifold optimization pose estimation framework is established through the following steps:

[0020] Define the increase and decrease form of the composite fully decoupled manifold;

[0021] Establish the camera feature point reprojection error based on the inverse depth stereo projection three-dimensional coordinates, and provide the corresponding Jacobian derivative matrix of the composite fully decoupled manifold;

[0022] Establish a propagation model for the inertial navigation state and the uncertainty of the manifold based on inertial navigation pre-integration, determine the numerical operation method of the inertial navigation pre-integration, and provide the corresponding Jacobian matrix of the manifold;

[0023] Construct an uncertainty-weighted visual reprojection error term;

[0024] Construct an uncertainty-weighted inertial navigation pre-integration error term, and provide the corresponding composite fully decoupled Jacobian derivative matrix of the manifold.

[0025] As an alternative implementation, for the pose estimation framework based on the pre-established composite fully decoupled manifold optimization, the steps of calculating the pose data of the device to be estimated according to the image observation data and the inertial navigation pre-integration data include:

[0026] Determine the minimized energy function;

[0027] Based on the minimized energy function, the composite fully decoupled pose estimation framework of manifold optimization, and the iterative optimization algorithm, optimize and calculate the pose data of the device to be estimated according to the image observation data and the inertial navigation pre-integration data under a fixed lag window.

[0028] As an alternative implementation, the iterative optimization algorithm is the Levenberg-Marquardt iterative optimization method.

[0029] The second aspect of the present invention discloses a visual-inertial fusion pose estimation device based on composite manifold optimization, and the device includes:

[0030] An acquisition module, configured to acquire real-time image data through an image acquisition module of the device to be estimated and acquire inertial measurement data through a sensing module of the device to be estimated;

[0031] A first calculation module, configured to calculate image observation data according to the real-time image data based on a preset observation projection model corresponding to the image acquisition module;

[0032] A second calculation module, configured to calculate inertial navigation pre-integration data according to the inertial measurement data based on a pre-established inertial navigation measurement and dynamic model;

[0033] An estimation module, configured to calculate the pose data of the device to be estimated according to the image observation data and the inertial navigation pre-integration data based on a pre-established composite fully decoupled pose estimation framework of manifold optimization.​

[0034] The third aspect of the present invention discloses another visual inertial fusion pose estimation device based on composite manifold optimization, and the device includes:

[0035] A memory storing executable program code;

[0036] A processor coupled to the memory;

[0037] The processor calls the executable program code stored in the memory and executes the visual inertial fusion pose estimation method based on composite manifold optimization disclosed in the first aspect of the present invention.

[0038] The fourth aspect of the present invention discloses a computer storage medium, which stores computer instructions that, when called, are used to execute the visual inertial fusion pose estimation method based on composite manifold optimization disclosed in the first aspect of the present invention.

[0039] Compared with the prior art, the embodiments of the present invention have the following beneficial effects:

[0040] In the embodiments of the present invention, real-time image data is acquired through an image acquisition module of the device to be estimated, and inertial measurement data is acquired through a sensing module of the device to be estimated; based on a preset observation projection model corresponding to the image acquisition module, image observation data is calculated according to the real-time image data; based on a pre-established inertial navigation measurement and dynamic model, inertial navigation pre-integrated data is calculated according to the inertial measurement data; based on a pre-established composite fully decoupled manifold optimization pose estimation framework, the pose data of the device to be estimated is calculated according to the image observation data and the inertial navigation pre-integrated data. It can be seen that implementing the present invention can perform composite manifold optimization on the pose of the device to be estimated, achieve longer time window optimization under limited computing resources, reserve more sufficient processing time for the visual front end, improve the calculation efficiency of the visual inertial fusion optimization algorithm, and maintain the consistency of inertial navigation pre-integration. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 is a schematic flow chart of a visual inertial fusion pose estimation method based on composite manifold optimization disclosed in an embodiment of the present invention;

[0042] Figure 2 is a schematic structural diagram of a visual inertial fusion pose estimation device based on composite manifold optimization disclosed in an embodiment of the present invention;

[0043] Figure 3 is a schematic structural diagram of another visual inertial fusion pose estimation device based on composite manifold optimization disclosed in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0044] To enable those skilled in the art to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0045] The terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish different objects, rather than to describe a specific order. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, device, product or terminal comprising a series of steps or units is not limited to the listed steps or units, but optionally further includes steps or units not listed, or optionally further includes other steps or units inherent to these processes, methods, products or terminals.

[0046] Referring to "embodiment" herein means that a specific feature, structure or characteristic described in connection with the embodiment may be included in at least one embodiment of the present invention. The phrase appears in various places in the specification and does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments.

[0047] Those skilled in the art explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0048] The present invention discloses a method and device for visual inertial fusion pose estimation based on composite manifold optimization, which can perform composite manifold optimization on the pose of the device to be estimated, achieve longer time window optimization under limited computing resources, reserve more sufficient processing time for the visual front end, improve the computational efficiency of the visual inertial fusion optimization algorithm and maintain the consistency of inertial pre-integration. The following will be described in detail respectively.

[0049] Embodiment 1

[0050] Please refer to Figure 1 , Figure 1 which is a schematic flowchart of a method for visual inertial fusion pose estimation based on composite manifold optimization disclosed in an embodiment of the present invention. Among them, Figure 1 The described method for visual inertial fusion pose estimation based on composite manifold optimization can be applied to a device for visual inertial fusion pose estimation based on composite manifold optimization, wherein the pose estimation based on composite manifold optimization can be integrated in a local server or a cloud server, and the embodiments of the present invention do not make any limitations. As Figure 1As shown in the figure, the visual-inertial fusion pose estimation method based on composite manifold optimization may include the following operations:

[0051] 101. Obtain real-time image data through the image acquisition module of the device to be estimated and obtain inertial measurement data through the sensing module of the device to be estimated.

[0052] In an embodiment of the present invention, the image acquisition module includes an imaging device capable of detecting visible light, infrared light, or ultraviolet light. Correspondingly, the real-time image data includes visible light, infrared light, or ultraviolet light imaging of the photographed target object obtained by the image acquisition module. The sensing module obtains relevant measurement data of the acceleration and angular velocity of the target object.

[0053] For ease of understanding, the image acquisition module in this embodiment selects a terminal camera, and the image data is an image captured by the terminal camera.

[0054] 102. Based on the preset observation projection model corresponding to the image acquisition module, calculate the image observation data according to the real-time image data.

[0055] The observation projection model is established through the following steps:

[0056] Represent the surrounding space environment information of the device to be estimated by a three-dimensional point cloud;

[0057] Extract feature points from the real-time image data acquired by the image acquisition module;

[0058] Convert the three-dimensional coordinates of the feature points to the coordinate system of the image acquisition module and project them onto the image pixel coordinates;

[0059] Combined with the Kannala-Brandt camera distortion parameters, substitute the homogeneous coordinates of the image acquisition module coordinate system into the Kannala-Brandt camera distortion parameters to obtain the observation projection model.

[0060] Among them, the FAST corner detection algorithm is used to extract feature points. The full name of the FAST corner detection algorithm is Features From Accelerated Segment Test. Since it does not involve complex operations such as scale and gradient, the FAST corner detection algorithm has a very fast detection speed. It judges whether a pixel is a corner by comparing the gray value of the pixels within a certain neighborhood with the center point. If a certain pixel differs greatly from enough pixel points in its surrounding neighborhood, then this pixel may be a corner.

[0061] The two-dimensional feature point tracking of the image is implemented by using the Kanade-Lucas-Tomasi algorithm, which is abbreviated as the KLT algorithm. It is an object tracking algorithm based on feature points. By selecting feature points in the video sequence and using the optical flow method to calculate the motion trajectories of these feature points between consecutive frames, object tracking is achieved.

[0062] The feature points extracted by the FAST corner detection algorithm are rotated and translated in the three-dimensional point cloud coordinate system to the terminal camera coordinate system, and then projected onto the image pixel coordinates. The Kannala-Brandt camera distortion projection model is used to adapt to the wide-angle lens of the terminal camera. The fourth-order model of this projection model is as follows in formula (1):

[0063]

[0064] Among them, among is the camera focal length and the optical center coordinates, are the fourth-order Kannala-Brandt camera distortion parameters, which can be obtained through offline calibration of the camera internal parameters, is the Kannala-Brandt camera projection function, is the homogeneous coordinate of the three-dimensional point in the current camera coordinate system, where is the unit azimuth vector of the three-dimensional point, is the inverse depth of the three-dimensional point.

[0065] 103. Based on the pre-established inertial navigation measurement and dynamic model, according to the inertial measurement data, the inertial navigation pre-integration data is calculated.

[0066] The inertial navigation measurement and dynamic model is established through the following steps:

[0067] The inertial navigation measurement and dynamic model includes measuring angular velocity and measuring acceleration. Based on the inertial measurement data, the angular velocity in the true inertial navigation coordinate system is superimposed with the angular velocity offset and angular velocity noise to obtain the measured angular velocity;

[0068] The product of the rotation matrix from the world coordinate system to the inertial navigation coordinate system and the true acceleration from the world coordinate system to the inertial coordinate system and the gravitational acceleration is superimposed with the acceleration offset and acceleration noise to obtain the measured acceleration.

[0069] As can be seen from the above, the inertial measurement data includes angular velocity data and acceleration data. The inertial navigation model measurement includes the three-axis angular velocity measurement of the gyroscope and the three-axis linear acceleration measurement of the accelerometer. The measurement model of this inertial navigation is as follows in formula (2):

[0070]

[0071] Where They are for measuring angular velocity and acceleration respectively, is the angular velocity in the true inertial navigation coordinate system, are the true acceleration and gravitational acceleration in the world coordinate system, is the rotation matrix from the world coordinate system to the inertial navigation coordinate system, are the angular velocity and acceleration offsets respectively, are the angular velocity and acceleration measurement noises. The noise-free discrete dynamic model for inertial navigation state propagation is as follows in Equation (3):

[0072]

[0073] where and are respectively and the translation, velocity and rotation at the time of two image frames, is the time interval between two image frames, is the inertial navigation predicted "pseudo-measurement value" of the frame-to-frame from to moments. In continuous time, it can be expressed as Equation (4):

[0074]

[0075] 104. Based on the pre-established composite fully decoupled pose estimation framework of manifold optimization, calculate the pose data of the device to be estimated according to the image observation data and inertial navigation pre-integration data.

[0076] The composite fully decoupled pose estimation framework of manifold optimization is established through the following steps:

[0077] Define the increase and decrease form of the composite fully decoupled manifold;

[0078] Establish the camera feature point reprojection error based on the inverse depth stereo projection three-dimensional coordinates, and provide the corresponding Jacobian derivative matrix of the composite fully decoupled manifold;

[0079] Establish the propagation model of the inertial navigation state and the uncertainty of the manifold based on inertial navigation pre-integration, determine the numerical operation method of inertial navigation pre-integration, and provide the corresponding Jacobian matrix of the manifold;

[0080] Construct the uncertainty-weighted visual reprojection error term;

[0081] Construct the uncertainty-weighted inertial navigation pre-integration error term, and provide the corresponding composite fully decoupled Jacobian derivative matrix of the manifold.

[0082] Among them, The increase and decrease of the manifold are different from the manifold representation forms of rotation, translation, and velocity estimated by existing visual inertial odometers: Velocity-decoupled chained rotation-translation manifold, Velocity-decoupled synchronous rotation-translation manifold, Synchronous rotation-translation and velocity manifold. As the synchronous rotation-translation and velocity manifold, it can be expressed as formula (5):

[0083]

[0084] Among them is the rotation matrix, is the velocity, is the translation, is the increment, , and respectively represent the increments of rotation, velocity, and translation.

[0085] Define The increase and decrease operations on the manifold can be expressed as formula (6) on the manifold:

[0086]

[0087] Similarly, the increase and decrease operations on the manifold can be expressed as formula (7) on the manifold:

[0088]

[0089] The increase and decrease operations on the manifold can be expressed as formula (8) on the manifold:

[0090]

[0091] Among them the increase and decrease operations of the manifold can be expressed as formula (9):

[0092]

[0093] It can be found that the increase and decrease of the manifold have fewer operation operations. In formulas (6), (7), (8), and (9), and are the exponential and logarithmic mappings for the rotation matrix respectively. The exponential mapping is formula (10):

[0094]

[0095] wherein R^3;

[0096]

[0097] The logarithmic mapping is given by Equation (12):

[0098]

[0099] wherein

[0100]

[0101] In Equations (6), (7), (8), and (9), and is the left Jacobian matrix and its inverse matrix of

[0102]

[0103] Furthermore, in the step of establishing the reprojection error of camera feature points based on the inverse depth stereographic projection three-dimensional coordinates, the reprojection error of a single three-dimensional feature point can be expressed as the residual between the feature observation pixel coordinates and its projection on the target frame , and the variables involved in this residual are the pose of the inertial navigation of the target frame and the pose of the inertial navigation of the frame where the three-dimensional feature point is located . Both the target frame and the frame where the three-dimensional feature point is located (the image frame where this three-dimensional feature point is first obtained through triangulation) observe this three-dimensional feature point simultaneously, and the three-dimensional feature points in the embodiments of the present invention are expressed in the parametric form of stereographic projection . This parametric form can achieve unconstrained optimization for the three-dimensional feature points. The residual function is Equation (15):

[0104]

[0105] wherein is the projection function that projects the three-dimensional feature point in the homogeneous coordinate system onto the pixel coordinates on the target frame. See Equation (1) for details is the pixel position of the two-dimensional feature point of this three-dimensional feature point on the target frame are the fixed extrinsic parameters of the inertial navigation and the camera of the frame where it is located and the target frame respectively is the stereo back-projection function that maps the stereo parametric form of the three-dimensional feature point on the target frame to its equivalent homogeneous parametric form with inverse depth , as shown in Equation (16) below:

[0106]

[0107] The specific conversion from the stereoscopic parametric form to the homogeneous parametric form is as follows:

[0108]

[0109] According to the residual function (15) of the reprojection error of a single three-dimensional feature point and the chain rule of differentiation, the derivative Jacobian of this residual term with respect to the variables involved in this residual can be obtained as formula (18):

[0110]

[0111] Where is the Jacobian matrix of the reprojection residual with respect to the inertial poses of the target frame and the current frame, is the Jacobian matrix of the reprojection residual with respect to the stereoscopically parameterized three-dimensional feature points. Among them, for the derivation of the intermediate term, in the embodiments of the present invention, due to the adoption of a composite fully decoupled manifold, the following theorem (19) can be obtained:

[0112]

[0113] Applying theorem (19) to formula (18), we can obtain and Jacobian matrices.

[0114] Furthermore, in the step of establishing the inertial navigation state and its uncertainty propagation model based on inertial navigation pre-integration, the inertial navigation pre-integration measurement can be obtained through numerical calculation according to specific assumptions. The present invention assumes that the angular velocity measurement value in the inertial navigation measurement interval is a constant, and the acceleration in the world coordinate system is a constant. The present invention uses the semi-rotation Euler discretization to discretize this integral term. Define to the rotations (5a) and semi-rotations (5b) between two inertial navigation measurement times as:

[0115]

[0116]

[0117] Meanwhile, it is also assumed that the angular velocity deviation from to and the acceleration deviation between two image frame times remain unchanged. From this, the deviation from to The discrete iterative formula (6) of inertial navigation pre-integration, where is the inertial navigation measurement time between two image frame times, where the measurement frequency of inertial navigation (about 200 Hz) is much greater than the image measurement frequency (about 30 Hz), .

[0118]

[0119] Where and are respectively the translation, velocity, and rotation terms of the inertial navigation pre-integration term at and the inertial navigation measurement time. and are respectively the angular velocity and acceleration measurement values from to time. The initial value of inertial navigation pre-integration is zero, , , .

[0120] To propagate the uncertainty of inertial navigation pre-integration, the increment state of pre-integration is defined as:

[0121]

[0122] Using manifold, the discrete-time linearized pre-integration increment state dynamics of "small signal propagation" is defined as:

[0123]

[0124] Then, the pre-integration error covariance matrix can be iteratively expressed as

[0125]

[0126] Where are respectively and the pre-integration error covariance matrices at time, and its initial state is zero . Among them, the increment state Jacobian matrix can be expressed as:

[0127]

[0128] The present invention adopts semi-rotational inertial navigation pre-integration, and thus defines the semi-rotational acceleration as

[0129]

[0130] Thus, it can be obtained that

[0131]

[0132] Substituting (27) into (25), the Jacobian matrix can be obtained . In addition, the accelerometer and the gyroscope measure the Jacobian matrix as follows:

[0133]

[0134] where .

[0135] During the inertial navigation pre-integration process, the bias is set to a constant, which can avoid recalculating the IMU pre-integration pseudo-measurement terms. The small change in the bias can be expressed as the increment at the linearization point:

[0136]

[0137] These small bias changes act on the inertial navigation pre-integration from to , and these bias increments can be approximated as the first-order expansion of the pre-integration pseudo-measurement:

[0138]

[0139] The iterative calculation of the constant bias Jacobian matrix during inertial navigation pre-integration can be expressed as:

[0140]

[0141] where exists in after the iterative operation of (31):

[0142]

[0143] Furthermore, in the step of constructing the uncertainty-weighted visual reprojection error term, the weighted sum of the residuals (15) is calculated by computing the reciprocal of the observation covariance to obtain the reprojection error term:

[0144]

[0145] where is the covariance of the feature observation in pixels. In the embodiments of the present invention, it is assumed that the observation noises in the horizontal and vertical axes directions in the image plane are independent and have the same standard deviation , then . To improve the robustness against dynamic feature points, the present invention also adopts the Cauchy robust function to reduce the influence of abnormal feature points, such as dynamic feature points, on the optimization.

[0146] Furthermore, in the step of constructing the uncertainty-weighted inertial navigation pre-integration error term, for an inertial navigation pre-integration residual, the optimization mainly involves the two states at and moments:

[0147]

[0148] where each state consists of rotation, translation, velocity, and bias:

[0149]

[0150] The present invention defines the inertial navigation pre-integration residual with a fixed inertial navigation bias as:

[0151]

[0152] where , is the first-order expansion term (30).

[0153] Define the Jacobian matrix of the inertial navigation pre-integration residual as:

[0154]

[0155] where,

[0156]

[0157] The present invention adopts a composite fully decoupled manifold, and the specific Jacobian matrix can be obtained as follows:

[0158]

[0159] And it can be obtained that:

[0160]

[0161]

[0162] The overall Jacobian matrix of the state at the subsequent moment is expressed as:

[0163]

[0164] The inertial navigation pre-integration error is the weighted sum of the residuals (36), where the weighting matrix is the reciprocal of the propagation covariance:

[0165]

[0166] where is the covariance matrix of the inertial navigation pre-integration propagation:

[0167]

[0168] Among them is the covariance matrix of rotation, velocity and translation obtained through (24), is the covariance matrix of the deviation assuming Brownian motion. Given the continuous-time inertial navigation deviation random walk standard deviation obtained through inertial navigation calibration and , the discrete-time deviation covariance is and .

[0169] Furthermore, based on the pre-established composite fully decoupled manifold optimization pose estimation framework, the steps of calculating the pose data of the device to be estimated according to the image observation data and inertial navigation pre-integration data include:

[0170] Determine the energy function to be minimized;

[0171] Based on the energy function to be minimized, the composite fully decoupled manifold optimization pose estimation framework, and the iterative optimization algorithm, optimize and calculate the pose data of the device to be estimated according to the image observation data and inertial navigation pre-integration data under a fixed lag window.

[0172] Preferably, in this embodiment, the iterative optimization algorithm is the Levenberg-Marquardt iterative optimization method.

[0173] The manifold optimization in visual inertial odometry is a non-linear least squares problem, which can be expressed as minimizing the energy function :

[0174]

[0175] Among them is the state involved in the optimization in visual inertial odometry, including rotation, velocity, translation, and the positions of three-dimensional feature points within the sliding window. represents the measurement value in visual inertial odometry, which can be the visual feature pixel position on the image plane or the inertial navigation pre-integration pseudo-measurement value between two image frames. represents the covariance matrix, and its inverse represents the weighting matrix of the weighted least squares (LS) problem. Given the linearization point , the non-linear model can be linearized and approximated as:

[0176]

[0177] Among them is the function The Jacobian matrix at the linearization point. is the increment between the current state and the linearization point. Now we linearly approximate the energy function as,

[0178]

[0179] where the residuals and the Jacobian matrix are weighted in square root form:

[0180]

[0181] Adding a damping term (Levenberg - Marquardt), the LM optimal step size can be obtained by minimizing the damped :

[0182]

[0183] Embodiment 2

[0184] Please refer to Figure 2 , Figure 2 which is a schematic structural diagram of a visual - inertial fusion pose estimation device based on composite manifold optimization disclosed in an embodiment of the present invention. As Figure 2 shown, the visual - inertial fusion pose estimation device based on composite manifold optimization may include:

[0185] An acquisition module 201, configured to acquire real - time image data through an image acquisition module of a device to be estimated and inertial measurement data through a sensing module of the device to be estimated;

[0186] A first calculation module 202, configured to calculate image observation data based on a preset observation projection model corresponding to the image acquisition module according to the real - time image data;

[0187] A second calculation module 203, configured to calculate inertial pre - integration data based on a pre - established inertial navigation measurement and dynamic model according to the inertial measurement data;

[0188] An estimation module 204, configured to calculate pose data of the device to be estimated based on a pre - established composite fully - decoupled manifold - optimized pose estimation framework according to the image observation data and the inertial pre - integration data.

[0189] It can be seen that the implementation Figure 2The described device can acquire real-time image data and inertial measurement data through the sensing module of the device to be estimated. Based on the real-time image data, it calculates image observation data, and based on the inertial measurement data, it calculates inertial navigation pre-integrated data. Based on the image observation data and the inertial navigation pre-integrated data, it calculates the pose data of the device to be estimated, performs composite manifold optimization on the pose of the device to be estimated, realizes obtaining a longer time window optimization under limited computing resources, reserves more sufficient processing time for the visual front-end, improves the calculation efficiency of the visual-inertial fusion optimization algorithm, and maintains the consistency of the inertial navigation pre-integration.

[0190] Specifically, for the details and technical advantages of the module function steps in the above device, reference can be made to the corresponding descriptions in Embodiment 1, and details will not be elaborated herein.

[0191] Embodiment 3

[0192] Please refer to Figure 3 , Figure 3 which is a schematic structural diagram of another visual-inertial fusion pose estimation device based on composite manifold optimization disclosed in the embodiments of the present invention. As Figure 3 shown, the visual-inertial fusion pose estimation device based on composite manifold optimization may include:

[0193] A memory 301 storing executable program code;

[0194] A processor 302 coupled to the memory;

[0195] The processor 302 calls the executable program code stored in the memory 301 and executes the steps in the visual-inertial fusion pose estimation method based on composite manifold optimization described in Embodiment 1 of the present invention.

[0196] Embodiment 4

[0197] The embodiments of the present invention disclose a computer storage medium. When the computer instructions stored in this computer storage medium are called, they are used to execute the steps in the visual-inertial fusion pose estimation method based on composite manifold optimization described in Embodiment 1 of the present invention.

[0198] Embodiment 5

[0199] The embodiments of the present invention disclose a computer program product. This computer program product includes a non-transitory computer-readable storage medium storing a computer program, and this computer program is operable to cause a computer to execute the steps in the visual-inertial fusion pose estimation method based on composite manifold optimization described in Embodiment 1.

[0200] The device embodiments described above are merely illustrative. The modules described as separate components may or may not be physically separated. The components shown as modules may or may not be physical modules, that is, they may be located in one place or distributed to multiple network modules. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. A person of ordinary skill in the art can understand and implement it without creative effort.

[0201] Through the above specific descriptions of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, and of course also by hardware. Based on this understanding, the above technical solution, 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, and the storage medium includes read-only memory (ROM), random access memory (RAM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), one-time programmable read-only memory (OTPROM), electrically-erasable programmable read-only memory (EEPROM), compact disc read-only memory (CD-ROM) or other optical disc memories, magnetic disk memories, tape memories, or any other computer-readable medium that can be used to carry or store data.

[0202] Finally, it should be noted that: The method and device for visual inertial fusion pose estimation based on composite manifold optimization disclosed in the embodiments of the present invention only disclose the preferred embodiments of the present invention, and are only used to illustrate the technical solutions of the present invention, rather than limiting them; Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; 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 invention.

Claims

1. A visual inertial navigation fusion pose estimation method based on composite manifold optimization, characterized in that: The method comprises: Acquiring real-time image data through an image acquisition module of the device to be estimated and acquiring inertial measurement data through a sensor module of the device to be estimated; Based on the preset observation projection model corresponding to the image acquisition module, the image observation data is calculated according to the real-time image data; Based on the pre-established inertial navigation measurement and dynamic model, the inertial navigation pre-integrated data is calculated according to the inertial measurement data; Based on pre-built composite full decoupling A manifold-optimized pose estimation framework is used to calculate the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integrated data; The composite full decoupling The pose estimation framework of manifold optimization is established through the following steps: Defining Composite Full Decoupling The increasing and decreasing forms of manifolds; Establish the camera feature point reprojection error based on the inverse depth stereo projection 3D coordinates and provide the corresponding composite full decoupling Jacobian derivative matrix of manifold; Establish the inertial navigation state based on inertial navigation pre-integration and The uncertainty propagation model of the manifold is determined, and the numerical calculation method of the inertial navigation pre-integration is determined, and the corresponding Jacobian matrix of the manifold; Construct uncertainty-weighted visual reprojection error terms; Construct uncertainty-weighted inertial navigation pre-integrated error terms to provide corresponding composite full decoupling Jacobian derivative matrix of the manifold.

2. The method for posture estimation according to claim 1, characterized in that: The observation projection model is established by the following steps: Representing the surrounding spatial environment information of the device to be estimated by a three-dimensional point cloud; Extracting feature points from the real-time image data acquired by the image acquisition module; Convert the three-dimensional coordinates of the feature points to the image acquisition module coordinate system, and project the image to the image pixel coordinates; Combined with the Kannala-Brandt camera distortion parameters, the homogeneous coordinates of the image acquisition module coordinate system are substituted into the Kannala-Brandt camera distortion parameters to obtain the observation projection model.

3. The method for posture estimation according to claim 1, characterized in that: The inertial measurement data includes angular velocity data and acceleration data.

4. The method for posture estimation according to claim 1, characterized in that: The inertial navigation measurement and dynamic model are established by the following steps: The inertial navigation measurement and dynamic model includes measuring angular velocity and measuring acceleration. Based on the inertial measurement data, the angular velocity in the real inertial navigation coordinate system is superimposed with the angular velocity offset and the angular velocity noise to obtain the measured angular velocity; The measured acceleration is obtained by multiplying the rotation matrix from the world coordinate system to the inertial coordinate system, the real acceleration from the world coordinate system to the inertial coordinate system and the gravitational acceleration, and superimposing the acceleration offset and acceleration noise.

5. The method for posture estimation according to claim 1, characterized in that: The pre-built composite full decoupling The manifold-optimized pose estimation framework calculates the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integrated data, comprising: Determine the minimized energy function; Based on the minimized energy function, the composite full decoupling A manifold-optimized pose estimation framework and an iterative optimization algorithm optimize the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integrated data under a fixed lag window.

6. The method for posture estimation according to claim 5, characterized in that: The iterative optimization algorithm is the Levenberg-Marquardt iterative optimization method.

7. A visual inertial navigation fusion pose estimation device based on composite manifold optimization, characterized in that: The device comprises: An acquisition module, used to acquire real-time image data through an image acquisition module of the device to be estimated and to acquire inertial measurement data through a sensor module of the device to be estimated; A first calculation module, configured to calculate image observation data according to the real-time image data based on a preset observation projection model corresponding to the image acquisition module; A second calculation module is used to calculate the inertial navigation pre-integrated data according to the inertial navigation measurement data based on the pre-established inertial navigation measurement and dynamic model; Estimation module for full decoupling of complex A manifold-optimized pose estimation framework is used to calculate the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integrated data; The composite full decoupling The pose estimation framework of manifold optimization is established through the following steps: Defining Composite Full Decoupling The increasing and decreasing forms of manifolds; Establish the camera feature point reprojection error based on the inverse depth stereo projection 3D coordinates and provide the corresponding composite full decoupling Jacobian derivative matrix of manifold; Establish the inertial navigation state based on inertial navigation pre-integration and The uncertainty propagation model of the manifold is determined, and the numerical calculation method of the inertial navigation pre-integration is determined, and the corresponding Jacobian matrix of the manifold; Construct uncertainty-weighted visual reprojection error terms; Construct uncertainty-weighted inertial navigation pre-integrated error terms to provide corresponding composite full decoupling Jacobian derivative matrix of the manifold.

8. A visual inertial navigation fusion pose estimation device based on composite manifold optimization, characterized in that: The device comprises: A memory storing executable program code; a processor coupled to the memory; The processor calls the executable program code stored in the memory to execute the visual inertial navigation fusion pose estimation method based on composite manifold optimization as described in any one of claims 1 to 6.

9. A computer storage medium, characterized in that The computer storage medium stores computer instructions, which, when called, are used to execute the visual inertial navigation fusion pose estimation method based on composite manifold optimization as described in any one of claims 1 to 6.

Images