Learning VINS-Mono/Fusion source code from scratch (6): back-end optimization

This section analyzes VINS back-end optimization, including the construction of optimization variables, residual constraints, ceres solver, etc.

VINS-Mono/Fusion code learning series:
Learning VINS-Mono/Fusion source code from scratch (1): Main function
learning VINS-Mono/Fusion source code from scratch (2): Front-end image tracking
learning VINS-Mono/Fusion from scratch Source code (3): IMU pre-integration formula derivation and
learning VINS-Mono/Fusion from scratch source code (4): Error Kalman filter
learning VINS-Mono/Fusion from scratch source code (5): VIO initialization
learning VINS- from scratch Mono/Fusion source code (6): back-end optimization

1 ceres automatic derivation and analytical derivation

• VIO initialization uses the ceres automatic derivation method to construct cost_function. This method is relatively simple. It only needs to construct the optimization variables and residual solution formulas, and does not need to manually calculate the Jacobian; the disadvantage is that the solution speed is slow.

• Back-end optimization uses ceres analytical derivation , which requires manual definition of optimization variables, residuals and Jacobians. In this way, the calculation speed is improved to meet the real-time requirements of the odometer.
  

2 Back-end optimization process

2.1 Define parameter blocks to be optimized

problem.AddParameterBlock()

parameter block	dimension	Code naming
Position in the sliding window	7	para_Pose[WINDOW_SIZE + 1][SIZE_POSE]
Speed ​​and offset	9	para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]
Feature point depth	Upper limit 1000, dimension 1	para_Feature[NUM_OF_F][SIZE_FEATURE]
external reference	7	para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]
Postures related to looping	7	para_Retrive_Pose[SIZE_POSE]
timestamp correction	1	for_Td[1][1]
Not used	1	to_Tr[1][1]

• For rotation, the parameter addition method needs to be redefined
• The ceres parameter blocks are all double array types. The eigen type object for initialization and solution needs to be converted into double, which is implemented through vector2double().

2.2 Add residual blocks through residual constraints

Residual constraints: prior, IMU, vision, loop closure

problem.AddResidualBlock()

(1) IMU pre-integration constraints

Derived from the pre-integration formula, when the pose velocity is optimized and adjusted, formula (5) is no longer equal. At the same time, considering that the adjusted result still cannot deviate too much from the pre-integration, based on this, the IMU residual is constructed.
Where , the bias has also been optimized, and the Jacobian matrix is ​​used to compensate for the pre-integration amount.

IMU pre-integration residual calculation formula:

IMU pre-integration Jacobian formula:
The pre-integration residual r is a 15x1-dimensional vector. The state quantity involves pose, speed and bias. It is necessary to calculate para_Pose (7 dimensions) and para_SpeedBias (9 dimensions) at k and k+1 moments. Derivative, so its Jacobian matrix form is (15x7, 15x9, 15x7, 15x9)

(2) Visual reprojection constraints

The reprojection error of the feature points is constructed in the i-th frame and the j-th frame. The feature points are expressed in inverse depth form. Essentially, the feature points under the i-th frame are converted to the j-th frame and compared with the observed normalized coordinates. Make a difference to construct the visual reprojection error.

Residual calculation formula:

Visual residual Jacobian:
The state quantities involved in the reprojection error include the rotation external parameter Rbc, the translation external parameter pbc, the inverse depth of the feature point, and the pose at two moments i and j. Using the idea of ​​chain derivation, the derivative of the residual to the normalized coordinate point and the derivative of the normalized coordinate point to the state quantity are calculated in sequence.

(3) Marginalization prior

For the old information in the sliding window, Shure patching is used to marginalize old key frames and map points.
As for why all map points are marginalized, it is because the process of marginalization will cause the originally sparse matrix to become dense, which is not conducive to calculation, so vins marginalizes all map points to reduce the dimension of the matrix.

With the following formula, we want to leave $\delta x{\_}_b$, marginalization $\delta x{\_}_a$, so the lower left matrix must be blocked into a 0 matrix.

Here is borrowed from the blog of White Chocolate Yiweixin: https://blog.csdn.net/heyijia0327/article/details/52822104

Essentially solving for $\delta x{\_}_b$In the process, δ xa \delta {x_a} is retained$\delta x{\_}_a$Some constraint information in .

3 Information matrix, FEJ, zero space drift

3.1 Information matrix

It is generally believed that the information matrix is ​​equal to the inverse of the covariance, which is a measure of error uncertainty and is used to unify the dimensions of different error quantities in the sliding window.
For derivation, please see this paper "Relationship between the Hessian and Covariance Matrix for Gaussian Random Variables"

It should be noted that the information matrix is ​​not defined in the ceres solver, so during the processing of the vins code, the information matrix is ​​decomposed by LLT and then multiplied by the Jacobian matrix.

3.2 CHAPTER

After FEJ (First Estimated Jacobian) performs marginalization, the new residual term and the old information matrix use different linearization points to solve the Jacobian matrix, which will cause the zero space of the information matrix to change, which may lead to changes in the zero space . The solutions become a fixed set of solutions.
Therefore, when different residuals calculate the Jacobian for the same state, the linearization points must be consistent to avoid zero space degradation.

3.3 Zero space drift

VIO generally has four degrees of freedom. Yaw+xyz, roll and pitch can determine the absolute direction through gravity, and the scale can be determined through an accelerometer. Therefore, during the sliding window optimization process, the entire sliding window image frame may drift as a whole.

There are usually two methods adopted:
1. Fix the first frame without optimizing, so that other frames will not deviate too much from the first frame.
2. After drifting, calculate how much the first frame has drifted, and move all frames back.

The second method is adopted in VINS