Vins-Fusion初始化-初始化陀螺零偏Bgs

基本思想：在得到IMU-Camera外参旋转之后，将camera的旋转通过外参变换到IMU系下，理论上这个旋转应该与IMU系下对应的旋转一致，差为0，但是由于误差（角速度偏置目前还是个估计值）的存在，不会为0，那么构建最小二乘问题，最小化旋转差量，优化角速度偏置。

一、零偏求解理论基础

1.根据基本思想可将其转化为最小二乘问题，如下：

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

根据预备知识，其高斯牛顿的解为：

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

其中，f(x)为残差，J为残差对优化变量的雅克比。此处优化变量为bg，故J为旋转残差对角速度偏置的雅克比。

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

Vins-Fusion 中使用LDLT求解

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

其中，A 为 $J^{T}_{d_{q}d_{bg}}J_{d_{q}d_{bg}}$ ，b为 $-J^{T}_{d_{q}d_{bg}}r$ ，这里特别注意残差r的求解。

残差r是前后两帧相机求得的旋转 $q_{c_{k}c_{k+1}}$ 与两帧间通过imu预积分求得的 $q_{i_{k}i_{k+1}}$ ，乘积得到的，如果无误差结果为0。但由于误差的存在结果应该是一个小量，故在求解残差r时直接将2倍的虚部取出，即代码中：

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

2.求得偏置bg后，更新滑窗内的Bgs

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

3.更新偏置Bgs后，重新计算滑窗最后一帧的预积分

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]);
}

完整解析见下述源码解析。

二、源码解析

详细源码解析如下：

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]);
    }
}

三、预备知识：凸优化方法

常用的凸优化方法，梯度下降、牛顿法、高斯牛顿法、LM，具体如下：