Vins-mono IMU和相机之间的旋转外参估计

一、概述

具体实现在函数:

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

函数的入参corres是包含两帧之间的多个匹配点对的归一化坐标的vector数组，入参delta_q_imu是通过两帧间陀螺仪积分得到相对旋转（用的四元数 ），输出calib_ric_result就是求解出的结果。

二、原理

构建超定方程即公式

三、代码

// 标定imu和相机之间的旋转外参ric，通过imu和图像计算的旋转使用手眼标定计算获得
// 最小二乘法，构建Ax=0的形式，然后使用奇异值分解（SVD）对矩阵进行分解，最小奇异值对应的右向量就是的非零解
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    // 根据特征关联求解两个连续帧相机的旋转R12
    Rc.push_back(solveRelativeR(corres));
    Rimu.push_back(delta_q_imu.toRotationMatrix());
    // 通过外参把imu的旋转转移到相机坐标系
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);  // 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_DEBU
                    "%d %f", i, angular_distance);
        // 一个简单的核函数
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;
        //! 得到相机对极关系得到的旋转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);    // 作用在残差上面
    }
	//！通过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();
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
    //！判断对于相机与IMU的相对旋转是否满足终止条件    
    //！1.用来求解相对旋转的IMU-Camera的测量对数是否大于滑窗大小。    
    //！2.A矩阵第二小的奇异值是否大于某个阈值，使A得零空间的秩为1
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    // 倒数第二个奇异值，因为旋转是3个自由度，因此检查一下第三小的奇异值是否足够大，通常需要足够的运动激励才能保证得到没有奇异的解
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}