`VINS`中关于陀螺仪零偏的初始化估计

对于窗口中得连续两帧 \(b_{k}\) 和 \(b_{k+1}\) ，已经从视觉SFM中得到了旋转 \(q_{b_{k}}^{c_{0}}\) 和 \(q_{b_{k+1}}^{c_{0}}\) ，从IMU预积分中得到了相邻帧旋转 \(\hat{\gamma}^{b_{k}}_{b_{k+1}}\) 。根据约束方程，联立所有相邻帧，最小化代价函数（论文式(7)）：

\[\min _{\delta b_{w}} \sum_{k \in \mathcal{B}}\left\|q_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}\right\|^{2} \]

其中对陀螺仪偏置求IMU预积分项线性化，有：

\[\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w} \end{array}\right] \]

在具体实现的时候，因为上述约束方程为：

\[q_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}=\left[\begin{array}{l} 1 \\ 0 \end{array}\right] \]

有：

\[\gamma_{b_{k+1}}^{b_{k}}=q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}} \otimes\left[\begin{array}{l} 1 \\ 0 \end{array}\right] \]

代入 \(\delta b_{w}\) 得 :

\[\hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w} \end{array}\right]=q_{b_{k}}^{c 0-1} \otimes q_{b_{k+1}}^{c 0} \otimes\left[\begin{array}{l} 1 \\ 0 \end{array}\right] \]

只考虑虚部，有 :

\[J_{b_{w}}^{\gamma} \delta b_{w}=2\left(\hat{\gamma}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c} \]

两侧乘以 \(\mathbf{J_{b_{w}}^{\gamma}}^{T}\) ，用LDLT分解求得 \(\delta b_{w}\) 。
在代码中， \(\mathrm{q}_\mathrm{i,j}\) 即 \(q_{b_{k+1}}^{b_{k}}=q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\)
\(\mathrm{tmp}_{-} \mathrm{A}\) 即 \(J_{b_{w}}^{\gamma}\)

\(\mathrm{tmp}_{-} \mathrm{B}\) 即

\[ 2\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k+1}}^{b_{k}}\right)_{v e c}=2\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c} \]

根据上面的代价函数构造 \(\mathrm{Ax}=\mathrm{B}\) 即

\[J_{b_{w}}^{\gamma T} J_{b_{w}}^{\gamma} \delta b_{w}=2 J_{b_{w}}^{\gamma T}\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{\text {vec }} \]

然后采用LDLT分解求得 \(\delta b_{w}\) 。

VINS中的代码

/**
 * @brief   陀螺仪偏置校正
 * @optional    根据视觉SFM的结果来校正陀螺仪Bias -> Paper V-B-1
 *              主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
 *              注意得到了新的Bias后对应的预积分需要repropagate
 * @param[in]   all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
 * @param[out]  Bgs 陀螺仪偏置
 * @return      void
*/
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();
 
        //R_ij = (R^c0_bk)^-1 * (R^c0_bk+1) 转换为四元数 q_ij = (q^c0_bk)^-1 * (q^c0_bk+1)
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        //tmp_A = J_j_bw
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        //tmp_b = 2 * (r^bk_bk+1)^-1 * (q^c0_bk)^-1 * (q^c0_bk+1)
        //      = 2 * (r^bk_bk+1)^-1 * q_ij
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        //tmp_A * delta_bg = tmp_b
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;
 
    }
    // LDLT方法
    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;
    // 得到了新的Bias后对应的预积分需要repropagate
    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]);
    }
}

之所以 A +=tmp_A.transpose() * tmp_A，其实就是 \(A^T Ax=A^Tb\)。在求解 \(Ax=b\) 的最小二乘解时，两边同乘以A矩阵的转置得到的 \(A^TA\) 一定是可逆的。