Vins-Fusion之 相机—IMU在线标定（十一）

计算找到相机和 IMU 之间的旋转关系（外参旋转 ric）

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);//把 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);// 如果角度差大，说明当前估计的ric不好，降低权重
        ROS_DEBUG(
            "%d %f", i, angular_distance);

        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;//使用 Huber 权重函数，对大角度差进行抑制
        ++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();
    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;
}

这个是整个Camera和IMU标定的过程代码，通过：相机看到的旋转 = 转换关系 × IMU感受到的旋转 × 转换关系的逆 求解转换关系。

阶段1：收集数据

frame_count++;  // 计数器+1（第几次调用）
Rc.push_back(solveRelativeR(corres));  // 视觉算出的旋转
Rimu.push_back(delta_q_imu.toRotationMatrix());  // IMU算出的旋转
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);  // 把IMU旋转转换到相机系

含义：

• Rci：第 i 次调用时，视觉算出的两帧相机相对旋转

• Rimui：第 i 次调用时，IMU 算出的两帧 IMU 相对旋转

• Rc_gi：用当前估计的 ric 把 IMU 旋转转换到相机系

阶段2：构建方程

Eigen::MatrixXd A(frame_count * 4, 4);  // 大矩阵：行数=帧数×4，列数=4
A.setZero();  // 初始化为0

目的：构建线性方程组 A×x=0，其中 x 是外参旋转的四元数

for (int i = 1; i <= frame_count; i++)  // 遍历所有收集的数据
{
    // 1. 计算角度差（用于权重）
    Quaterniond r1(Rc[i]);      // 视觉旋转（四元数）
    Quaterniond r2(Rc_g[i]);    // IMU旋转转换到相机系（四元数）
    
    double angular_distance = 180 / M_PI * r1.angularDistance(r2);
    // 如果角度差大，说明当前估计的ric不好，降低权重
    double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;

Huber 权重：

• 角度差 ≤ 5°：权重 = 1.0

• 角度差 > 5°：权重 = 5.0 / 角度差（降低影响）

    // 2. 构造四元数乘法的矩阵形式
    Matrix4d L, R;  // 两个4×4矩阵
    
    // L矩阵：表示 q_camera × q_ext 的乘法
    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;
    
    // R矩阵：表示 q_ext × q_imu 的乘法
    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;

四元数乘法的矩阵表示：

阶段3：求解方程

JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);  // SVD分解
Matrix<double, 4, 1> x = svd.matrixV().col(3);  // 取最小奇异值对应的向量
Quaterniond estimated_R(x);  // 这就是估计的外参旋转（四元数）
ric = estimated_R.toRotationMatrix().inverse();  // 转换为旋转矩阵并求逆

SVD 求解：

• 对 A×x=0做 SVD：A=UΣVT

• 最小奇异值对应的 V 的列就是最优解

• 取最后一列（第3列，索引从0开始）

为什么求逆？

• 代码中估计的是 ric 的逆，所以最后要再求一次逆

阶段4：判断收敛

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;  // 继续收集数据

收敛判断：

• frame_count >= WINDOW_SIZE：收集了足够的数据（通常10帧）

• ric_cov(1) > 0.25：第二个奇异值足够大，说明解稳定