OpenVINS系统中，只估计了重力方向、加速度计/陀螺仪biases。
初始化函数位于：ov_msckf/src/core/VioManager.cpp 中
Manager::try_to_initialize()
静止初始化是指在视频序列开始阶段，需要有一段静止状态。
系统利用静止状态的IMU数据来估计重力方向、加速度计陀螺仪biases。
静止初始化需要检测imu的跳变过程：
其中包含两个状态：静止状态、跳变状态
通过设定一个阈值［_imu_excite_threshold］,可以判断imu是否得到足够的激励，从得到运动激励前的一段静止状态下的imu测量值，根据静止状态测量值，估算出imu初始化参数。
主要步骤：
代码框图如下：
step１：计算初始旋转矩阵
窗口w2的加速度计平均值 ： am2_avg
窗口w2的陀螺仪平均值 ： wm2_avg
因为，在世界系下，静止时的加速度测量值为（0,0,g）
将窗口w2的am2_avg除以其模长，得到平均加速度的方向（单位向量），即为世界系ｚ轴的方向在imu坐标系上的投影：
确定z轴方向后，通过施密特正交化构建单位坐标系。
假设，e1为imu系的xi轴对应的单位方向向量：
将xi轴投影到 zw-xw平面，求出xw轴在imu坐标系下的方向向量：
y轴方向由x轴和z轴叉乘得到:
通过上述步骤，得到了从世界系w到imu坐标系的初始旋转 Rwi=[x y z]
step2：计算陀螺仪bias：
静止时刻的陀螺仪理想测量值（角速度）应该为0，因此陀螺仪的bias即窗口w2陀螺仪数据的平均值 [wm2_avg]
step3：计算加速度计bias:
静止时的加速度计bias为加速度平均值与实际重力加速度的差：
g为世界系下的重力加速度，Ｒ为第一步求出来的旋转矩阵，将重力矢量从世界系投影到imu坐标系下。
在 feed_measurement_stereo／feed_measurement_monocular 过程中调用初始化函数try_to_initialize()
bool VioManager::try_to_initialize() {
// Returns from our initializer
double time0;
Eigen::Matrix<double, 4, 1> q_GtoI0;
Eigen::Matrix<double, 3, 1> b_w0, v_I0inG, b_a0, p_I0inG;
// Try to initialize the system
// We will wait for a jerk if we do not have the zero velocity update enabled
// Otherwise we can initialize right away as the zero velocity will handle the stationary case
bool wait_for_jerk = (updaterZUPT == nullptr);
bool success = initializer->initialize_with_imu(time0, q_GtoI0, b_w0, v_I0inG, b_a0, p_I0inG, wait_for_jerk);
// Return if it failed
if (!success) {
return false;
}
// Make big vector (q,p,v,bg,ba), and update our state
// Note: start from zero position, as this is what our covariance is based off of
Eigen::Matrix<double,16,1> imu_val;
imu_val.block(0,0,4,1) = q_GtoI0;
imu_val.block(4,0,3,1) << 0,0,0;
imu_val.block(7,0,3,1) = v_I0inG;
imu_val.block(10,0,3,1) = b_w0;
imu_val.block(13,0,3,1) = b_a0;
state->_imu->set_value(imu_val);
state->_imu->set_fej(imu_val);
state->_timestamp = time0;
startup_time = time0;
// Cleanup any features older then the initialization time
trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp);
if(trackARUCO != nullptr) {
trackARUCO->get_feature_database()->cleanup_measurements(state->_timestamp);
}
// Else we are good to go, print out our stats
printf(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET,state->_imu->quat()(0),state->_imu->quat()(1),state->_imu->quat()(2),state->_imu->quat()(3));
printf(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET,state->_imu->bias_g()(0),state->_imu->bias_g()(1),state->_imu->bias_g()(2));
printf(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET,state->_imu->vel()(0),state->_imu->vel()(1),state->_imu->vel()(2));
printf(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET,state->_imu->bias_a()(0),state->_imu->bias_a()(1),state->_imu->bias_a()(2));
printf(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET,state->_imu->pos()(0),state->_imu->pos()(1),state->_imu->pos()(2));
return true;
}
继而调用 initialize_with_imu (time0, q_GtoI0, b_w0, v_I0inG, b_a0, p_I0inG, wait_for_jerk);
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) {
// 若无imu测量值，则返回
if(imu_data.empty()) {
return false;
}
// 最新的imu数据时间
double newesttime = imu_data.at(imu_data.size()-1).timestamp;
// _window_length 为launch中设定的ｉｍｕ初始化窗口的采用时间：0.5~2s
// 取最近两段时间内的ｉｍｕ数据进行用于初始化
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
// 计算最近一个窗口内IMU加速度方差
Eigen::Matrix<double,3,1> a_avg = Eigen::Matrix<double,3,1>::Zero();
for(IMUDATA data : window_newest) {
a_avg += data.am;
//cout<<data.am<<endl;
}
a_avg /= (int)window_newest.size();
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));
//方差过小
// _imu_excite_threshold 是launch中设定的判断ｉｍｕ激励的阈值，高于阈值则表示运动，低于阈值为静止
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;
}
// 方差够大
// 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加速度方差
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)
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);
// Set our biases equal to our noise (subtract our gravity from accelerometer bias)
Eigen::Matrix<double,3,1> bg = angavg;
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;
}
此处进行imu初始化，是通过静止时加速度计测量值与重力矢量方向一致的原理，得到世界坐标系ｗ到ｉｍｕ坐标系的相对旋转Ｒ．（ｚ轴（０，０，１））
在其他常用的vio系统中，通常将视觉的第一帧作为世界系，由于相机与ｉｍｕ之间的相对关系已知，因此可方便地得到相机系相对于世界系的位姿，以及ｉｍｕ系相对于世界系的位姿