spoon无法初始化至少一个步骤_OpenVINS代码阅读（1）-初始化

在VIO/VINS系统中，初始化是指在系统启动阶段，需要估计(设置)重力方向(gravity direction)、初始速度(initial velocity)、初始位置(initial position)、加速度计以及陀螺仪biases (accelerometer and gyroscope biases)。系统将初始位置和速度设置为0，并且由于IMU积分存在尺度，因此，OpenVINS系统中，只估计了重力方向、加速度计/陀螺仪biases。包含了多种初始化方法，具体见下图：

OpenVINS的初始化方法分类，其中运动初始化暂未实现(2020.11.10).

一. GroundTruth初始化

基于Groundtruth的初始化是值在存在真值数据的情况，直接从GT文件中读取数据，完成系统的初始化。例如，OpenVINS系统直接读取了EuRoC数据的真值，设置state->imu->setvalue()以及state->imu->setfej()完成系统的初始化。

/**

二. 静止初始化

静止初始化是指在视频序列开始阶段，需要有一段静止状态。系统利用静止状态的IMU数据来估计重力方向、加速度计/陀螺仪biases。OpenVINS中，静止初始化包含了两种模式，需要跳变的初始化和不需要跳变的初始化。

静止初始化示意图

“跳变”是指相机从静止到运动的过程。如图中的的开始运动时刻，我们可以设定一个阈值来判断相机是否静止或者运动。图中w1,w2和w3为一固定时间的IMU序列窗口。在w3窗口中，相机处于运动状态。假设w3窗口大于设定阈值，w2小于设定阈值，则利用w2的IMU数据来进行系统初始化，对应“需要跳变的初始化”。假设w1和w2都小于运动阈值，则利用w1中的IMU数据进行系统初始化，对应“不需要跳变的初始化”系统中利用IMU数据的方差来衡量相机是否处于运动状态。

1.需要跳变初始化

     * To initialize from standstill:
     *  a. Collect all inertial measurements
     *  b. See if within the last window there was a jump in acceleration
     *  c. If the jump is past our threshold we should init (i.e. we have started moving)
     *  d. Use the *previous* window, which should have been stationary to initialize orientation
     *  e. Return a roll and pitch aligned with gravity and biases.

算法步骤：

a. 搜集所有的IMU数据

b. 查看当前窗口是否有加速度方差跳变

c. 如果跳变大于阈值则启动初始化

d. 利用上一窗口的IMU数据进行初始化（该窗口应该处于静止状态）

e. 返回与重力对其的roll和pitch，返回加速度计陀螺仪biases

2.不需要跳变初始化

这一过程与1中步骤类似，其中步骤b,c合并为检测当前窗口是否静止。

三. 静止初始化理论

计算初始旋转矩阵：

1. 假设利用窗口w2进行系统初始化，w2中共有n个IMU数据，其中加速度计数据为
   ，陀螺仪数据为
   。（
   和
   都为3x1的向量）则加速度计数据的方差为:

假设方差大于阈值，则进行初始化。

2. 窗口w2的加速度计数据平均值

，则

为z轴方向（静止时加速度计测量值为g）。

3. 确定z轴方向后，我们利用施密特正交化构建单位坐标系。施密特正交化过程为：

因此对于z轴，可得

对于x轴（

），

,则x轴为

y轴方向可由x轴和z轴叉乘得到:

通过上述1,2,3步骤，我们得到了与初始的旋转R=[x y z]。

计算陀螺仪bias：

静止时刻的陀螺仪数据（角速度）应该为0，因此陀螺仪的bias即窗口w2陀螺仪数据的平均值

计算加速度计bias:

,其中g为当地的重力加速度。

这一部分的代码如下：

   // 计算平均加速度和平均角速度 
   // Sum up our current accelerations and velocities
    Eigen::Vector3d linsum = Eigen::Vector3d::Zero();
    Eigen::Vector3d angsum = Eigen::Vector3d::Zero();
    for(size_t i=0; i<window_secondnew.size(); i++) {
        linsum += window_secondnew.at(i).am;
        angsum += window_secondnew.at(i).wm;
    }
    // Calculate the mean of the linear acceleration and angular velocity
    Eigen::Vector3d linavg = Eigen::Vector3d::Zero();
    Eigen::Vector3d angavg = Eigen::Vector3d::Zero();
    linavg = linsum/window_secondnew.size();
    angavg = angsum/window_secondnew.size();

    // 计算IMU与重力加速度之间的旋转矩阵
    // Get z axis, which alines with -g (z_in_G=0,0,1)
    Eigen::Vector3d z_axis = linavg/linavg.norm();
    // Create an x_axis
    Eigen::Vector3d e_1(1,0,0);
    // Make x_axis perpendicular（垂直的） to z
    Eigen::Vector3d x_axis = e_1-z_axis*z_axis.transpose()*e_1;
    x_axis= x_axis/x_axis.norm();
    // Get z from the cross product of these two
    Eigen::Matrix<double,3,1> y_axis = skew_x(z_axis)*x_axis;

    // From these axes get rotation
    Eigen::Matrix<double,3,3> Ro;
    Ro.block(0,0,3,1) = x_axis;
    Ro.block(0,1,3,1) = y_axis;
    Ro.block(0,2,3,1) = z_axis;

    // Create our state variables
    Eigen::Matrix<double,4,1> q_GtoI = rot_2_quat(Ro);

    // 计算加速度计与陀螺仪的biases
    // Set our biases equal to our noise (subtract our gravity from accelerometer bias)
    Eigen::Matrix<double,3,1> bg = angavg;//静止不动时，角速度的均值即为bg
    Eigen::Matrix<double,3,1> ba = linavg - quat_2_Rot(q_GtoI)*_gravity;// 加速度理论上应该为0，用平均值-重力加速度在该方向的投影即为加速度偏置