从零手写VIO——（六）VINS 初始化和 VIO 系统_人工智能

VIO 相关知识回顾

IMU 传感器模型：

IMU 预积分：

将一段时间内的 IMU 数据直接积分起来就能得到两时刻 i，j 之间关于 IMU 的测量约束，即 预积分量：

VINS 鲁棒初始化

视觉和 IMU 之间的联系

相机坐标系
$c_0$ 为世界坐标系，利用外参数
$q_{bc},t_{bc}$ 构建等式：

其中，
$s$ 为尺度因子，
$\overline{p}$ 表示非米制单位的轨迹。等式(3)等价：

式子中的
$R_{c_0b_k}$ 这样理解：camera 和 imu body 之间有一个
$p_{bc}$ 轨迹的变换，这个变换是在 world 坐标系下，那
$R_{c_0b_k}$ 就是从 world 坐标系（
$c_0$ 坐标系）变换到现在要计算轨迹的
$b_k$ 坐标系下。(4)第一个中第一个式子也可以写成这样：
$\overline{p}_{c_0b_k}=\overline{p}_{c_0c_k}+\frac{1}{s}R_{c_0b_k}p_{cb}$ 。

估计流程

① 旋转外参数
$q_{bc}$ 未知，则先估计旋转外参数. ② 利用旋转约束估计陀螺仪 bias.

③ 利用平移约束估计重力方向，速度，以及尺度初始值.

④ 对重力向量
$g^{c_0}$ 进一步优化. ⑤ 求解世界坐标系
$w$ 和初始相机坐标系
$c_0$ 之间的旋转矩阵
$q_{wc_0}$ ，并将轨迹对齐到世界坐标系（可以用
$g^{c_0}$ 也就是相机坐标系
$c_0$ 的重力加速度，与
$g=\begin{bmatrix}0\\0\\-9.81\end{bmatrix}$ 对比，求算出之间的旋转关系）. 论文：

Zhenfei, Yang, Shaojie,等. Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration[J]. IEEE Transactions on Automation Science & Engineering, 2017.

2. Qin T , Shen S . Robust initialization of monocular visual-inertial estimation on aerial robots[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE, 2017:4225-4232.

利用旋转约束估计外参数旋转
$q_{bc}$

相邻两时刻
$k,k+1$ 之间有：IMU 旋转积分
$q_{b_kb_{k+1}}$ ，视觉测量
$q_{c_kc_{k+1}}$ 。则有：

上式可写成：

其中，
$[·]_L,[·]_R$ 表示 left and right quaternion multiplication。

将多个时刻线性方程累计起来，并加上鲁棒核权重得到：

其中：

由旋转矩阵和轴角之间的关系
$\text{tr}(R)=1+2\cos\theta$ ，能得到角度误差
$r$ 的计算为：

公式 (7) 最小二乘解仍使用 SVD 分解，取最小奇异值对应的奇异向量。这里简单解释一下为什么取
$Q_N^TQ_N$ 的最小奇异值对应的奇异向量，能使得最小二乘问题
$||Q_N·q_{bc}||^2_2$ 最接近零：可以假设
$q_{bc}=\omega_u u_k+\omega_v v\quad(v\bot u_k)$ 是任意的奇异向量，
$\omega$ 是系数，这样一对基向量可以表示在
$q_{bc}$ 的空间下所有向量。
$q_{bc}^TQ_N^TQ_Nq_{bc}$ 就可以展开为：

当
$v=0,u_k=u_{\min}$ 能使问题最小。最小奇异值对应的奇异向量是
$U\Sigma V^T$ 分解后，
$V$ 的最右边一列。

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);

    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;

        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;
}

基于旋转约束的 Gyroscope Bias

如果外参数
$q_{bc}$ 已经标定好，利用旋转约束，可估计陀螺仪 bias：

${q_{c_0b_{k+1}}^c}^{-1}\otimes q_{c_0b_k}^c$ 是相机测得的 k 到 k+1 的变换，
$q_{b_kb_{k+1}}$ 是 imu 测得的，这里用视觉修正 imu。其中，
$[·]_{xyz}$ 表示取虚部，对于这个问题来说，2倍的就是角度
$\theta$ ，因为
$\begin{bmatrix}\cos\frac{\theta}{2}\\\sin\frac{\theta}{2}\Bbb n\end{bmatrix}\underrightarrow{\theta\approx0}\begin{bmatrix}1\\\frac{\theta}{2}\end{bmatrix}$ 。
$B$ 表示所有的图像关键帧集合，另有预积分的一阶泰勒近似：

公式(10)是普通的最小二乘问题，求其雅可比矩阵，构建正定方程
$Hx=b$ 即可求解。前面那个方程是超正定的，即方程个数超过更新变量的个数。

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    delta_bg = A.ldlt().solve(b);
    // ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

初始化速度、重力和尺度因子

需要估计的变量：

其中，
$v_k^{b_k}$ 表示 k 时刻 body 坐标系的速度在 body 坐标系下的表示。
$\Bbb g^{c_0}$ 为重力向量在第 0 帧相机坐标系下的表示。
$s$ 表示 scale 就是尺度因子，将视觉轨迹拉伸到米制单位，统一 imu 和视觉单位。 预积分量约束： 世界坐标系
$w$ 下有：

简单来说：公式就是就是把影响
$p_{wb_j}$ 和
$p_{wb_i}$ 的除预积分外的所有因素都减去了，其实就是等于预积分量。把世界坐标系
$w$ 换成相机初始时刻坐标系
$c_0$ 有：

与↓

结合，有：

$R_{b_kc_0}$ 而不是
$R_{c_0b_k}$ 的原因是
$-\overline{p}_{c_0b_k}$ 的这个负号。将待估计的变量放在方程右边，有：

由于预积分的计算中 imu 测得的角速度和加速度并不是完全满足公式的，存在 bias 和高斯白噪声，可以用协方差矩阵的传递，最终得到公式估计值和真实值的偏差
$n_{b_{k+1}}^{b_k}$ ，构建最小二乘问题求解，即
$\cal X_\it I^k$ 何值时，能使得偏差最小，估计值最接近真实值：

优化重力向量
$\Bbb g^{c_0}$

重力向量在三维空间下是三自由度的，但是加入
$||\Bbb g^{c_0}||=9.81$ 的限定，实际上只剩下两个自由度。多一个限制就少一个自由度这个很好理解吧，和机械设计中低副限制两个自由度、高副限制一个自由度是一样的。 重力向量参数化： 一种简单的参数化方法：一个三维向量，模长一定即
$x^2+y^2+z^2=1$ ，和球面方程相同，可以通过求下图中
$\begin{bmatrix}\theta_1\\\theta_2\end{bmatrix}$ 与球面相交于一点，球心到这一点即重力向量方向。这里的
$\begin{bmatrix}\theta_1\\\theta_2\end{bmatrix}$ 就是两个自由度的参数化。