VINS-Mono-相机与IMU外参标定原理及源码解析

前言

相机与IMU的标定方法很多，有在线和离线两种方式．其中通过Kalibr工具箱进行标定的方法属于离线标定，并且还依赖场景中的标定板，是很麻烦的一种标定方法．而在线标定方法操作简便，不需要特定的场景布置，直接多角度移动设备即可实现标定，因此是极力推崇的方法，本文将介绍VINS-Mono中在线标定相机与IMU外参的方法．

旋转部分标定

• 原理推导
  说明: ${\mathbf{q}}_{b_{k+1}}^{b_k}$是从时刻$b_k$到时刻$b_{k+1}$过程中IMU测量值姿态预积分结果
           ${\mathbf{q}}_{c_{k+1}}^{c_k}$是$b_{k+1}$时刻图像帧相对于$b_k$时刻图像帧的姿态变化(通过本质矩阵求解)
           ${\mathbf{q}}_c^b$ 是相机相对于IMU的旋转变换
  根据旋转矩阵性质可得:
  $${\mathbf{q}}_{b_{k+1}}^{b_k}={\mathbf{q}}_{c_k}^{b_k}\otimes {\mathbf{q}}_{c_{k+1}}^{c_k}\otimes {\mathbf{q}}_{b_{k+1}}^{c_{k+1}}\text{\hspace{1em}(1)}$$
  由于 ${\mathbf{q}}_{c_k}^{b_k}={\mathbf{q}}_{c_{k+1}}^{b_{k+1}}={\mathbf{q}}_c^b$，所以有:
  $${\mathbf{q}}_{b_{k+1}}^{b_k}={\mathbf{q}}_c^b\otimes {\mathbf{q}}_{c_{k+1}}^{c_k}\otimes {\mathbf{q}}_b^c\text{\hspace{1em}(2)}$$
  将 ${\mathbf{q}}_b^c$ 移到等式左边有:
  $${\mathbf{q}}_{b_{k+1}}^{b_k}\otimes {\mathbf{q}}_c^b={\mathbf{q}}_c^b\otimes {\mathbf{q}}_{c_{k+1}}^{c_k}\text{\hspace{1em}(3)}$$
  根据文章 Quaternion kinematics for the error-state KF 中的"一些四元素的性质"章节中提到两四元素的乘法可以转换成矩阵与四元素的乘法，转换方法如下:
  $$\begin{array}{rl}{\mathbf{q}}_1\otimes {\mathbf{q}}_2& ={\mathbf{Q}}_1^{+}{\mathbf{q}}_2\to {\mathbf{Q}}_1^{+}=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^T\\ {\mathbf{q}}_v& {\left[\begin{array}{c}{\mathbf{q}}_v\end{array}\right]}_{\times }\end{array}\right]\\ {\mathbf{q}}_1\otimes {\mathbf{q}}_2& ={\mathbf{Q}}_2^{-}{\mathbf{q}}_1\to {\mathbf{Q}}_2^{-}=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^T\\ {\mathbf{q}}_v& -{\left[\begin{array}{c}{\mathbf{q}}_v\end{array}\right]}_{\times }\end{array}\right]\\ {\left[\begin{array}{c}{\mathbf{q}}_v\end{array}\right]}_{\times }& =\left[\begin{array}{ccc}0& -q_z& q_y\\ q_z& 0& -q_x\\ -q_y& q_x& 0\end{array}\right]\\ \mathbf{q}& =\left[\begin{array}{cc}{q}_w& {\mathbf{q}}_v\end{array}\right]\end{array}$$
  其中${\mathbf{Q}}^{+},{\mathbf{Q}}^{-}$在某些博客或书本上分别称为左乘、右乘矩阵．
  根据上面的性质，可将公式(3)写成如下形式:
  $${\mathbf{Q}}_{b_{k+1}}^{b_k+}{\mathbf{q}}_c^b={\mathbf{Q}}_{c_{k+1}}^{c_k-}{\mathbf{q}}_c^b\text{\hspace{1em}(4)}$$
  合并同类项得:
  $$({\mathbf{Q}}_{b_{k+1}}^{b_k+}-{\mathbf{Q}}_{c_{k+1}}^{c_k-}){\mathbf{q}}_c^b=0\text{\hspace{1em}(5)}$$
  假设有n组测量数据用于外参标定，可建立如下方程组:
  $$\{\begin{array}{c}({\mathbf{Q}}_{b_1}^{b_0+}-{\mathbf{Q}}_{c_1}^{c_0-}){\mathbf{q}}_c^b=0\\ ({\mathbf{Q}}_{b_2}^{b_1+}-{\mathbf{Q}}_{c_2}^{c_1-}){\mathbf{q}}_c^b=0\\ ......\\ ({\mathbf{Q}}_{b_n}^{b_{n-1}+}-{\mathbf{Q}}_{c_n}^{c_{n-1}-}){\mathbf{q}}_c^b=0\end{array}\text{\hspace{1em}(6)}$$
  写成矩阵形式:
  $${\left[\begin{array}{c}{\mathbf{Q}}_{b_1}^{b_0+}-{\mathbf{Q}}_{c_1}^{c_0-}\\ {\mathbf{Q}}_{b_2}^{b_1+}-{\mathbf{Q}}_{c_2}^{c_1-}\\ ......\\ {\mathbf{Q}}_{b_n}^{b_{n-1}+}-{\mathbf{Q}}_{c_n}^{c_{n-1}-}\end{array}\right]}_{4n\times 4}{\mathbf{q}}_c^b={\mathbf{A}}_{4n\times 4}{\mathbf{q}}_c^b=0\text{\hspace{1em}(7)}$$
  公式(7)是一个齐次线性方程组，可通过SVD方法求解: 该方法首先对 ${\mathbf{A}}_{4n\times 4}$ 进行SVD分解，然后取最小奇异值对应的特征向量作为${\mathbf{q}}_c^b$最终结果．
• 源码解析

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++; // 总共用于外参标定的数据帧数
    Rc.push_back(solveRelativeR(corres)); //通过本质矩阵求解相邻两帧之间的旋转矩阵 q_c(k+1)_c(k)
    Rimu.push_back(delta_q_imu.toRotationMatrix()); //从b_(k+1)到b_(k)时刻IMU姿态预积分结果 q_b(k+1)_b(k)
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);

    Eigen::MatrixXd A(frame_count * 4, 4); //申明并定义矩阵A，维度为4nx4
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        Quaterniond r1(Rc[i]); //对应公式(4)中的q_c(k+1)_c(k)
        Quaterniond r2(Rc_g[i]); //对应公式(4)中的q_b(k+1)_b(k)

        double angular_distance = 180 / M_PI * r1.angularDistance(r2); // 计算相机姿态增量和imu姿态增量之间的夹角
        ROS_DEBUG(
            "%d %f", i, angular_distance);

        // 根据两姿态之间的夹角计算Huber核系数，降噪声影响
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        // 计算左乘矩阵,对应公式(5)中的Q_(+)
        double w = Quaterniond(Rc[i]).w();
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        // 计算右乘矩阵,对应公式(5)中的Q_(-)
        Quaterniond R_ij(Rimu[i]);
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;

        // 对应公式(7)，其实一组数据就可以解算出外参信息，此处累积多组数据构成超定方程，提升结果的准确性
        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    // 进行SVD分解求解齐次线性方程组
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    // 最小奇异值对应的特征向量即为求解的结果
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    ric = estimated_R.toRotationMatrix().inverse();
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
    // 将SVD分解后的最小的三个奇异值作为外参旋转的协方差值(只有对角线上有值)
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

如果算法对相机与IMU之间的平移变换要求不高的话，就可以利用该旋转变换结果进行算法开发．

旋转和平移变换在线优化矫正

problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);

VINS-Mono在获得旋转变换之后，在进行VIO的过程中，将相机与IMU外参作为优化参数在整个算法运行过程中进行矫正，从而保证外参的准确性．

代码注释

https://github.com/chennuo0125-HIT/vins-note