[VINS]IMU与相机之间旋转量的标定

VINS-Mono^[1]中IMU-Camera外参旋转量\(R_b^c\)的计算方法在他们实验室发的之前的论文有详细讲解^[2]。视觉利用匹配特征点中的基础矩阵求出相机坐标系下两帧的旋转量\(R_{c_k}^{c_{k+1}}\)，通过IMU预积分得到的两帧之间IMU坐标系下的旋转量$ R_{b_k}^{b_{k+1}}$,两个旋转量满足：

\[R_b^c R_{b_k}^{b_{k+1}}=R_{c_k}^{c_{k+1}}R_b^c \tag{1}
\]

四元数表示，则有

\[q_b^c \otimes q_{b_k}^{b_{k+1}}-q_{c_k}^{c_{k+1}} \otimes q_b^c = 0 \tag{2}
\]

将四元数乘法运算化为一个\(4 \times 4\)的矩阵运算，YouTube上有个很好的视频讲解^[3]。伯克利CS184也作出很好的讲解^[4]，使用行向量表示四元数，推过程类似。这里做简单的归纳讲述。两个四元数分别为：\(q_a=\left[\begin{array} {c}{x_a}\\{y_a}\\{z_a}\\{w_a} \end{array}\right]\)，\(q_b=\left[\begin{array} {c}{x_b}\\{y_b}\\{z_b} \\{w_b}\end{array}\right]\)。更具四元数乘法规则，可以得到：

\[q_a \otimes q_b=\left[\begin{array}{c}{ w_b x_a+ z_b y_a-y_b z_a + x_b w_a}\\{- z_b x_a+ w_b y_a+ x_b z_a+ y_b w_a}\\{ y_b x_a- x_b y_a + w_b z_a+ z_b w_a}\\{- x_b x_a- y_b y_a- z_b z_a+ w_b w_a} \end{array}\right]= \left[\begin{array}{c}w_b & z_b & -y_b & x_b\\ -z_b & w_b & x_b & y_b\\y_b & -x_b & w_b & z_b\\ -x_b & -y_b &-z_b & w_b\end{array} \right]\left[\begin{array}{c} x_a \\y_a\\z_a\\w_a \end{array} \right] \tag{3}
\]

记\(q_{xyz}=\left[\begin{array}{c}x & y & z \end{array} \right]\)，\(q_{xyz}^{\wedge}=\left[\begin{array}{c}0 & -z & y \\ z & 0 & -x \\ -y & z & 0 \end{array} \right]\), 则有：

\[q_a \otimes q_b=\left[\begin{array}{c}w_bI-q_{x_b y_b z_b}^{\wedge}& q_{x_b y_b z_b}^{T}\\-q_{x_b y_b z_b} & w_b \end{array}\right]\left[\begin{array}{c} x_a \\y_a\\z_a\\w_a \end{array} \right] 记Q_R(q_{xyz})=\left[\begin{array}{c}w_I-q_{x y z}^{\wedge}& q_{x y z}\\-q_{x y z}^T & w \end{array}\right]_{4 \times 4} \tag{4}
\]

同样的，我们整理\((3)\)式按照\(b\)排序，则有:

\[q_a \otimes q_b=\left[\begin{array}{c}{ w_a x_b - z_a y_b+ y_a z_b+ x_a w_b}\\{ z_a x_b+ w_a y_b- x_a z_b+ y_a w_b}\\{ - y_a x_b+ x_a y_b + w_a z_b + z_a w_b}\\{- x_a x_b- y_a y_b- z_a z_b+ w_a w_b} \end{array}\right]= \left[\begin{array}{c}w_a & -z_a & y_a & x_a\\ z_a & w_a & -x_a & y_a\\-y_a & x_a & w_a & z_a\\ -x_a & -y_a &-z_a & w_a\end{array} \right]\left[\begin{array}{c} x_b \\y_b\\z_b\\w_b \end{array} \right] \tag{5}
\]

那么，同样有我们就有左乘法四元数的矩阵：

\[q_a \otimes q_b=\left[\begin{array}{c}w_aI+q_{x_ay_az_a}^{\wedge}& q_{x_ay_az_a}^{T}\\-q_{x_ay_az_a} & w_a \end{array}\right]\left[\begin{array}{c} x_b \\y_b\\z_b\\w_b \end{array} \right] 记Q_L(q_{xyz})=\left[\begin{array}{c}w_I+q_{x y z}^{\wedge}& q_{x y z}\\-q_{x y z}^T & w \end{array}\right]_{4 \times 4}\tag{6}
\]

然后，我们整理\((2)\)式得到：

\[\left(Q_R(q_{b_k}^{b_{k+1}})-Q_L(q_{c_k}^{c_{k+1}})\right)q_b^c=0 记A=Q_R(q_{b_k}^{b_{k+1}})-Q_L(q_{c_k}^{c_{k+1}}) \tag{7}
\]

取IMU旋转角\(\left\{q_{b_k} ^{b_{k+1}},q_{b_{k+1}} ^{b_{k+2}}...q_{b_{k+n-1}} ^{b_{k+n}}\right\}\),相机旋转角\(\left\{q_{ck} ^{c_{k+1}},q_{c_{k+1}} ^{c_{k+2}}...q_{c_{k+n-1}} ^{c_{k+n}}\right\}\),构建矩阵\(\rm{A}_{4n \times 4}\) 使用SVD分解该最小二乘问题^[5]。

\[A=U_{4n \times 4n} \Sigma_{4n \times 4} V^T_{4 \times 4}
\]

最小二乘解即为最小奇异值对应V的特征向量，即\(A^TA\)最小特征值对应V的特征向量。在VINS-Mono中，加入每组旋转角相差的加入权重Huber，再去最后的一列的特征向量作为最小二乘的解。

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    Rc.push_back(solveRelativeR(corres));//帧间cam的R，由对极几何得到
    Rimu.push_back(delta_q_imu.toRotationMatrix());//帧间IMU的R，由IMU预积分得到
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每帧IMU相对于起始帧IMU的R

    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;
        //求Q_L
        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;
        //求Q_R
        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;
}

[[3]](#CS184)，另外一种方法是，设两个四元数分别为：$q_a=\left[\begin{array} {c}{x_a}\end{array}\right]$，$q_b=\left[\begin{array} {c}{x_b}\end{array}\right]$。

$$q_a \otimes q_b=\left[\begin{array}{c}{x_a w_b+y_a z_b-z_a y_b+w_a x_b}\\{-x_a z_b+y_a w_b+z_a x_b+w_a y_b}\\{x_a y_b-y_a x_b + z_a w_b+w_a z_b}\\{-x_a x_b-y_a y_b-z_a z_b+w_a w_b} \end{array}\right]^T= \left[\begin{array}{c} x_a &y_a&z_a&w_a \end{array} \right]\left[\begin{array}{c}w_b & -z_b & y_b & -x_b\\ z_b & w_b & -x_b & -y_b\\-y_b & x_b & w_b & -z_b\\ x_b & y_b &z_b & w_b\end{array} \right]$$ -->

参考：

[1]VINS-Mono

[2]Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration

[3]Quaternions as 4x4 Matrices - Connections to Linear Algebra

[4]CS184: Using Quaternions to Represent Rotation

[5]SVD分解及线性最小二乘问题

[6]VINS 估计器之外参初始化