Monocular vio fusion rtk system based on incremental graph optimization

Abstract

The invention discloses a monocular vio fusion rtk system based on incremental graph optimization, and the system comprises a data collection and synchronization module which is used for collecting IMU, image and RTK data, and guaranteeing the timestamp synchronization of the IMU, image and RTK data; the initial state estimation module is used for carrying out initial state estimation by using a vio algorithm in combination with IMU and visual data, carrying out global position calibration by using RTK data, and taking an initial state as a first node of a graph; the incremental state diagram construction and constraint residual module is used for constructing an incremental state diagram and calculating a residual; and the incremental optimization module is used for calculating the sum of squares of all newly added constraint residuals, calculating a Jacobian matrix and only optimizing the newest state node. According to the method, the situation that the whole graph needs to be reconstructed during data updating each time can be avoided, and the calculation efficiency and the real-time performance are improved; rTK data is fused as an accurate absolute position reference, so that the positioning accuracy is improved, and the problem of scale uncertainty is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and mapping, and in particular to a monocular VIO fusion RTK system based on incremental graph optimization. Background Art

[0002] Monocular vio is a positioning technology that combines a monocular camera and inertial measurement unit (IMU) data. The basic principle is to estimate the movement of the camera by extracting feature points from the image captured by the camera, and use the acceleration and angular velocity information provided by the IMU to supplement the visual information and improve the accuracy of motion estimation. However, monocular vio also has some problems. First, the monocular camera cannot directly obtain the depth information of the scene and can only estimate the depth, which leads to scale uncertainty and positioning drift when running for a long time. Moreover, the noise and deviation of the IMU will affect the long-term stability of the system. Especially when running for a long time, cumulative errors may occur, resulting in a decrease in positioning accuracy.

[0003] Graph optimization is a technique that uses graph structures to represent variables and constraint relationships in optimization problems. However, the graph needs to be rebuilt every time the data is updated, resulting in large amounts of computation and low computational efficiency.

[0004] Real-time kinematic positioning (RTK) is a high-precision positioning technology based on satellite navigation systems that can provide accurate location information in open-air environments. Summary of the invention

[0005] The purpose of the present invention is to overcome the shortcomings and deficiencies of the prior art and propose a monocular VIO fusion RTK system based on incremental graph optimization, which avoids the need to rebuild the entire graph every time the data is updated, thereby improving the computational efficiency and real-time performance; fusing RTK data as an accurate absolute position reference improves positioning accuracy and solves the problem of scale uncertainty.

[0006] To achieve the above object, the technical solution provided by the present invention is: a monocular VIO fusion RTK system based on incremental graph optimization, comprising:

[0007] The data acquisition and synchronization module is used to collect IMU data from inertial sensors, image data from monocular cameras, and RTK data from GNSS sensors, and then use timestamps to match data from different sensors to ensure that their timestamps are synchronized;

[0008] The initial state estimation module is used to combine the synchronized IMU and image data to perform preliminary state estimation using the vio algorithm, and then use the RTK data for global position calibration, with the initial state as the first node of the state diagram;

[0009] The incremental state diagram construction and constraint residual module is used to construct an incremental state diagram based on the initial state diagram using the new state, and perform state prediction at the same time, and calculate the constraint residual between the predicted value and the true value;

[0010] The incremental optimization module is used to calculate the sum of squares of all newly added constraint residuals, calculate the Jacobian matrix, obtain the intermediate variables in the incremental optimization process through the Jacobian matrix, and use the intermediate variables to optimize the latest state node.

[0011] Furthermore, the data acquisition and synchronization module specifically performs the following operations:

[0012] Get IMU, image and RTK data:

[0013] a=[a x ,a y ,a z ] T

[0014] w=[w x ,w y ,w z ] T

[0015] z visual =[u,v] T

[0016] p RTK =[x,y,z]

[0017] v RTK =[v x ,v y ,v z ]

[0018] q RTK =[q w ,q x ,q y ,q z ]

[0019] In the formula, a represents acceleration, a x 、a y 、a z They represent the acceleration components on the x, y, and z axes respectively; w represents the angular velocity, w x 、w y 、w z Respectively represent the angular velocity components on the x, y, and z axes; visual represents the coordinates of the feature points obtained from the image data, and u and v represent the x-axis and y-axis coordinates of the feature points in the image respectively; [·] T represents transpose; p RTKIndicates the position coordinates in RTK data; v RTK Indicates the speed information in RTK data, v x 、v y 、v z Respectively represent the velocity components on the x, y, and z axes; q RTK Represents the attitude quaternion, q w Represents the scalar part of the quaternion, which is related to the cosine of the rotation angle, q x ,q y ,q z is the imaginary part of the quaternion, corresponding to the direction of the rotation axis and the sine part of the rotation angle;

[0020] After obtaining the above data, synchronize the data to ensure that all types of data have matching timestamps and output the data.

[0021] Further, the initial state estimation module specifically performs the following operations:

[0022] 1) Use the VIO algorithm to combine the synchronized IMU and visual data to obtain the state estimate at the initial moment:

[0023] x0=[p0,v0,q0,z visual,0 ]

[0024] p0=[x0,y0,z0] T

[0025] v0=[v x0 ,v y0 ,v z0 ] T

[0026]

[0027] z visual,0 =[u0,v0] T

[0028] In the formula, x0 represents the initial state; p0 represents the initial position, x0, y0, z0 represent the components of the initial position on the x, y, and z axes respectively; v0 represents the initial velocity, v x0 、v y0 、v z0 They represent the components of the initial velocity on the x, y, and z axes respectively; q0 represents the initial attitude quaternion, Indicates the angle of rotation, Respectively represent the components of the rotation axis on the x, y, and z axes; visual,0 Represents the x- and y-axis coordinates (u0, v0) of the feature points in the initial image;

[0029] 2) Calibrate using the global position provided by RTK:

[0030]

[0031] Where p0′ is the initial estimated global position, is the accurate position provided by RTK. Replace p0 in x0 with That is, p0′ is replaced to achieve global position calibration;

[0032] The replaced x0 is output as the initial state and also as the first node in constructing the incremental state graph.

[0033] Furthermore, the incremental state diagram construction and constraint residual module specifically performs the following operations:

[0034] 1) Use the IMU, image and RTK data at time k-1 to predict the state at time k

[0035]

[0036] In the formula, p k-1 represents the position at time k-1; v k-1 represents the speed at time k-1; q k-1 Represents the attitude quaternion at time k-1; h visual (·) represents the prediction equation for predicting the coordinates of the feature points in the image at the next moment; z visual,k-1 represents the coordinates of the feature point in the image at time k-1; a k-1 is the acceleration at time k-1; Δq(w k-1 ,Δt) represents the angular velocity w at time k-1 k-1 The incremental quaternion after Δt time; Represents the multiplication of two quaternions; It means that at time k, the state x at time k-1 is k-1 The predicted position prediction value; It means that at time k, the state x at time k-1 is k-1 The predicted speed prediction value; It means that at time k, the state x at time k-1 is k-1 The predicted attitude quaternion prediction value; Indicates the coordinates z of the feature point at time k according to time k-1 visual,k-1 The predicted value of the feature point coordinates;

[0037] 2) Update the state x at the current time k with the new IMU, image and RTK data k :

[0038] x k=[p k ,v k ,q k ,z visual,k ]

[0039] In the formula, p k represents the position at time k; v k represents the speed at time k; q k The quaternion representing the attitude at time k; z visual,k Represents the coordinates of the feature points in the image at time k;

[0040] Every time a new state is added, a new node is added to the existing state diagram to implement an incremental state diagram;

[0041] 3) Calculate the new constraint residual based on the predicted value obtained at time k-1 and the true value obtained from the IMU data at time k:

[0042]

[0043] In the formula, represents the state x at the current time k k The constrained residual between the predicted value and the true value of the location; represents the state x at the current time k k The constrained residual between the speed prediction and the true value; represents the state x at the current time k k The constrained residual between the predicted value and the true value of the attitude quaternion;

[0044] 4) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the image data at time k:

[0045]

[0046] In the formula, z visual,k represents the actual feature point coordinates obtained through the image at time k; r visual,k (x k ) represents the state x at the current time k k The constrained residual between the predicted value and the true value of the feature point coordinates;

[0047] 5) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the RTK data at time k:

[0048]

[0049] In the formula, Indicates the real position provided by RTK; r RTK,k (x k ) represents the state x at the current time kk The constrained residual between the position prediction value and the true value provided by RTK.

[0050] Furthermore, the incremental optimization module specifically performs the following operations:

[0051] 1) Calculate the sum of squares of all newly constrained residuals:

[0052]

[0053] In the formula, R represents the sum of squares of constrained residuals;

[0054] 2) Calculate the partial derivative of each constraint residual with respect to the state variable to obtain a Jacobian matrix:

[0055]

[0056]

[0057] J=[J1,J2,...J i ,...J5]

[0058] In the formula, J1 represents The vector after partial derivative at position p; J2 represents The vector after taking the partial derivative of the velocity v; J3 represents The vector after partial derivative of the posture q; J4 represents r visual,k (x k ) is the vector obtained by partial derivative of the number of feature points n in the image; J5 represents r RTK,k (x k ) The vector after partial derivative of position p; J represents the vector obtained by J i The Jacobian matrix composed of

[0059] 3) Calculate the optimized intermediate variables using the Jacobian matrix and the sum of squares of the constraint residuals:

[0060] Δx=(J T J+λI) -1 J T R

[0061] In the formula, Δx represents the control x in the optimization process k The amount of change; λ is the damping factor; J T J is the regularization term of the Jacobian matrix; R represents the newly added constraint residual sum;

[0062] 4) Iterative optimization based on intermediate variables:

[0063]

[0064] In the formula, Indicates that it is used to replace x during the optimization process k The intermediate variable of Intermediate variables representing positions; An intermediate variable representing speed; Intermediate variables representing posture; Intermediate variables representing feature points in the image;

[0065] Will Replace the original predicted value:

[0066]

[0067]

[0068] Repeat steps 1), 2), and 3) for multiple optimizations until R converges and outputs

[0069] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0070] The present invention effectively solves the scale uncertainty problem of the monocular VIO system, improves the positioning accuracy of the system, and solves the problem of rebuilding the state graph every time the data is updated during the graph optimization process, thereby reducing the amount of calculation and ensuring calculation efficiency and real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 Schematic diagram of the system of the present invention. DETAILED DESCRIPTION

[0072] The present invention is further described in detail below in conjunction with embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0073] like Figure 1 As shown, this embodiment discloses a monocular VIO fusion RTK system based on incremental graph optimization, including the following functional modules:

[0074] The data acquisition and synchronization module is used to collect IMU data from inertial sensors, image data from monocular cameras, and RTK data from GNSS sensors, and then use timestamps to match data from different sensors to ensure that their timestamps are synchronized;

[0075] The initial state estimation module is used to combine the synchronized IMU and image data to perform preliminary state estimation using the vio algorithm, and then use the RTK data for global position calibration, with the initial state as the first node of the state diagram;

[0076] The incremental state diagram construction and constraint residual module is used to construct an incremental state diagram based on the initial state diagram using the new state, and perform state prediction at the same time, and calculate the constraint residual between the predicted value and the true value;

[0077] The incremental optimization module is used to calculate the sum of squares of all newly added constraint residuals, calculate the Jacobian matrix, obtain the intermediate variables in the incremental optimization process through the Jacobian matrix, and use the intermediate variables to optimize the latest state node.

[0078] Specifically, the data acquisition and synchronization module specifically performs the following operations:

[0079] Get IMU, image and RTK data:

[0080] a=[a x ,a y ,a z ] T

[0081] w=[w x ,w y ,w z ] T

[0082] z visual =[u,v] T

[0083] p RTK =[x,y,z]

[0084] v RTK =[v x ,v y ,v z ]

[0085] q RTK =[q w ,q x ,q y ,q z ]

[0086] In the formula, a represents acceleration, a x 、a y 、a z They represent the acceleration components on the x, y, and z axes respectively; w represents the angular velocity, w x 、w y 、w z Respectively represent the angular velocity components on the x, y, and z axes; visual represents the coordinates of the feature points obtained from the image data, and u and v represent the x-axis and y-axis coordinates of the feature points in the image respectively; [·] T represents transpose; p RTKIndicates the position coordinates in RTK data; v RTK Indicates the speed information in RTK data, v x 、v y 、v z Respectively represent the velocity components on the x, y, and z axes; q RTK Represents the attitude quaternion, q w Represents the scalar part of the quaternion, which is related to the cosine of the rotation angle, q x ,q y ,q z is the imaginary part of the quaternion, corresponding to the direction of the rotation axis and the sine part of the rotation angle;

[0087] After obtaining the above data, synchronize the data to ensure that all types of data have matching timestamps and output the data.

[0088] Specifically, the initial state estimation module specifically performs the following operations:

[0089] 1) Use the VIO algorithm to combine the synchronized IMU and visual data to obtain the state estimate at the initial moment:

[0090] x0=[p0,v0,q0,z visual,0 ]

[0091] p0=[x0,y0,z0] T

[0092] v0=[v x0 ,v y0 ,v z0 ] T

[0093]

[0094] z visual,0 =[u0,v0] T

[0095] In the formula, x0 represents the initial state; p0 represents the initial position, x0, y0, z0 represent the components of the initial position on the x, y, and z axes respectively; v0 represents the initial velocity, v x0 、v y0 、v z0 They represent the components of the initial velocity on the x, y, and z axes respectively; q0 represents the initial attitude quaternion, Indicates the angle of rotation, Respectively represent the components of the rotation axis on the x, y, and z axes; visual,0 Represents the x- and y-axis coordinates (u0, v0) of the feature points in the initial image;

[0096] 2) Calibrate using the global position provided by RTK:

[0097]

[0098] Where p0′ is the initial estimated global position, is the accurate position provided by RTK. Replace p0 in x0 with That is, p0′ is replaced to achieve global position calibration;

[0099] The replaced x0 is output as the initial state and also as the first node in constructing the incremental state graph.

[0100] Specifically, the incremental state diagram construction and constraint residual module specifically performs the following operations:

[0101] 1) Use the IMU, image and RTK data at time k-1 to predict the state at time k

[0102]

[0103]

[0104] In the formula, p k-1 represents the position at time k-1; v k-1 represents the speed at time k-1; q k-1 Represents the attitude quaternion at time k-1; h visual (·) represents the prediction equation for predicting the coordinates of the feature points in the image at the next moment; z visual,k-1 represents the coordinates of the feature point in the image at time k-1; a k-1 is the acceleration at time k-1; Δq(w k-1 ,Δt) represents the angular velocity w at time k-1 k-1 The incremental quaternion after Δt time; Represents the multiplication of two quaternions; It means that at time k, the state x at time k-1 is k-1 The predicted position prediction value; It means that at time k, the state x at time k-1 is k-1 The predicted speed prediction value; It means that at time k, the state x at time k-1 is k-1 The predicted attitude quaternion prediction value; Indicates the coordinates z of the feature point at time k according to time k-1 visual,k-1 The predicted value of the feature point coordinates;

[0105] 2) Update the state x at the current time k with the new IMU, image and RTK data k :

[0106] x k =[p k ,v k ,q k ,z visual,k ]

[0107] In the formula, p k represents the position at time k; v k represents the speed at time k; q k The quaternion representing the attitude at time k; z visual,k Represents the coordinates of the feature points in the image at time k;

[0108] Every time a new state is added, a new node is added to the existing state diagram to implement an incremental state diagram;

[0109] 3) Calculate the new constraint residual based on the predicted value obtained at time k-1 and the true value obtained from the IMU data at time k:

[0110]

[0111] In the formula, represents the state x at the current time k k The constrained residual between the predicted value and the true value of the location; represents the state x at the current time k k The constrained residual between the speed prediction and the true value; represents the state x at the current time k k The constrained residual between the predicted value and the true value of the attitude quaternion;

[0112] 4) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the image data at time k:

[0113]

[0114] In the formula, z visual,k represents the actual feature point coordinates obtained through the image at time k; r visual,k (x k ) represents the state x at the current time k k The constrained residual between the predicted value and the true value of the feature point coordinates;

[0115] 5) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the RTK data at time k:

[0116]

[0117] In the formula, Indicates the real position provided by RTK; rRTK,k (x k ) represents the state x at the current time k k The constrained residual between the position prediction value and the true value provided by RTK.

[0118] Specifically, the incremental optimization module performs the following operations:

[0119] 1) Calculate the sum of squares of all newly constrained residuals:

[0120]

[0121] In the formula, R represents the sum of squares of constrained residuals;

[0122] 2) Calculate the partial derivative of each constraint residual with respect to the state variable to obtain a Jacobian matrix:

[0123]

[0124] J=[J1,J2,...J i ,...J5]

[0125] In the formula, J1 represents The vector after partial derivative at position p; J2 represents The vector after taking the partial derivative of the velocity v; J3 represents The vector after partial derivative of the posture q; J4 represents The vector after partial derivative of the number of feature points n in the image; J5 represents r RTK,k (x k ) The vector after partial derivative of position p; J represents the vector obtained by J i The Jacobian matrix composed of

[0126] 3) Calculate the optimized intermediate variables using the Jacobian matrix and the sum of squares of the constraint residuals:

[0127] Δx=(J T J+λI) -1 J T R

[0128] In the formula, Δx represents the control x in the optimization process k The amount of change; λ is the damping factor; J T J is the regularization term of the Jacobian matrix; R represents the newly added constraint residual sum;

[0129] 4) Iterative optimization based on intermediate variables:

[0130]

[0131] In the formula, Indicates that it is used to replace x during the optimization processk The intermediate variable of Intermediate variables representing positions; An intermediate variable representing speed; Intermediate variables representing posture; Intermediate variables representing feature points in the image;

[0132] Will Replace the original predicted value:

[0133]

[0134] Repeat steps 1), 2), and 3) for multiple optimizations until R converges and outputs

[0135] In summary, the present invention can effectively avoid rebuilding the entire map every time the data is updated, thereby improving the computational efficiency and real-time performance, integrating RTK data as an accurate absolute position reference, improving positioning accuracy, and solving the scale uncertainty problem.

[0136] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. A monocular vio fusion rtk system based on incremental graph optimization, characterized in that: include: The data acquisition and synchronization module is used to collect IMU data from inertial sensors, image data from monocular cameras, and RTK data from GNSS sensors, and then use timestamps to match data from different sensors to ensure that their timestamps are synchronized; The initial state estimation module is used to combine the synchronized IMU and image data to perform preliminary state estimation using the vio algorithm, and then use the RTK data for global position calibration, with the initial state as the first node of the state diagram; The incremental state diagram construction and constraint residual module is used to construct an incremental state diagram based on the initial state diagram using the new state, and perform state prediction at the same time, and calculate the constraint residual between the predicted value and the true value; The incremental optimization module is used to calculate the sum of squares of all newly added constraint residuals, calculate the Jacobian matrix, obtain the intermediate variables in the incremental optimization process through the Jacobian matrix, and use the intermediate variables to optimize the latest state node.

2. The monocular VIO fusion RTK system based on incremental graph optimization according to claim 1 is characterized in that: The data acquisition and synchronization module specifically performs the following operations: Get IMU, image and RTK data: a=[a x ,a y ,a z ] T in=[in x ,In y ,In z ] T z visual =[u,v] T p RTK =[x,y,z] v RTK =[v x ,v y ,v z ] q RTK =[q w ,q x ,q y ,q z ] In the formula, a represents acceleration, a x 、a y 、a z They represent the acceleration components on the x, y, and z axes respectively; w represents the angular velocity, w x 、w y 、w z Respectively represent the angular velocity components on the x, y, and z axes; visual represents the coordinates of the feature points obtained from the image data, and u and v represent the x-axis and y-axis coordinates of the feature points in the image respectively; [·] T represents transpose; p RTK Indicates the position coordinates in RTK data; v RTK Indicates the speed information in RTK data, v x 、v y 、v z Respectively represent the velocity components on the x, y, and z axes; q RTK Represents the attitude quaternion, q w Represents the scalar part of the quaternion, which is related to the cosine of the rotation angle, q x ,q y ,q z is the imaginary part of the quaternion, corresponding to the direction of the rotation axis and the sine part of the rotation angle; After obtaining the above data, synchronize the data to ensure that all types of data have matching timestamps and output the data.

3. The monocular VIO fusion RTK system based on incremental graph optimization according to claim 2 is characterized in that: The initial state estimation module specifically performs the following operations: 1) Use the VIO algorithm to combine the synchronized IMU and visual data to obtain the state estimate at the initial moment: x0=[p0,v0,q0,z visual,0 ] p0=[x0,y0,z0] T v0=[v x0 ,v y0 ,v z0 ] T from visual,0 =[u0,v0] T In the formula, x0 represents the initial state; p0 represents the initial position, x0, y0, z0 represent the components of the initial position on the x, y, and z axes respectively; v0 represents the initial velocity, v x0 、v y0 、v z0 They represent the components of the initial velocity on the x, y, and z axes respectively; q0 represents the initial attitude quaternion, Indicates the angle of rotation, Respectively represent the components of the rotation axis on the x, y, and z axes; visual,0 Represents the x- and y-axis coordinates (u0, v0) of the feature points in the initial image; 2) Calibrate using the global position provided by RTK: Where p0′ is the initial estimated global position, is the accurate position provided by RTK. Replace p0 in x0 with That is, p0′ is replaced to achieve global position calibration; The replaced x0 is output as the initial state and also as the first node in constructing the incremental state graph.

4. The monocular VIO fusion RTK system based on incremental graph optimization according to claim 3 is characterized in that: The incremental state diagram construction and constraint residual module specifically performs the following operations: 1) Use the IMU, image and RTK data at time k-1 to predict the state at time k In the formula, p k-1 represents the position at time k-1; v k-1 represents the speed at time k-1; q k-1 Represents the attitude quaternion at time k-1; h visual (·) represents the prediction equation for predicting the coordinates of the feature points in the image at the next moment; z visual,k-1 represents the coordinates of the feature point in the image at time k-1; a k-1 is the acceleration at time k-1; Δq(w k-1 ,Δt) represents the angular velocity w at time k-1 k-1 The incremental quaternion after Δt time; Represents the multiplication of two quaternions; It means that at time k, the state x at time k-1 is k-1 The predicted position prediction value; It means that at time k, the state x at time k-1 is k-1 The predicted speed prediction value; It means that at time k, the state x at time k-1 is k-1 The predicted attitude quaternion prediction value; Indicates the coordinates z of the feature point at time k according to time k-1 visual,k-1 The predicted value of the feature point coordinates; 2) Update the state x at the current time k with the new IMU, image and RTK data k : x k =[p k ,v k ,q k ,z visual,k ] In the formula, p k represents the position at time k; v k represents the speed at time k; q k The quaternion representing the attitude at time k; z visual,k Represents the coordinates of the feature points in the image at time k; Every time a new state is added, a new node is added to the existing state diagram to implement an incremental state diagram; 3) Calculate the new constraint residual based on the predicted value obtained at time k-1 and the true value obtained from the IMU data at time k: In the formula, Represents the state x at the current time k k The constrained residual between the predicted value and the true value of the location; Represents the state x at the current time k k The constrained residual between the speed prediction and the true value; Represents the state x at the current time k k The constrained residual between the predicted value and the true value of the attitude quaternion; 4) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the image data at time k: In the formula, z visual,k represents the actual feature point coordinates obtained through the image at time k; r visual,k (x k ) represents the state x at the current time k k The constrained residual between the predicted value and the true value of the feature point coordinates; 5) Calculate the new constrained residual based on the predicted value obtained at time k-1 and the true value obtained from the RTK data at time k: In the formula, Indicates the real position provided by RTK; r RTK,k (x k ) represents the state x at the current time k k The constrained residual between the position prediction value and the true value provided by RTK.

5. The monocular VIO fusion RTK system based on incremental graph optimization according to claim 4 is characterized in that: The incremental optimization module specifically performs the following operations: 1) Calculate the sum of squares of all newly constrained residuals: In the formula, R represents the sum of squares of constrained residuals; 2) Calculate the partial derivative of each constraint residual with respect to the state variable to obtain a Jacobian matrix: J\[J1,J2,...J i ,…J5] In the formula, J1 represents The vector after partial derivative at position p; J2 represents The vector after taking the partial derivative of the velocity v; J3 represents The vector after partial derivative of the posture q; J4 represents r visual,k (x k ) is the vector obtained by partial derivative of the number of feature points n in the image; J5 represents r RTK,k (x k ) The vector after partial derivative of position p; J represents the vector obtained by J i The Jacobian matrix composed of 3) Calculate the optimized intermediate variables using the Jacobian matrix and the sum of squares of the constraint residuals: Δx=(J T J+λI) -1 J T R In the formula, Δx represents the control x in the optimization process k The amount of change; λ is the damping factor; J T J is the regularization term of the Jacobian matrix; R represents the newly added constraint residual sum; 4) Iterative optimization based on intermediate variables: In the formula, Indicates that it is used to replace x during the optimization process k The intermediate variable of Intermediate variables representing positions; An intermediate variable representing speed; Intermediate variables representing posture; Intermediate variables representing feature points in the image; Will Replace the original predicted value: Repeat steps 1), 2), and 3) for multiple optimizations until R converges and outputs

Images