VIO学习笔记（七）—— VINS 初始化和 VIO 系统

学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。

VIO学习笔记（一）—— 概述
VIO学习笔记（二）—— IMU 传感器
VIO学习笔记（三）—— 基于优化的 IMU 与视觉信息融合
VIO学习笔记（四）—— 基于滑动窗口算法的 VIO 系统:可观性和 一致性
VIO学习笔记（五）—— 后端优化实践:逐行手写求解器
VIO学习笔记（六）—— 前端 Frontend

VIO 相关知识回顾

IMU 预积分技术

有关预积分的内容请参考我的这篇博客的IMU 测量值积分部分。
IMU 传感器模型
$$\begin{gathered} {\tilde{\omega }}^b={\omega }^b+b^g+n^g \\ {\tilde{a}}^b=q_{bw}(a^w+g^w)+b^a+n^a \end{gathered}$$
            
将一段时间内的 IMU 数据直接积分起来就能得到两时刻 i, j 之间关于 IMU 的测量约束,即 预积分量:
$$\begin{gathered} {\alpha }_{b_i{b}_j}={\iint }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t^2 \\ {\beta }_{b_i{b}_j}={\int }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t \\ {q}_{b_i{b}_j}={\int }_{t\in [i,j]}{q}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{w}^{b_t}\end{array}\right] \end{gathered}$$

视觉几何基础

相关内容可以参考高翔博士的《视觉SLAM十四讲》。

1. 已知俩图像:特征点提取 (fast),匹配 (光流,特征描述子).
2. 已知俩图像特征匹配点:利用对极几何约束 (E 矩阵,H 矩阵),计算两图像之间的 pose (update to scale).
3. 已知相机 pose, 已知特征点二维坐标: 通过三角化得到三维坐标.
4. 已知 3d 点,2d 特征点:通过 Perspective-n-Point(PnP) 求取新的相机 pose.
           

问题

1. IMU 怎么和世界坐标系对齐,计算初始时刻的 $q_{w{b}_0}$ ?
2. 单目视觉姿态如何和 IMU 轨迹对齐,尺度如何获取?
3. VIO 系统的初始速度 v,传感器 bias 等如何估计?
4. IMU 和相机之间的外参数等…
   

VINS 鲁棒初始化

视觉和 IMU 之间的联系

          
考虑相机坐标系 $c_0$ 为世界坐标系,则利用外参数 $q_{bc}$ , $t_{bc}$ 构建等式
$$\begin{gathered} q_{c_0{b}_k}=q_{c_0{c}_k}\otimes q_{bc}^{-1} \\ s{\tilde{p}}_{c_0{b}_k}=s{\tilde{p}}_{c_0{c}_k}-R_{c_0{b}_k}{p}_{bc} \end{gathered}$$
其中,s 为尺度因子,$\tilde{p}$ 表示非米制单位的轨迹。上式等价于
$$\begin{gathered} {\tilde{p}}_{c_0{b}_k}={\tilde{p}}_{c_0{c}_k}-\frac{1}{s}{R}_{c_0{b}_k}{p}_{bc} \\ {\tilde{p}}_{c_0{c}_k}=\frac{1}{s}{R}_{c_0{b}_k}{p}_{bc}+{\tilde{p}}_{c_0{b}_k} \end{gathered}$$

视觉 IMU 对齐流程

1. 旋转外参数 q bc 未知, 则先估计旋转外参数.
2. 利用旋转约束估计陀螺仪 bias.
   $$q_{c_0{b}_k}=q_{c_0{c}_k}\otimes q_{bc}^{-1}$$
3. 利用平移约束估计重力方向,速度,以及尺度初始值.
   $$s{\tilde{p}}_{c_0{b}_k}=s{\tilde{p}}_{c_0{c}_k}-R_{c_0{b}_k}{p}_{bc}$$
4. 对重力向量 $g^{c_0}$ 进行进一步优化.
5. 求解世界坐标系 w 和初始相机坐标系 $c_0$ 之间的旋转矩阵 $q_{w{c}_0}$ ,并将轨迹对齐到世界坐标系。

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

相邻两时刻 k, k + 1 之间有:IMU 旋转积分 $q_{b_k{b}_{k+1}}$ ,视觉测量$q_{c_k{c}_{k+1}}$ 。则有:
$$q_{b_k{b}_{k+1}}\otimes q_{bc}=q_{bc}\otimes q_{c_k{c}_{k+1}}$$
图形解释为;

上式可写成:
$$([q_{b_k{b}_{k+1}}{]}_L-[q_{c_k{c}_{k+1}}])q_{bc}=Q_{k+1}^k\cdot q_{bc}=0$$
其中,$[\cdot {]}_L$ , $[\cdot {]}_R$ 表示 left and right quaternion multiplication。
将多个时刻线性方程累计起来,并加上鲁棒核权重得到:
          
其中:
        
由旋转矩阵和轴角之间的关系 $tr(R)=1+2cos\theta$,能得到角度误差 $r$ 的计算为:
    
公式的求解同样采用 SVD 分解,即最小奇异值对应的奇异向量。
具体代码见:initial_ex_rotation.cpp 函数 CalibrationExRotation().

 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:
      

其中, B 表示所有的图像关键帧集合,另有预积分的一阶泰勒近似:
            
公式为普通的最小二乘问题,求取雅克比矩阵,构建正定方程HX = b 即可以求解。
具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias().

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 坐标系下的表示。$g^{c_0}$ 为重力向量在第 0 帧相机坐标系下的表示。s 表示尺度因子,将视觉轨迹拉伸到米制单位。
imu预积分误差：
          
预积分量约束,世界坐标系 w 下有:
       
将世界坐标系 w 换成相机初始时刻坐标系 $c_0$ 有:

将公式进行简单整理有:

将待估计变量放到方程右边,有:

公式中:

对以上公式进行说明;

转化成线性最小二乘问题对状态量进行求解:
              
具体代码见:initial_aligment.cpp 函数 LinearAlignment().

bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 3 + 1;

    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
    b.setZero();

    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    int i = 0;
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
    {
        frame_j = next(frame_i);

        MatrixXd tmp_A(6, 10);
        tmp_A.setZero();
        VectorXd tmp_b(6);
        tmp_b.setZero();

        double dt = frame_j->second.pre_integration->sum_dt;

        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
        tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     
        tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
        //cout << "delta_p   " << frame_j->second.pre_integration->delta_p.transpose() << endl;
        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
        tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
        tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
        //cout << "delta_v   " << frame_j->second.pre_integration->delta_v.transpose() << endl;

        Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
        //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
        //MatrixXd cov_inv = cov.inverse();
        cov_inv.setIdentity();

        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
        b.segment<6>(i * 3) += r_b.head<6>();

        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
        b.tail<4>() += r_b.tail<4>();

        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
    }
    A = A * 1000.0;
    b = b * 1000.0;
    x = A.ldlt().solve(b);
    double s = x(n_state - 1) / 100.0;
    // ROS_DEBUG("estimated scale: %f", s);
    g = x.segment<3>(n_state - 4);
    // ROS_DEBUG_STREAM(" result g     " << g.norm() << " " << g.transpose());
    if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    {
        return false;
    }

    RefineGravity(all_image_frame, g, x);
    s = (x.tail<1>())(0) / 100.0;
    (x.tail<1>())(0) = s;
    // ROS_DEBUG_STREAM(" refine     " << g.norm() << " " << g.transpose());
    if(s < 0.0 )
        return false;   
    else
        return true;
}

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

疑问:为什么需要优化重力向量

利用公式(16)求解重力向量 $g^{c_0}$ 过程中,并没有加入模长限制$\Vert gc0\Vert =9.81$。三维变量$g^{c_0}$实际只有两个自由度。

重力向量参数化

三维向量自由度为 2,可以采用球面坐标进行参数化:

              
            
其中,$w_1$, $w_2$ 为待优化变量
          
待优化变量变为:
              
观测方程变为:

采用最小二乘对 $X_I$进行重新优化。

将相机坐标系对齐世界坐标系

对齐流程:

1. 找到 $c_0$ 到 w 系的旋转矩阵 $R_{w{c}_0}=exp([\theta u])$
   
2. 把所有$c_0$ 坐标系下的变量旋转到 w 下。
3. 把相机平移和特征点尺度恢复到米制单位。
4. 至此,完成了系统初始化过程。

        

VINS 系统

VINS 系统三大块

1. 前端,数据处理:特征提取匹配,imu 积分
2. 初始化:系统初始状态变量(重力方向,速度,尺度等等)
3. 后端:滑动窗口优化

后端滑动窗口优化

VINS 系统优化的状态变量为:
          
通过最小化滑动窗口中的残差项来估计系统的状态变量:

注意:其中鲁棒核函数 $\rho (\cdot )$ 仅处理视觉 outlier.

参考资料：

1. Zhenfei Yang and Shaojie Shen. “Monocular visual–inertial state estimation with online initialization and camera–IMU extrinsic calibration”. In: IEEE Transactions on Automation Science and Engineering 14.1 (2016), pp. 39–51.
2. Tong Qin and Shaojie Shen. “Robust initialization of monocular visual-inertial . estimation on aerial robots”. In: . 2017 .IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE.2017,pp. 4225–4232
3. Janne Mustaniemi et al. “Inertial-based scale estimation for structure from motion on mobile devices”. In:2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE. 2017, pp. 4394–4401.
4. Javier Domínguez-Conti et al. “Visual-Inertial SLAM Initialization: A General Linear Formulation and a Gravity-Observing Non-Linear Optimization”. In: 2018 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE. 2018, pp. 37–45.