ORB_ Slam2 visual inertial tight coupling positioning technology route and code explanation 1 - IMU flow pattern pre integration

2. Flow pattern based IMU Preintegration
This part mainly refers to the thesis （2） The content in ,ORB_SLAM2 In the algorithm of inertial data fusion, the pre integration processing is the reference paper （2） Implemented in . Ahead of VINS The detailed explanation of the pre integration calculation process , That article uses quaternion rotation solution , The process is the same as here , It mainly calculates three quantities , Namely IMU The premixed value , The covariance matrix of the pre integrated measurement and the Jacobian matrix of the residual , I won't say much about their respective uses , If you don't understand, you can refer to VINS Detailed explanation of
https://blog.csdn.net/wangshuailpp/article/details/78461171
（1）IMU Pre integration calculation
World coordinate system W、IMU Coordinate system B And the camera coordinate system C The relationship and transformation of are shown in the figure below 2 Shown , All pose and feature point coordinates are ultimately expressed in the world coordinate system , We will first convert the state quantity in the camera coordinate system to IMU Coordinate system and then convert to world coordinate system .TBC On behalf of the IMU Coordinate system B Lower camera to IMU Transformation matrix of external parameters between ,TWB=(RWB,wp) Represents the world coordinate system W Next IMU Transformation matrix to world coordinates . The relationship between coordinates is very important , It feels complicated when you first touch it , Need to think more . The leading subscripts of all formulas and variables below represent the reference coordinate system , for example wp Is the displacement in the world coordinate system , The first subscript is the reference coordinate system , for example RWB It's in the world coordinate system .

from IMU Accelerometers and gyroscopes can be obtained IMU Acceleration in the coordinate system Ba And angular velocity BwWB, It can be expressed as the following formula （27）（28）, To the left of the formula is IMU Direct output data .
rotate 、 The derivatives of translation and velocity can be expressed as formulas （29）

Rotation in the world coordinate system 、 The translation and velocity can be obtained from the general integral formula

The above continuous time integral can be rewritten into the following formula by using Euler integral in discrete time （30）

The simultaneous formula （27）（28） You can get the formula （31）

At this time, the state quantity is in the world coordinate system . Suppose there is now i and j Two adjacent keyframes , Need to know i The moment corresponds to IMU State quantity , Solving discrete time j Corresponding to the time key frame IMU The premixed value ,


To avoid repeated integration, use IMU The pre integration method calculates i、j The relative change of the state quantity at a time . That is to convert the reference coordinate system from the world coordinate system to IMU Of i Time coordinate system , To calculate the IMU The state quantity is i and j The relative quantity between moments , This relative quantity takes only the value in the integral formula , Instead of spinning 、 Changes in velocity and displacement , and VINS In the same way .

As can be seen from the above formula , Entire preintegration formula and bias , Noise has something to do with it , Equivalent to a binary equation , Therefore, it is necessary to analyze the influence of bias and noise on the system respectively , The noise obeys Gaussian normal distribution , Relatively simple , The offset follows the random walk model , The relationship is complicated , Therefore, the two methods are different . For the noise component, the Gaussian noise propagation equation is used to solve , And the offset adopts the first-order Taylor expansion . The general idea is to offset at first i The moment bi First order expansion at , Get the formula （44）

The value here is IMU The final pre integration value , But it is worth noting that in initialization IMU Preintegration is a first-order term without bias , It's just the first term on the right side of the equation , The Jacobian matrix behind the equation is calculated after initialization and before optimization , obtain IMU Accurate estimated score （ Truth value ）, And the estimated value IMU Residual of . So first we have to manually push the front term on the right of the above formula , Note that the antisymmetric sign is omitted from all the exponential mappings here .


thus IMU The value of the pre integral is calculated , Which is the first term on the right side of the equation .
（2） Preintegrated Jacobian and covariance matrix
Next, we need to calculate the covariance matrix of the noise propagation term and the Jacobian matrix of the corresponding offset . This part is derived in the appendix A and B, and VINS The process is surprisingly consistent , But here we discuss the state variables separately , Namely （deta_Rj,deta_vj,deta_pj） and （deta_Rj-1,deta_vj-1,deta_pj-1）、（nba,nbg） as well as （deta_Rj,deta_vj,deta_pj） and （deta_ba,deta_bg） The relationship between , Combine the two and VINS identical ., This part of the derivation is relatively simple, so we can paste the formula directly .
Here is the covariance matrix ：




The system obeys Gaussian distribution , Covariance matrix is calculated according to Gaussian distribution , It's a 9*9 Matrix .
Here is the Jacobian matrix for the preintegrated pair offset ：


（3） Residual Jacobi



The Jacobian corresponding to the displacement residuals is shown in the figure above ,J The Jacobi corresponding to the time can be deduced in the way of pictures , Pay attention to that pi and pj There is a mistake in the original text of the Jacobian formula of time , The derivation process has been given . Jacobian formulas for velocity residuals and rotations are similar to those for translation , It is not necessary to deduce and directly give , thus IMU It is estimated that the project will be completed by .