okvis代码解读

1. IMU消费者线程：

在imuConsumerLoop()中主要处理imu的propagation

每次imuMeasurementsReceived_队列中出现IMU数据，就会propagate一次，如果刚完成BA优化（需要repropagationNeeded_），则将优化后的状态值作为propagation的初值，否则在上一状态基础上完成状态propagation。

主要对应ImuError::propagation()函数，该函数大概两百行，主要实现OKVIS论文中的 4.2 IMU Kinematics and bias model。

    if (end < nexttime) {
      double interval = (nexttime - it->timeStamp).toSec();
      nexttime = t_end;
      dt = (nexttime - time).toSec();
      const double r = dt / interval;
      omega_S_1 = ((1.0 - r) * omega_S_0 + r * omega_S_1).eval();  //cha zhi
      acc_S_1 = ((1.0 - r) * acc_S_0 + r * acc_S_1).eval();
    }

    if (dt <= 0.0) {
      continue;
    }
    Delta_t += dt;

    if (!hasStarted) {
      hasStarted = true;
      const double r = dt / (nexttime - it->timeStamp).toSec();
      omega_S_0 = (r * omega_S_0 + (1.0 - r) * omega_S_1).eval();  //kaishijieduanzuochazhi
      acc_S_0 = (r * acc_S_0 + (1.0 - r) * acc_S_1).eval();
    }

这两段代码把time 和 end与imu的时间戳做插值对齐，因为两个相机帧跟imu帧可能存在时间戳不一致

 

imu运动学模型

 

旋转的传播

首先对相邻的两个角速度取中值并去除偏差

    const Eigen::Vector3d omega_S_true = (0.5*(omega_S_0+omega_S_1) - speedAndBiases.segment<3>(3));

旋转角度等于角速度乘以时间，根据四元数的计算公式，我们取计算旋转角度的一半

const double theta_half = omega_S_true.norm() * 0.5 * dt;

然后按照旋转角和四元数的转换关系计算四元数q

    Eigen::Quaterniond dq;
    const Eigen::Vector3d omega_S_true = (0.5*(omega_S_0+omega_S_1) - speedAndBiases.segment<3>(3));
    const double theta_half = omega_S_true.norm() * 0.5 * dt;
    const double sinc_theta_half = ode::sinc(theta_half);
    const double cos_theta_half = cos(theta_half);
    dq.vec() = sinc_theta_half * omega_S_true * 0.5 * dt;
    dq.w() = cos_theta_half;
    Eigen::Quaterniond Delta_q_1 = Delta_q * dq;

对旋转矩阵和加速度进行积分

    const Eigen::Matrix3d C = Delta_q.toRotationMatrix();
    const Eigen::Matrix3d C_1 = Delta_q_1.toRotationMatrix();
    const Eigen::Vector3d acc_S_true = (0.5*(acc_S_0+acc_S_1) - speedAndBiases.segment<3>(6));
    const Eigen::Matrix3d C_integral_1 = C_integral + 0.5*(C + C_1)*dt;
    const Eigen::Vector3d acc_integral_1 = acc_integral + 0.5*(C + C_1)*acc_S_true*dt;

对旋转矩阵和加速度进行二重积分

 

    C_doubleintegral += C_integral*dt + 0.25*(C + C_1)*dt*dt;
    acc_doubleintegral += acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt;

计算雅可比矩阵

协方差传播

 

2. Frame消费者线程

// Loop to process frames from camera with index cameraIndex
void ThreadedKFVio::frameConsumerLoop(size_t cameraIndex) {

2.1 判断该帧是否关键帧（第一帧是关键帧）

2.2 利用IMU预测pose，为特征点匹配提供方向参考

在frameConsumerLoop()中Image和IMU的同步策略是这样的：

若没有IMU数据，则不处理；IMU第一帧数据之前的那一帧Image也抛弃，下一帧Image（第一帧Frame）才进行特征检测处理。同时第一帧之前的IMU数据会用来计算pose（该函数返回值永远是true，因此initPose是否准确完全依赖IMU给出的读数）：

 

bool success = okvis::Estimator::initPoseFromImu(imuData, T_WS);

第一帧之后的IMU数据进行propagation（注意multiframe在单目情形下就是frame），注意到这里propagation的covariance和jacobian均为0，仅仅用于预测，对特征点检测提供先验的T_WC：

okvis::ceres::ImuError::propagation(imuData, parameters_.imu, T_WS, speedAndBiases, lastTimestamp, multiFrame->timestamp());

2.3 Harris角点检测+BRISK描述子计算

接下来对frame特征检测（Harris）和描述子（BRISK）计算（这里的T_WC由前一步的propagation提供，主要为了获取重力方向，提高描述子匹配鲁棒性）：