slam 基本算法 --- VINS-mono 之【相机与IMU外参Ric估计】

注

vins-mono是使用 机器人手眼标定的方法，计算出外参中的旋转 $R_c^i$ ，也可以是 $R_c^b$ （把imu坐标系当作body坐标系）

本文将详细进行推导，以及与代码对应；

1. 理论

1.1 前言

对于 $k$与 $k+1$帧相机之间的位姿 $R_{c_{k+1}}^{c_k}$，可以直接通过两帧之间的视觉测量（对极约束，分解本质矩阵E计算得到），也可以先通过IMU积分获得$R_{i_{k+1}}^{i_k}$ ,然后根据 $R_c^i$,经过下面的变换

$$R_{c_{k+1}}^{c_k}=R_i^c{R}_{i_{k+1}}^{i_k}{R}_c^i（1）$$
也可以得到$R_{c_{k+1}}^{c_k}$

由相机和imu分别计算出来的 $R_{c_{k+1}}^{c_k}$ 存在差异，构建最小二乘问题，$Ax=0$,SVD分解A矩阵，解出非零解，即最佳的 $R_c^i$

1.2 利用旋转约束估计外参数

相邻两个时刻 k, k+1 之间 $R_{i_{k+1}}^{c_k}$ 存在 ：

$$R_i^c{R}_{i_{k+1}}^{i_k}=R_{c_{k+1}}^{c_k}{R}_i^c$$

$$q_i^c\otimes q_{i_{k+1}}^{i_k}=q_{c_{k+1}}^{c_k}\otimes q_i^c$$

四元数 $q=[xyzw{]}^T=\left[\begin{array}{c}{q}_v\\ q_w\end{array}\right]$ 的左乘矩阵与右乘矩阵：
$$[q{]}_L=q_{w}I+\left[\begin{array}{cc}{\left[q_v\right]}_{\times }& q_v\\ -q_v^T& 0\end{array}\right],[q{]}_R=q_{w}I+\left[\begin{array}{cc}-{\left[q_v\right]}_{\times }& q_v\\ -q_v^T& 0\end{array}\right]$$

得：
$$[q_{i_{k+1}}^{i_k}{]}_R{q}_i^c=[q_{c_{k+1}}^{c_k}{]}_L{q}_i^c$$

{ [ q c k + 1 c k ] L − [ q i k + 1 i k ] R } q i c = 0 \{[q^{c_k}_{c_{k+1}}]_{L} - [q^{i_k}_{i_{k+1}} ]_{R} \} q^c_{i} = 0 {[qck+1​ck​​]L​−[qik+1​ik​​]R​}qic​=0

通过多对 $q_{c_{k+1}}^{c_k}，q_{i_{k+1}}^{i_k}$ 可以构建超定方程
$$A=\left\{\begin{array}{l}{\left[q_{i_k{i}_{k+1}}^1\right]}_L-{\left[q_{c_k{c}_{k+1}}^1\right]}_R\\ {\left[q_{i_k{i}_{k+1}}^2\right]}_L-{\left[q_{c_k{c}_{k+1}}^2\right]}_R\\ \cdots \\ {\left[q_{i_k{i}_{k+1}}^n\right]}_L-{\left[q_{c_k{c}_{k+1}}^n\right]}_R\end{array}\right\},A\cdot q_i^c=0$$
通过SVD分解 解出 $q_i^c$ ,转为旋转矩阵 $R_i^c$,取逆得 $R_c^i$

2. 代码实践

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)

• corres 两帧之间匹配的特征点 归一化坐标，通过对极约束，构建本质矩阵E，分解得到两帧之间的旋转矩阵， solveRelativeR(corres) 函数实现
• delta_q_imu 为相邻两时刻 IMU预计分旋转量，四元数表示
• 传入进来的 匹配的归一化坐标，IMU预计分旋转量，会先存在vector中，因为需要多对信息，构建超定方程矩阵 A
• calib_ric_result 为待求的外参数 $R_c^i$

 Rc.push_back(solveRelativeR(corres));
 Rimu.push_back(delta_q_imu.toRotationMatrix());
 Rc_g.push_back(ric.inverse() * delta_q_imu * ric);

• Rc 存储直接通过相机测量得到 $R_{c_{k+1}}^{c_k}$
• Rimu 存储传入的 delta_q_imu
• Rc_g 对应上面的公式 （1），通过imu陀螺仪计算出来的 $R_{c_{k+1}}^{c_k}$ ，用于与相机测量得到 $R_{c_{k+1}}^{c_k}$，进行比较，若差异过大，使用简易的核函数弱化异常值

当 frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25成立，则完成计算

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    Rc.push_back(solveRelativeR(corres));
    Rimu.push_back(delta_q_imu.toRotationMatrix());
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);

    Eigen::MatrixXd A(frame_count * 4, 4);
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);

        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);

        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        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;

        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;

        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

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