Vins-Fusion initialization - Initialize gyro bias Bgs

Basic idea : After obtaining the IMU-Camera external parameter rotation, transform the camera rotation to the IMU system through the external parameter. In theory, this rotation should be consistent with the corresponding rotation under the IMU system, and the difference is 0. However, due to the error (angular velocity deviation It is still an estimated value) and will not be 0, then construct a least squares problem to minimize the rotation difference and optimize the angular velocity bias.

1. Theoretical basis of zero offset solution

1. According to the basic idea, it can be transformed into a least squares problem, as follows:

$arg min\sum \left \| (q_{i_{k}i_{k+1}})^{-1} \cdot (q_{c_{k}c_{k+1}})\right \|$

According to the preliminary knowledge, its Gauss-Newton solution is:

$J^{T}J\bigtriangleup x = -J^{T}f(x)$

Among them, f(x) is the residual, and J is the Jacobian of the residual to the optimization variable. Here the optimization variable is bg, so J is the Jacobian of the rotation residual to the angular velocity bias.

$J^{T}_{d_{q}d_{bg}}J_{d_{q}d_{bg}} \delta b_{g}= -J^{T}_{d_{q}d_{bg}}r$

Using LDLT to solve in Vins-Fusion

$\delta b_{g} = A.ldlt().solve(b))$

Among them, A is $J^{T}_{d_{q}d_{bg}}J_{d_{q}d_{bg}}$ , b is $-J^{T}_{d_{q}d_{bg}}r$ , here special attention is paid to the solution of the residual r.

The residual r is obtained by multiplying the rotation obtained by the two frames before and after the camera $q_{c_{k}c_{k+1}}$ and the imu pre-integration between the two frames $q_{i_{k}i_{k+1}}$ . If there is no error, the result is 0. However, due to the existence of the error, the result should be a small amount, so when solving the residual r, the imaginary part of 2 times is directly taken out, that is, in the code:

tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec()

2. After obtaining the bias bg, update the Bgs in the sliding window

for (int i = 0; i <= WINDOW_SIZE; i++)
	Bgs[i] += delta_bg;

3. After updating the bias Bgs, recalculate the pre-integration of the last frame of the sliding window

for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
{
	frame_j = next(frame_i);
	frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
}

For a complete analysis, see the source code analysis below.

2. Source code analysis

The detailed source code analysis is as follows:

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    // 从滑窗第一帧遍历到倒数第二帧
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        // 与前一帧之间的旋转
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;
    }
    delta_bg = A.ldlt().solve(b);
    ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    // 更新偏置
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

    // 更新偏置之后，重新计算积分
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

3. Preliminary knowledge: convex optimization method

Commonly used convex optimization methods, gradient descent, Newton method, Gauss-Newton method, LM, are as follows: