Vins-Fusion 初期化 - ジャイロバイアス Bgs を初期化します。

基本的な考え方: IMU カメラの外部パラメータの回転を取得した後、外部パラメータを通じてカメラの回転を IMU システムに変換します。理論的には、この回転は IMU システムでの対応する回転と一致するはずであり、その差は 0 です。、誤差（角速度偏差。まだ推定値です）により 0 にはなりません。次に、最小二乗問題を構築して回転差を最小限に抑え、角速度バイアスを最適化します。

1. ゼロオフセットソリューションの理論的基礎

1. 基本的な考え方によれば、次のように最小二乗問題に変換できます。

$引数 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$

LDLT を使用して Vins-Fusion で解決する

$\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 は、カメラの前後 2 フレームで得られる回転 $q_{c_{k}c_{k+1}}$ と 2 フレーム間の 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. バイアス背景を取得した後、スライディングウィンドウの 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]);
}

完全な分析については、以下のソースコード分析を参照してください。

2. ソースコード分析

詳細なソースコード分析は次のとおりです。

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. 予備知識：凸最適化法

一般的に使用される凸最適化手法、勾配降下法、ニュートン法、ガウスニュートン法、LM は次のとおりです。