IMU 座標系 與 相機 座標系旋轉對齊

IMU 座標系 與 相機 座標系旋轉對齊

四元數的左乘和右乘

$q_1 \otimes q_2 = \left[q_1\right]_L q_2, \qquad q_1 \otimes q_2 = \left[q_2\right]_R q_1$
$\left[q\right]_L=q_wI + \begin{bmatrix} 0 & -{q_v}^T \\ q_v & \left[q_v\right]_{\times} \end{bmatrix}, \qquad \left[q\right]_R=q_wI + \begin{bmatrix} 0 & -{q_v}^T \\ q_v & -\left[q_v\right]_{\times} \end{bmatrix}$
  其中：
$\left[a\right]_{\times}\triangleq \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix}$
$\left[q\right]_L= \begin{bmatrix} q_w & -q_x & -q_y & -q_z \\ q_x & q_w & -q_z & q_y \\ q_y & q_z & q_w & -q_x \\ q_z & -q_y & q_x & q_w \end{bmatrix}, \qquad \left[q\right]_R= \begin{bmatrix} q_w & -q_x & -q_y & -q_z \\ q_x & q_w & q_z & -q_y \\ q_y & -q_z & q_w & q_x \\ q_z & q_y & -q_x & q_w \end{bmatrix}$

相機 與 IMU之間的相對旋轉對齊

  假設相機與IMU之間存在一個相對旋轉$Q_{ic}$，$Q_{ic}$表示從IMU轉到相機的四元數。

根據以上條件可得到方程：
$Q_{ic} \otimes Q_{imu} \otimes {Q_{ic}^{-1}} = Q_{cam}$
$Q_{ic} \otimes Q_{imu} = Q_{cam} \otimes Q_{ic}$
根據四元數的左乘和右乘得到：
$\left[Q_{imu}\right]_RQ_{ic} = \left[Q_{cam}\right]_LQ_{ic}$
$\Rightarrow \left(\left[Q_{cam}\right]_L - \left[Q_{imu}\right]_R\right)Q_{ic} = 0$

具體代碼：
代碼中會通過迭代的方式計算$Q_{ic}$，同時計算的過程中會加一些權重。

 bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    //! Step1：滑窗內的幀數加1
    frame_count++;

    //! Step2:計算兩幀之間的旋轉矩陣
    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]);

        //！Step3:求取估計出的相機與IMU之間旋轉的殘差
        //！r = acos((tr(R_cbRbR_bcR_c)-1)/2)
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        //! Step4:計算外點剔除的權重
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        //! Step5：求取係數矩陣
        //! 得到相機對極關係得到的旋轉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;

        //! 得到由IMU預積分得到的旋轉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;
        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    //！Step6：通過SVD分解，求取相機與IMU的相對旋轉
    //！解爲係數矩陣A的右奇異向量V中最小奇異值對應的特徵向量
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    ric = estimated_R.toRotationMatrix().inverse();
    
    //！Step7：判斷對於相機與IMU的相對旋轉是否滿足終止條件
    //！1.用來求解相對旋轉的IMU-Cmaera的測量對數是否大於滑窗大小。
    //！2.A矩陣第二小的奇異值是否大於某個閾值，使A得零空間的秩爲1
    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;
    }
}