0 相关参考

四元数
VINS-Mono代码笔记（七）

《Quaternion kinematics for error-state kalman filter》、《Indirect Kalman Filter for 3D Attitude Estimation》、《SLAM十四讲》

1 旋转角度距离计算

2VINS中相关代码

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    //solveRelativeR函数中可以根据corres中特征点的对应关系计算出前后两帧图像之间的旋转矩阵，加到Rc中，Rc表示相机位姿的旋转矩阵
    Rc.push_back(solveRelativeR(corres));
    //delta_q_imu为IMU预积分得到的旋转四元数，转换成矩阵形式存入Rimu当中。则Rimu中存放的是imu预积分得到的旋转矩阵
    Rimu.push_back(delta_q_imu.toRotationMatrix());
    //每帧IMU相对于起始帧IMU的R,ric初始化值为单位矩阵，则Rc_g中存入的第一个旋转向量为IMU的旋转矩阵
    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]);    //IMU的旋转矩阵
        //旋转角度距离计算，这里计算的是图像帧和IMU数据之间的旋转角度的距离
        //TODO 四元数函数angularDistance()操作参考链接：[https://blog.csdn.net/hzwwpgmwy/article/details/84846016#1__116]
        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;
}