Motion data processing method, device, terminal device and storage medium

Abstract

The present invention discloses a motion data processing method, apparatus, terminal device and storage medium. The motion data processing method is applied to a SLAM system, and obtains target visual and inertial data and corresponding residual factors; solves the state variables of the target visual and inertial data according to the residual factors, and calculates the Jacobian matrix of the residual factors to the state variables; calculates an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtains the covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix; determines the state variables according to the obtained target visual and inertial data and the corresponding residual factors, and thus calculates the corresponding covariance matrix. A method for solving the covariance matrix in a SLAM system based on optimization is provided, which is used to characterize the changes and fluctuations of the posture in the SLAM system, so that the flight control system can more accurately control the aircraft using the posture.

Description

Technical Field

[0001] The present invention relates to the field of motion control, and in particular to a motion data processing method, apparatus, terminal equipment and storage medium. Background Art

[0002] Visual-inertial odometry (VIO) is an algorithm that integrates camera and inertial measurement unit data to achieve visual SLAM (Simultaneous Localization and Mapping). It can solve the position and posture of moving objects, including aircraft, at any moment in real time. However, the covariance corresponding to the position and posture cannot usually be obtained. The covariance of the position and posture can characterize the fluctuation range of the position and posture calculated by the system, the correlation of the position and posture, and the accuracy of the position and posture. Therefore, the role of the covariance matrix is ​​mainly to characterize whether the calculated position and posture are accurate. Therefore, for a visual-inertial odometry, it must be able to output not only the position and posture, but also the covariance matrix of the position and posture, so that the flight control system can more accurately use the position and posture to control the aircraft.

[0003] Currently, visual SLAM is mainly divided into filtering-based methods and optimization-based methods. Among them, filtering-based methods can easily obtain the covariance matrix of the pose. However, it is difficult for optimization-based methods to solve the pose covariance.

[0004] Therefore, it is necessary to propose a solution to solve the covariance matrix in the optimization-based SLAM system.

[0005] The above content is only used to assist in understanding the technical solution of the present invention and does not constitute an admission that the above content is prior art. Summary of the Invention

[0006] The main purpose of the present invention is to provide a motion data processing method, apparatus, terminal device and storage medium, aiming to provide a solution for solving the covariance matrix in an optimization-based SLAM system.

[0007] To achieve the above object, the present invention provides a motion data processing method, which is applied to a SLAM system and includes:

[0008] Obtain target visual and inertial data and corresponding residual factors;

[0009] Solving the state variables of the target visual and inertial data according to the residual factors, and calculating the Jacobian matrix of the residual factors with respect to the state variables;

[0010] An approximate Hessian matrix is ​​calculated according to the Jacobian matrix based on a preset covariance formula, and a covariance matrix corresponding to the posture in the SLAM system is obtained according to the approximate Hessian matrix.

[0011] Optionally, after the step of calculating the corresponding covariance matrix according to the approximate Hessian matrix, the method further includes:

[0012] Hypothesis testing is performed on the covariance matrix.

[0013] Optionally, before the step of obtaining target visual and inertial data and corresponding residual factors, the step further includes:

[0014] Obtain pre-collected raw visual and inertial data;

[0015] Preprocessing the original visual and inertial data to obtain a preprocessing result;

[0016] Initialize the preprocessing results to obtain the target visual and inertial data and corresponding residual factors.

[0017] Optionally, before the step of calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix, the step further includes:

[0018] The preset covariance formula is obtained by deduction, specifically including:

[0019] Determine the Jacobian matrix of a single residual factor to the state variable based on the iterative equation of the state variable;

[0020] Determine an incremental equation based on the Jacobian matrix of the single residual factor to the state variable;

[0021] Determine the incremental formula and the residual formula of the system according to the incremental equation;

[0022] The preset covariance formula is obtained based on the incremental formula and the residual formula of the system.

[0023] Optionally, the step of performing hypothesis testing on the covariance matrix includes:

[0024] Testing the covariance matrix based on a preset chi-square distribution statistic to determine whether the test passes;

[0025] If the test passes, the first preset value is output;

[0026] If the test fails, the second preset value is output.

[0027] Optionally, before the step of testing the covariance matrix based on a preset chi-square distribution statistic, the step further includes:

[0028] Constructing a chi-square statistic based on the preset covariance formula;

[0029] Transforming the chi-square statistic based on the definition of chi-square distribution to obtain a transformed chi-square statistic;

[0030] Performing eigenvalue decomposition on the preset covariance formula to obtain a decomposition result;

[0031] The chi-square distribution statistic is obtained according to the decomposition result and the transformed chi-square statistic.

[0032] Optionally, the raw visual and inertial data includes image frame data and inertial measurement data, and the step of preprocessing the raw visual and inertial data to obtain a preprocessing result includes:

[0033] Performing image extraction and feature point tracking on the image frame data to obtain image preprocessing data;

[0034] Pre-integrating the inertial measurement data to obtain inertial preprocessing data;

[0035] The image preprocessing data and the inertial preprocessing data are used as the preprocessing results.

[0036] In addition, to achieve the above-mentioned object, the present invention further provides a motion data processing device, the motion data processing device comprising:

[0037] Acquisition module, used to obtain target visual and inertial data and corresponding residual factors;

[0038] A solving module, configured to solve the state variables of the target visual and inertial data according to the residual factors, and calculate the Jacobian matrix of the residual factors with respect to the state variables;

[0039] A calculation module is used to calculate an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtain a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix.

[0040] In addition, to achieve the above-mentioned purpose, the present invention also provides a terminal device, which includes a memory, a processor, and a motion data processing program stored on the memory and runnable on the processor, and when the motion data processing program is executed by the processor, the steps of the motion data processing method described above are implemented.

[0041] In addition, to achieve the above-mentioned purpose, the present invention also provides a computer-readable storage medium, on which a motion data processing program is stored. When the motion data processing program is executed by a processor, the steps of the motion data processing method described above are implemented.

[0042] An embodiment of the present invention proposes a motion data processing method, apparatus, terminal device and storage medium. The motion data processing method is applied to a SLAM system, by acquiring target visual and inertial data and corresponding residual factors; solving the state variables of the target visual and inertial data according to the residual factors, and calculating the Jacobian matrix of the residual factors to the state variables; calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining the covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix; determining the state variables according to the acquired target visual and inertial data and the corresponding residual factors, and thus calculating the corresponding covariance matrix. A method for solving the covariance matrix in a SLAM system based on optimization is provided, which is used to characterize the changes and fluctuations of the posture in the SLAM system, so that the flight control system can more accurately control the aircraft using the posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 This is a schematic diagram of the functional modules of the terminal device to which the motion data processing device of the present invention belongs;

[0044] Figure 2 1 is a flow chart of an exemplary embodiment of a motion data processing method of the present invention;

[0045] Figure 3 1 is a flow chart of another exemplary embodiment of a motion data processing method according to the present invention;

[0046] Figure 4 Schematic diagram of the overall principle of the VIO system in an embodiment of the present invention;

[0047] Figure 5 1 is a flow chart of another exemplary embodiment of a motion data processing method according to the present invention;

[0048] Figure 6 1 is a flow chart of another exemplary embodiment of a motion data processing method according to the present invention;

[0049] Figure 7 Schematic diagram of the overall flow of the motion data processing method in an embodiment of the present invention.

[0050] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0051] It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0052] The main solution of an embodiment of the present invention is: by obtaining target visual and inertial data and corresponding residual factors; solving the state variables of the target visual and inertial data according to the residual factors, and calculating the Jacobian matrix of the residual factors to the state variables; calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining the covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix, determining the state variables according to the obtained target visual and inertial data and the corresponding residual factors, and thus calculating the corresponding covariance matrix, thereby providing a method for solving the covariance matrix in an optimization-based SLAM system.

[0053] Technical terms involved in the embodiments of the present invention:

[0054] SLAM (Simultaneous Localization and Mapping): A machine starts moving from an unknown location in an unknown environment, locates itself based on its location and map, and constructs an incremental map based on its own positioning, enabling autonomous positioning and navigation.

[0055] IMU (Inertial Measurement Unit): used to measure the three-axis attitude angle (or angular rate) and acceleration of an object;

[0056] VIO (visual-inertial odometry): an algorithm for fusing camera and IMU data to implement SLAM.

[0057] Hessian Matrix: Hessian matrix, also translated as Hessian matrix, Hessian matrix and Hessian matrix, is a square matrix composed of the second-order partial derivatives of a multivariate function, which describes the local curvature of the function.

[0058] Existing technology uses VIO to determine the position and pose of an aircraft at any moment in real time, but generally cannot obtain the covariance corresponding to the position and pose. The covariance of the position and pose can represent information such as the fluctuation range of the position and pose correlation, as well as the accuracy of the position and pose obtained by the system. Therefore, the covariance matrix mainly serves to indicate the accuracy of the calculated position and pose. Therefore, for a visual inertial odometry, it is necessary to be able to output not only the position and pose, but also the covariance matrix of the position and pose, so that the flight control system can more accurately use the position and pose to control the aircraft.

[0059] Currently, visual SLAM is mainly divided into filtering-based methods and optimization-based methods. The main principle of filtering-based methods is Kalman filtering. In Kalman filtering, solving the covariance matrix is ​​a step in state updating. Therefore, the system can just solve the covariance matrix of the system in the process of solving the pose. For example, by inserting measurement updates to more closely combine the predicted state variables and the measured state variables, using image rate measurement to update the filter, and then updating the system covariance based on filtering, these methods are all filtering-based methods, so the covariance matrix of the pose can be obtained relatively easily. However, this is not the case for optimization-based methods.

[0060] The main principle of optimization-based methods is the Gauss-Newton method, which primarily solves the system's pose by minimizing a cost function. This process does not address the solution of covariance. Therefore, to solve pose covariance in an optimization-based SLAM system, other methods must be derived. However, most current systems that can output pose covariance are filtering-based systems.

[0061] Therefore, the present invention proposes a method for solving the pose covariance matrix in an optimized SLAM system, derives the pose covariance of the optimized SLAM system according to the Gauss-Newton iterative equation, and performs hypothesis testing on the covariance matrix. On the basis of the obtained covariance matrix, its accuracy can be further evaluated to ensure that the covariance matrix finally outputted can better characterize the changes and fluctuations of the pose.

[0062] Specifically, refer to Figure 1 , Figure 1 This is a schematic diagram of the functional modules of a terminal device to which the motion data processing device of the present invention belongs. The motion data processing device can be a device independent of the terminal device that is capable of processing motion data and can be hosted on the terminal device in the form of hardware or software. The terminal device can be a smart mobile terminal such as a mobile phone or tablet computer with motion data processing capabilities, or a fixed terminal device or server with motion data processing capabilities.

[0063] In this embodiment, the terminal device to which the motion data processing apparatus belongs includes at least an output module 110 , a processor 120 , a memory 130 and a communication module 140 .

[0064] The memory 130 stores an operating system and a motion data processing program. The motion data processing device can store information such as the acquired target visual and inertial data and the corresponding residual factors, the state variables and Jacobian matrix obtained from the residual factors, the approximate Hessian matrix calculated from the Jacobian matrix based on a preset covariance formula, and the covariance matrix calculated from the approximate Hessian matrix in the memory 130. The output module 110 can be a display screen, etc. The communication module 140 can include a Wi-Fi module, a mobile communication module, and a Bluetooth module, etc., and communicates with external devices or servers through the communication module 140.

[0065] When the motion data processing program in the memory 130 is executed by the processor, the following steps are implemented:

[0066] Obtain target visual and inertial data and corresponding residual factors;

[0067] Solving the state variables of the target visual and inertial data according to the residual factors, and calculating the Jacobian matrix of the residual factors with respect to the state variables;

[0068] An approximate Hessian matrix is ​​calculated according to the Jacobian matrix based on a preset covariance formula, and a covariance matrix corresponding to the posture in the SLAM system is obtained according to the approximate Hessian matrix.

[0069] Furthermore, when the motion data processing program in the memory 130 is executed by the processor, the following steps are also implemented:

[0070] Hypothesis testing is performed on the covariance matrix.

[0071] Furthermore, when the motion data processing program in the memory 130 is executed by the processor, the following steps are also implemented:

[0072] Obtain pre-collected raw visual and inertial data;

[0073] Preprocessing the original visual and inertial data to obtain a preprocessing result;

[0074] Initialize the preprocessing results to obtain the target visual and inertial data and corresponding residual factors.

[0075] Furthermore, when the motion data processing program in the memory 130 is executed by the processor, the following steps are also implemented:

[0076] The preset covariance formula is obtained by deduction, specifically including:

[0077] Determine the Jacobian matrix of a single residual factor to the state variable based on the iterative equation of the state variable;

[0078] Determine an incremental equation based on the Jacobian matrix of the single residual factor to the state variable;

[0079] Determine the incremental formula and the residual formula of the system according to the incremental equation;

[0080] The preset covariance formula is obtained based on the incremental formula and the residual formula of the system.

[0081] Furthermore, when the motion data processing program in the memory 130 is executed by the processor, the following steps are also implemented:

[0082] Testing the covariance matrix based on a preset chi-square distribution statistic to determine whether the test passes;

[0083] If the test passes, the first preset value is output;

[0084] If the test fails, the second preset value is output.

[0085] Furthermore, when the motion data processing program in the memory 130 is executed by the processor, the following steps are also implemented:

[0086] Constructing a chi-square statistic based on the preset covariance formula;

[0087] Transforming the chi-square statistic based on the definition of chi-square distribution to obtain a transformed chi-square statistic;

[0088] Performing eigenvalue decomposition on the preset covariance formula to obtain a decomposition result;

[0089] The chi-square distribution statistic is obtained according to the decomposition result and the transformed chi-square statistic.

[0090] Furthermore, when the motion data processing program in the memory 130 is executed by the processor, the following steps are also implemented:

[0091] Performing image extraction and feature point tracking on the image frame data to obtain image preprocessing data;

[0092] Pre-integrating the inertial measurement data to obtain inertial preprocessing data;

[0093] The image preprocessing data and the inertial preprocessing data are used as the preprocessing results.

[0094] This embodiment adopts the above scheme, specifically by obtaining target visual and inertial data and corresponding residual factors; solving the state variables of the target visual and inertial data according to the residual factors, and calculating the Jacobian matrix of the residual factors to the state variables; calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining the covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix, determining the state variables according to the obtained target visual and inertial data and the corresponding residual factors, and thus calculating the corresponding covariance matrix, thereby providing a method for solving the covariance matrix in an optimization-based SLAM system.

[0095] Based on the above terminal device architecture but not limited to the above architecture, an embodiment of the method of the present invention is proposed.

[0096] The execution subject of the method of this embodiment may be a motion data processing device or a terminal device, etc. This embodiment takes a motion data processing device as an example.

[0097] Reference Figure 2 , Figure 2 1 is a flow chart of an exemplary embodiment of a motion data processing method of the present invention. The motion data processing method includes:

[0098] Step S10, obtaining target visual and inertial data and corresponding residual factors;

[0099] The optimized SLAM (Simultaneous Localization and Mapping) system mainly includes a VIO system, a GNSS-VIO system, a Lidar-VIO system, and a wheel speed meter-VIO system. In the embodiment of the present invention, the VIO system is used as an example for explanation.

[0100] VIO (visual-inertial odometry) is an algorithm that implements SLAM by fusing camera and IMU (Inertial Measurement Unit) data. Depending on the fusion framework, it is divided into tight coupling and loose coupling. In loose coupling, the visual motion estimation and inertial motion estimation systems are two independent modules, and the output results of each module are fused. In tight coupling, the raw visual and inertial data of the two sensors are used to jointly estimate a set of variables. Sensor noise also affects each other. Tight coupling is more complex in terms of algorithm, but it makes full use of sensor data to achieve better results.

[0101] In an embodiment of the present invention, data is collected by a camera and an IMU respectively, and then the collected data is input into a VIO system, which pre-processes the data to obtain a pre-processing result, and then initializes the data. When the visual and IMU data are successfully initialized, the back-end optimization is further performed.

[0102] Specifically, the target visual and inertial data obtained after preprocessing and initialization are input into the optimizer for back-end optimization. Since the back-end optimization is based on the Gauss-Newton method to solve the system posture, it is also necessary to provide the corresponding residual factor to the optimizer. The optimizer will solve the state variables of the system based on the residual factor.

[0103] It should be noted that the embodiments of the present invention are described using the SLAM system in an aircraft as an example, which does not constitute a limitation on the application scenarios of the motion data processing method. That is, the applicable scenarios of the motion data processing method proposed in the present invention include but are not limited to SLAM systems of various types of aircraft, autonomous driving vehicles, mobile robots and other moving objects.

[0104] Step S20, solving the state variables of the target visual and inertial data according to the residual factors, and calculating the Jacobian matrix of the residual factors with respect to the state variables;

[0105] Furthermore, in a VIO system, the Jacobian matrix is ​​the derivative of each residual with respect to the state variables. During back-end optimization, the VIO system uses the Ceres Solver optimization library to solve for the residuals and Jacobian matrix. Because each residual is only related to a subset of the state variables, the Jacobian matrix must be expanded each time it is calculated. The expanded dimension is the sum of the dimensions of the system's current state variables. For a sliding window-based VIO system, the system's state variables include the pose (including position, attitude, and velocity) and bias (including accelerometer bias and gyroscope bias) for each frame in the sliding window, as well as the inverse depth of all feature points observed in the sliding window. Since the sliding window size is constant, the dimensions of the pose and bias in the sliding window are also constant, namely: (window size + 1) * 15 (because position, attitude, velocity, accelerometer bias, and gyroscope bias are all three-dimensional, and each frame contains these five state variables, starting at frame 0).

[0106] Alternatively, since the number of feature points visible in the sliding window at any one time is not fixed, the dimension of the feature points is not fixed at any moment. Assuming that M feature points can be seen at a certain moment, the dimension of the system state variable should be: N = (sliding window size + 1) * 15 + M. Because for the Jacobian matrix, its dimension must be expanded to N each time before it can be used to calculate the approximate Hessian matrix. For example, if the state variables associated with the i-th residual are: the 1st key frame, the 3rd key frame, the 9th key frame, and the 0th feature point, then its Jacobian matrix should have the following structure:

[0107]

[0108] In the above formula, K represents a keyframe, i.e., each frame in the sliding window, and F represents a feature point. Therefore, the Jacobian matrix corresponding to each residual must be expanded to the above form to calculate the approximate Hessian matrix. The approximate Hessian matrix is ​​an approximation of the Hessian matrix and can be used to describe the local curvature of a function.

[0109] Step S30: Calculate an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtain a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix.

[0110] Furthermore, after obtaining the Jacobian matrix after dimension expansion, the specific value of the approximate Hessian matrix can be directly calculated based on the preset covariance formula.

[0111] Optionally, in an embodiment of the present invention, the main steps for deriving the preset covariance formula include: determining the Jacobian matrix of a single residual factor to the state variable based on the iterative equation of the state variable, determining the incremental equation according to the Jacobian matrix of the single residual factor to the state variable, determining the incremental formula and the residual formula of the system according to the incremental equation, thereby obtaining the preset covariance formula.

[0112] Optionally, after calculating the specific value of the approximate Hessian matrix from the Jacobian matrix based on a preset covariance formula, the system's covariance matrix can be calculated in conjunction with the preset covariance formula. Generally, the system only needs to output the covariance matrix for the current moment (i.e., the most recent keyframe in the sliding window). Therefore, after obtaining the covariance matrix, only the matrix block corresponding to the current moment's pose is required.

[0113] Optionally, after calculating the covariance matrix, it is necessary to perform a hypothesis test on the obtained covariance matrix to determine whether the covariance matrix can better characterize the changes in the current posture. In an embodiment of the present invention, a chi-square test is performed on the obtained covariance matrix through the constructed chi-square distribution statistic to ensure that the covariance matrix can more realistically reflect the accuracy and volatility of the current posture and other information.

[0114] In this embodiment, target visual and inertial data and corresponding residual factors are obtained; state variables of the target visual and inertial data are solved according to the residual factors, and the Jacobian matrix of the residual factors to the state variables is calculated; an approximate Hessian matrix is ​​calculated according to the Jacobian matrix based on a preset covariance formula, and the covariance matrix corresponding to the posture in the SLAM system is obtained according to the approximate Hessian matrix. The state variables are determined according to the obtained target visual and inertial data and the corresponding residual factors, thereby calculating the corresponding covariance matrix. A method for solving the covariance matrix in an optimization-based SLAM system is provided.

[0115] Reference Figure 3 , Figure 3 This is a flow chart of another exemplary embodiment of the motion data processing method of the present invention. Figure 2 In the embodiment shown, in this embodiment, before the step of obtaining target visual and inertial data and corresponding residual factors, the motion data processing method further includes:

[0116] Step S01, obtaining pre-collected raw visual and inertial data;

[0117] Specifically, refer to Figure 4 , Figure 4 FIG. 1 is a schematic diagram of the overall principle of the VIO system in an embodiment of the present invention. Figure 4 As shown in the figure, the VIO system can be divided into four parts: measurement input, preprocessing, initialization, and back-end optimization. The pre-collected raw visual and inertial data is input into the VIO system, including image frame data collected by the camera and inertial measurement data collected by the IMU. The camera and IMU fusion are highly complementary. First, by aligning the pose sequence estimated by the IMU with the pose sequence estimated by the camera, the true scale of the camera trajectory can be estimated. The IMU can also predict the pose of the image frame and the position of the feature point at the previous moment in the next frame image, thereby improving the matching speed of the feature tracking algorithm and the robustness of the algorithm to cope with rapid rotation. Finally, the gravity vector provided by the accelerometer in the IMU can convert the estimated position into the world coordinate system required for actual navigation.

[0118] Step S02, preprocessing the original visual and inertial data to obtain a preprocessing result;

[0119] Furthermore, after the raw visual and inertial data are input, the image frame data and inertial measurement data need to be preprocessed respectively, including:

[0120] Performing image extraction and feature point tracking on the image frame data to obtain image preprocessing data;

[0121] Pre-integrating the inertial measurement data to obtain inertial preprocessing data;

[0122] The image preprocessing data and the inertial preprocessing data are used as the preprocessing results.

[0123] Specifically, the image frame data is extracted and feature points are tracked to obtain image preprocessing data; the IMU measurement value, i.e., the inertial measurement data, is pre-integrated to obtain inertial preprocessing data, and the image preprocessing data and the inertial preprocessing data are used as preprocessing results for initializing the system.

[0124] Step S03: Initialize the preprocessing result to obtain the target visual and inertial data and the corresponding residual factor.

[0125] Furthermore, during the initialization process, vision-only initialization is first performed, and the motion recovery structure is solved based on the image preprocessing data. Then, visual-inertial alignment is performed with the IMU pre-integration, the initialization parameters are solved, and the initialization is tested for success. If the initialization is unsuccessful, the system returns to visual initialization. If the initialization is successful, the target visual and inertial data and the corresponding residual factors obtained during the initialization process are input into the optimizer for back-end optimization, thereby calculating the covariance matrix of the state variables. In a VIO system, the residual factors generally include prior residuals, visual residuals, and pre-integration residuals. The optimizer will solve the system's state variables based on the residual factors.

[0126] This embodiment uses the above scheme to obtain pre-collected raw visual and inertial data; preprocess the raw visual and inertial data to obtain a preprocessing result; initialize the preprocessing result to obtain the target visual and inertial data and the corresponding residual factor, thereby preprocessing and initializing the raw visual and inertial data collected by the camera and IMU, obtaining the target visual and inertial data and the corresponding residual factor, and thus using them to calculate the covariance matrix.

[0127] Reference Figure 5 , Figure 5 This is a flow chart of another exemplary embodiment of the motion data processing method of the present invention. Figure 2 In the embodiment shown, in this embodiment, before the step of calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining the covariance matrix corresponding to the pose in the SLAM system according to the approximate Hessian matrix, the motion data processing method further includes:

[0128] In step S00, the preset covariance formula is obtained by deduction. In this embodiment, step S00 is performed before step S10. In other embodiments, step S00 may also be performed between step S10 and step S20, or between step S20 and step S30.

[0129] Compared with the above Figure 2 The embodiment shown in FIG. 1 also includes a solution for obtaining the preset covariance formula by deduction.

[0130] Specifically, the step of deriving the preset covariance formula may include:

[0131] Step S001, determining the Jacobian matrix of a single residual factor to the state variable based on an iterative equation of the state variable;

[0132] Step S002, determining an incremental equation according to the Jacobian matrix of the single residual factor to the state variable;

[0133] Step S003, determining the incremental formula and the residual formula of the system according to the incremental equation;

[0134] Step S004: obtaining the preset covariance formula based on the incremental formula and the residual formula of the system.

[0135] Specifically, the update of state variables in the optimization-based SLAM system is mainly based on the Gauss-Newton method. Assuming that the maximum number of iterations of the Gauss-Newton method is k, the iterative equation of the state variables is:

[0136]

[0137] In formula (1), is the initial value set before each iteration, Δx k According to the Gauss-Newton equation, we can get:

[0138]

[0139] In formula (2), r k is the residual factor, J k is the Jacobian matrix of the residuals versus the state variables.

[0140] The state variable x after k optimizations k That is the posterior estimate, so the posterior covariance of the state variable can be expressed as:

[0141]

[0142] In formula (3), the second equal sign is because: in the kth incremental equation, x k-1It can be regarded as a constant, and the covariance of the constant is 0, so the covariance of the state variable after k iterations is equal to the covariance of the kth increment.

[0143] Alternatively, in a VIO system, there are multiple residual factors, and formula (2) is only the incremental equation under a single residual factor. Therefore, to derive the covariance matrix of the VIO system, formula (2) needs to be changed accordingly. Assuming that there are n residual factors in the VIO, the incremental equation is:

[0144]

[0145] Therefore, the increment can be obtained as:

[0146]

[0147] In addition, the residual r of the system i It can generally be expressed as:

[0148]

[0149] Substituting formula (6) into formula (5) yields:

[0150]

[0151] According to formula (7), the covariance of the increment can be obtained as:

[0152]

[0153] Where v is the measurement noise, a random variable. In this embodiment of the present invention, v is assumed to be zero-mean Gaussian white noise, and the measurement noises of different residual factors are assumed to be uncorrelated. Where σ is the variance of v's Gaussian distribution, and H is the approximate Hessian matrix. For simplicity, the covariance of the measurement noise corresponding to different residual factors is assumed to be the same in the above formula.

[0154] It can be seen from Equation (8) that in order to solve the posterior covariance matrix of the optimization-based system, it is only necessary to find the Jacobian matrix of the state variables for different residual factors and then solve the approximate Hessian matrix.

[0155] This embodiment adopts the above scheme, specifically determining the Jacobian matrix of a single residual factor to the state variable through an iterative equation based on the state variable; determining the incremental equation according to the Jacobian matrix of the single residual factor to the state variable; determining the incremental formula and the residual formula of the system according to the incremental equation; obtaining the preset covariance formula based on the incremental formula and the residual formula of the system, and realizing the derivation of the covariance matrix of the posture based on the optimized SLAM system according to the Gauss-Newton iterative equation.

[0156] Reference Figure 6 , Figure 6 This is a flow chart of another exemplary embodiment of the motion data processing method of the present invention. Figure 2 In the embodiment shown, in this embodiment, after the step of calculating the corresponding covariance matrix according to the approximate Hessian matrix, the motion data processing method further includes:

[0157] Step S40: performing hypothesis testing on the covariance matrix.

[0158] Optionally, the step of performing hypothesis testing on the covariance matrix may include:

[0159] Step S401, testing the covariance matrix based on a preset chi-square distribution statistic to determine whether the test passes;

[0160] Step S402: If the test passes, output the first preset value;

[0161] Step S403: If the test fails, output a second preset value.

[0162] Specifically, after obtaining the approximate Hessian matrix, the covariance matrix of the system can be calculated according to formula (8). In general, the system only needs to output the covariance matrix of the current moment (i.e., the latest key frame in the sliding window). Therefore, after obtaining the covariance matrix, it is only necessary to take the matrix block corresponding to the current moment's posture in the matrix. Although the covariance matrix is ​​calculated, it is still uncertain whether the covariance matrix can more realistically reflect the accuracy and volatility of the current posture. Therefore, it is necessary to perform hypothesis testing on the obtained covariance.

[0163] Optionally, before the step of testing the covariance matrix based on a preset chi-square distribution statistic, it is necessary to construct a chi-square distribution statistic, specifically including:

[0164] Constructing a chi-square statistic based on the preset covariance formula;

[0165] Transforming the chi-square statistic based on the definition of chi-square distribution to obtain a transformed chi-square statistic;

[0166] Performing eigenvalue decomposition on the preset covariance formula to obtain a decomposition result;

[0167] The chi-square distribution statistic is obtained according to the decomposition result and the transformed chi-square statistic.

[0168] Specifically, in order to perform hypothesis testing on the covariance matrix, we first need to define the test statistic. Since VIO systems generally assume that state variables follow a normal distribution, we can construct a chi-square distribution statistic. Because formula (8) not only obtains the covariance of the state variable x, but also the covariance of the increment Δx, we can construct the chi-square statistic based on Δx:

[0169] χ=Δx T cov(Δx) -1 Δx (9)

[0170] Alternatively, the covariance matrix is ​​generally not diagonal, so the right-hand side of the above equation contains not only the quadratic term of the state variable Δx but also the linear term of Δx. However, from the definition of the chi-square distribution, the constructed chi-square statistic should only contain quadratic terms and not linear terms. Therefore, the right-hand side of equation (9) needs to be transformed accordingly to conform to the definition of the chi-square distribution.

[0171] Since the covariance matrix is ​​positive definite, its eigenvalues ​​λ1,λ2,…,λ n are all greater than 0. Therefore, we can find the orthogonal matrix Q and perform eigenvalue decomposition on it:

[0172] cov(Δx)=Q T diag(λ1,λ2,…,λ n )Q (10)

[0173] Therefore, formula (10) becomes:

[0174]

[0175] From formula (11), we can see that the transformed Δy is the final chi-square statistic.

[0176] After each covariance matrix is ​​obtained, a chi-square test is performed according to formula (11). If the test passes, the first preset value (for example, 1) is output, indicating that the obtained covariance matrix can better represent the change of posture; otherwise, if the test fails, the second preset value (for example, 0) is output, indicating that the obtained covariance matrix cannot well represent the change of posture.

[0177] This embodiment adopts the above scheme, specifically by testing the covariance matrix based on a preset chi-square distribution statistic to determine whether the test passes; if the test passes, a first preset value is output; if the test fails, a second preset value is output. By constructing a chi-square distribution statistic, a hypothesis test is performed on the covariance matrix, and the accuracy of the obtained covariance matrix is ​​further evaluated to ensure that the covariance matrix finally output can better represent the changes and fluctuations in posture.

[0178] In addition, an embodiment of the present invention further provides a motion data processing device, the motion data processing device comprising:

[0179] Acquisition module, used to obtain target visual and inertial data and corresponding residual factors;

[0180] A solving module, configured to solve the state variables of the target visual and inertial data according to the residual factors, and calculate the Jacobian matrix of the residual factors with respect to the state variables;

[0181] A calculation module is used to calculate an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtain a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix.

[0182] Specifically, refer to Figure 7 , Figure 7 FIG. 1 is a schematic diagram of the overall flow of the motion data processing method according to an embodiment of the present invention. Figure 7 As shown in the figure, the preprocessed and initialized measurement values ​​and the obtained residual factors are input into the optimizer. The optimizer will solve the state variables of the system according to the residual factors, and then calculate the Jacobian matrix and approximate Hessian matrix of the residual to the state variables. The covariance matrix is ​​calculated based on the approximate Hessian matrix, and the covariance matrix is ​​output after hypothesis testing.

[0183] In this embodiment, the main purpose of the VIO system is to output the system's posture at each moment in real time. The accuracy of the posture is closely related to the positioning accuracy of the system. Therefore, in actual engineering applications, it is very important to obtain the solved posture, and it is also very important to obtain the changes and fluctuations in the posture. Because only by knowing the changes and fluctuations in the posture can we determine whether the obtained posture is accurate. Currently, if you want to output the covariance matrix of the VIO system in real time, it is generally only possible in a filtering-based system. For optimization-based SLAM systems, there is no method for outputting the covariance matrix. The embodiment of the present invention realizes the output of the covariance matrix of the posture in the optimization-based SLAM system. According to the method proposed in the embodiment of the present invention, the covariance matrix of the posture can be obtained in real time in the optimization-based SLAM system in a relatively simple manner, and the system consumes less additional time and space. In addition, the hypothesis test proposed in the embodiment of the present invention can further evaluate its accuracy based on the obtained covariance matrix to ensure that the covariance matrix finally output can better represent the changes and fluctuations in the posture.

[0184] In addition, an embodiment of the present invention also proposes a terminal device, which includes a memory, a processor, and a motion data processing program stored in the memory and runnable on the processor. When the motion data processing program is executed by the processor, the steps of the motion data processing method described above are implemented.

[0185] Since this motion data processing program adopts all the technical solutions of all the aforementioned embodiments when executed by the processor, it has at least all the beneficial effects brought by all the technical solutions of all the aforementioned embodiments, which will not be described one by one here.

[0186] In addition, an embodiment of the present invention further provides a computer-readable storage medium, on which a motion data processing program is stored. When the motion data processing program is executed by a processor, the steps of the motion data processing method described above are implemented.

[0187] Since this motion data processing program adopts all the technical solutions of all the aforementioned embodiments when executed by the processor, it has at least all the beneficial effects brought by all the technical solutions of all the aforementioned embodiments, which will not be described one by one here.

[0188] Compared with the existing technology, the motion data processing method, device, terminal device and storage medium proposed in the embodiments of the present invention obtain target visual and inertial data and corresponding residual factors; solve the state variables of the target visual and inertial data according to the residual factors, and calculate the Jacobian matrix of the residual factors to the state variables; calculate an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtain the covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix, determine the state variables according to the obtained target visual and inertial data and the corresponding residual factors, and thus calculate the corresponding covariance matrix, thereby providing a method for solving the covariance matrix in an optimization-based SLAM system.

[0189] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or system comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or system. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or system comprising the element.

[0190] The serial numbers of the above-mentioned embodiments of the present application are for description only and do not represent the advantages or disadvantages of the embodiments.

[0191] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus the necessary general hardware platform, and of course can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as mentioned above, and includes a number of instructions for enabling a terminal device (which can be a mobile phone, computer, server, controlled terminal, or network device, etc.) to execute the method of each embodiment of the present application.

[0192] The above are only preferred embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention description and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A motion data processing method, characterized in that: The motion data processing method is applied to a SLAM system, and the motion data processing method comprises the following steps: Obtain target visual and inertial data and corresponding residual factors; Solving the state variables of the target visual and inertial data according to the residual factors, and calculating the Jacobian matrix of the residual factors with respect to the state variables; Calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix; The step of calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix further includes: The preset covariance formula is obtained by deduction, specifically including: Determine the Jacobian matrix of a single residual factor to the state variable based on the iterative equation of the state variable; Determine an incremental equation based on the Jacobian matrix of the single residual factor to the state variable; Determine the incremental formula and the residual formula of the system according to the incremental equation; The preset covariance formula is obtained based on the incremental formula and the residual formula of the system; wherein the preset covariance formula is the covariance of the increment.

2. The motion data processing method according to claim 1, wherein: After the step of calculating the corresponding covariance matrix according to the approximate Hessian matrix, the following step is further included: Hypothesis testing is performed on the covariance matrix.

3. The motion data processing method according to claim 1, wherein: Before the step of obtaining target visual and inertial data and corresponding residual factors, the method further includes: Obtain pre-collected raw visual and inertial data; Preprocessing the original visual and inertial data to obtain a preprocessing result; Initialize the preprocessing results to obtain the target visual and inertial data and corresponding residual factors.

4. The motion data processing method according to claim 1, wherein: The step of calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix further includes: The preset covariance formula is obtained by deduction, specifically including: Determine the Jacobian matrix of a single residual factor to the state variable based on the iterative equation of the state variable; Determine an incremental equation based on the Jacobian matrix of the single residual factor to the state variable; Determine the incremental formula and the residual formula of the system according to the incremental equation; The preset covariance formula is obtained based on the incremental formula and the residual formula of the system.

5. The motion data processing method according to claim 2, wherein: The step of performing hypothesis testing on the covariance matrix comprises: Testing the covariance matrix based on a preset chi-square distribution statistic to determine whether the test passes; If the test passes, the first preset value is output; If the test fails, the second preset value is output.

6. The motion data processing method according to claim 5, wherein: Before the step of testing the covariance matrix based on a preset chi-square distribution statistic, the method further includes: Constructing a chi-square statistic based on the preset covariance formula; Transforming the chi-square statistic based on the definition of chi-square distribution to obtain a transformed chi-square statistic; Performing eigenvalue decomposition on the preset covariance formula to obtain a decomposition result; The chi-square distribution statistic is obtained according to the decomposition result and the transformed chi-square statistic.

7. The motion data processing method according to claim 3, wherein: The raw visual and inertial data includes image frame data and inertial measurement data. The step of preprocessing the raw visual and inertial data to obtain a preprocessing result includes: Performing image extraction and feature point tracking on the image frame data to obtain image preprocessing data; Pre-integrating the inertial measurement data to obtain inertial preprocessing data; The image preprocessing data and the inertial preprocessing data are used as the preprocessing results.

8. A motion data processing device, characterized in that: The motion data processing device comprises: Acquisition module, used to obtain target visual and inertial data and corresponding residual factors; A solving module, configured to solve the state variables of the target visual and inertial data according to the residual factors, and calculate the Jacobian matrix of the residual factors with respect to the state variables; A calculation module is used to calculate an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtain a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix; The method further includes calculating an approximate Hessian matrix according to the Jacobian matrix based on a preset covariance formula, and obtaining a covariance matrix corresponding to the posture in the SLAM system according to the approximate Hessian matrix: The preset covariance formula is obtained by deduction, specifically including: Determine the Jacobian matrix of a single residual factor to the state variable based on the iterative equation of the state variable; Determine an incremental equation based on the Jacobian matrix of the single residual factor to the state variable; Determine the incremental formula and the residual formula of the system according to the incremental equation; The preset covariance formula is obtained based on the incremental formula and the residual formula of the system; wherein the preset covariance formula is the covariance of the increment.

9. A terminal device, characterized in that: The terminal device includes a memory, a processor, and a motion data processing program stored in the memory and executable on the processor. When the motion data processing program is executed by the processor, the steps of the motion data processing method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a motion data processing program, which, when executed by a processor, implements the steps of the motion data processing method according to any one of claims 1 to 7.

Images