不一样的静态初始化——OpenVins

今天讲一下黄国权老师实验室开源的OpenVins工程中的IMU初始化，一般VIO初始化分为两种，一种是静态初始化，一种是动态初始化，而OpenVins则利用加速度的方差差异将运动分为两种状态，一种是静止的，一种是刚开始运动的。个人比较符合应用情况，而且我觉得这个想法挺不错，很有意思。

VIO初始化就要任务一般是估计重力方向，速度，位置，IMU的偏置，尺度。而而在openvins中，只估计重力方向，IMU的偏置。

其实在代码里主要写了三个初始化模式，但是动态初始化好像没写出来，不知道现在有没有出来，大家可以去看看

1) GroundTruth初始化

2 )静止初始化

3 )动态初始化

而我们用到的是静态初始化，该初始化不同于smsckf开源代码中的初始化。

首先，将IMU数据分为两个部分，一个是静止的，一个是跳跃的。所谓“跳跃”是指相机从静止到运动的过程，如下图的w3阶段

 在代码中是通过时间的方式划分w2和w3。那么如何判断w3就是跳跃的窗口？openvins采用加速度的方差进行判断，如果方差大于一个阈值，则认为是跳跃窗口w3，如果方差小于阈值，则认为是静止的。然后通过静止的窗口w2进行初始化。这里个人觉得跳跃窗口的信息不应就这么不用了。

现在展示一下OpenVins初始化的大致流程：

 算法中用到了在估计重力坐标系的时候用到了施密特正交化，原理如下：

 代码中是这一样操作的，静止情况下，我们可以确定加速度在世界坐标系下为（0，0，g）,将窗口w2的am2_avg除以其模长，得到平均加速度的方向（单位向量），即为世界系ｚ轴的方向在imu坐标系上的投影：

，通过z轴我们可以利用正交化确定一个坐标系。

 这样我们又可以确定x轴，最后通过x与z叉乘，得到y轴。这样就可以确定初始旋转量了。

静止时刻的陀螺仪理想测量值（角速度）应该为0，因此陀螺仪的bias即窗口w2陀螺仪数据的平均值；静止时的加速度计bias为加速度平均值与实际重力加速度的差。

相关代码如下：

bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix<double,4,1> &q_GtoI0, Eigen::Matrix<double,3,1> &b_w0,
Eigen::Matrix<double,3,1> &v_I0inG, Eigen::Matrix<double,3,1> &b_a0, Eigen::Matrix<double,3,1> &p_I0inG, bool wait_for_jerk) {

// Return if we don't have any measurements
// 如果没有IMU数据，则返回
if(imu_data.empty()) {
return false;
}

// Newest imu timestamp
//最新的IMU时间戳
double newesttime = imu_data.at(imu_data.size()-1).timestamp;

// First lets collect a window of IMU readings from the newest measurement to the oldest
// 这里分为两个窗口，一个是从静态跳到动态，一个是就静态
std::vector<IMUDATA> window_newest, window_secondnew;
for(IMUDATA data : imu_data) {
if(data.timestamp > newesttime-1*_window_length && data.timestamp <= newesttime-0*_window_length) {
window_newest.push_back(data);
}
if(data.timestamp > newesttime-2*_window_length && data.timestamp <= newesttime-1*_window_length) {
window_secondnew.push_back(data);
}
}

// Return if both of these failed
if(window_newest.empty() || window_secondnew.empty()) {
//printf(YELLOW "InertialInitializer::initialize_with_imu(): unable to select window of IMU readings, not enough readings\n" RESET);
return false;
}

// Calculate the sample variance for the newest one
// a_avg平均加速度
Eigen::Matrix<double,3,1> a_avg = Eigen::Matrix<double,3,1>::Zero();
for(IMUDATA data : window_newest) {
a_avg += data.am;
}
a_avg /= (int)window_newest.size();
// a_var方差，通过方差可以判断运动的激励够不够
double a_var = 0;
for(IMUDATA data : window_newest) {
a_var += (data.am-a_avg).dot(data.am-a_avg);
}
a_var = std::sqrt(a_var/((int)window_newest.size()-1));

// If it is below the threshold and we want to wait till we detect a jerk
// 如果运动激励不足，说明是静止的，还没运动，那就不初始化了
if(a_var < _imu_excite_threshold && wait_for_jerk) {
printf(YELLOW "InertialInitializer::initialize_with_imu(): no IMU excitation, below threshold %.4f < %.4f\n" RESET,a_var,_imu_excite_threshold);
return false;
}

// Return if we don't have any measurements
//if(imu_data.size() < 200) {
// return false;
//}

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

// Calculate variance of the
double a_var2 = 0;
for(IMUDATA data : window_secondnew) {
a_var2 += (data.am-linavg).dot(data.am-linavg);
}
a_var2 = std::sqrt(a_var2/((int)window_secondnew.size()-1));

// If it is above the threshold and we are not waiting for a jerk
// Then we are not stationary (i.e. moving) so we should wait till we are
//静止的窗口运动激励要小于阈值才能继续初始化
if((a_var > _imu_excite_threshold || a_var2 > _imu_excite_threshold) && !wait_for_jerk) {
printf(YELLOW "InertialInitializer::initialize_with_imu(): to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET,a_var,a_var2,_imu_excite_threshold);
return false;
}

// Get z axis, which alines with -g (z_in_G=0,0,1)
// 静止时加速度计测量值为g
// 确定z轴方向后，我们利用施密特正交化构建单位坐标系。施密特正交化过程为
// v1 = x1 
// v2 = x2-(x2*v1)/(v1*v1)*v1
// 还剩下一个轴可以利用两个轴的叉乘来确定
Eigen::Vector3d z_axis = linavg/linavg.norm();

// Create an x_axis
// x轴
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
// 通过叉乘确定y轴
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);

// Set our biases equal to our noise (subtract our gravity from accelerometer bias)
//静止的时候，角速度应该就是bias了
Eigen::Matrix<double,3,1> bg = angavg;
//则加速度bias如下，因为静止加速度就是重力和bias了
Eigen::Matrix<double,3,1> ba = linavg - quat_2_Rot(q_GtoI)*_gravity;

// Set our state variables
time0 = window_secondnew.at(window_secondnew.size()-1).timestamp;
q_GtoI0 = q_GtoI;
b_w0 = bg;
v_I0inG = Eigen::Matrix<double,3,1>::Zero();
b_a0 = ba;
p_I0inG = Eigen::Matrix<double,3,1>::Zero();

// Done!!!
return true;

}

个人觉得这个想法在静态初始化中很不错，可以和动态初始化结合起来，更好的评估VIO联合初始化，不然那个动态窗口信息不是浪费了？