OKVIS中imu代码应用在ORB

Tracking.cpp中调用Eigen::Matrix4d Tcp= propagate(timestamp); // 返回的是pred_Tr_delta：imu预测的当前帧相对上一帧的变换关系T 这个函数又调用了

void Tracking::predictStates( const Eigen::Matrix4d &T_sk_to_w, const Eigen::Matrix<double, 9,1>& speed_bias_k, const double * time_pair, std::vector<Eigen::Matrix<double, 7,1> >& measurements, const Eigen::Matrix<double, 6,1> & gwomegaw, const Eigen::Matrix<double, 12, 1>& q_n_aw_babw, Eigen::Matrix4d * pred_T_skp1_to_w, Eigen::Matrix<double, 3,1>* pred_speed_kp1, Eigen::Matrix<double, 15,15> *covariance, Eigen::Matrix<double, 15,15> *jacobian)

其中， T_sk_to_w代入的第一个值是T_s1_to_w，是标定得到的imu到相机坐标的变换关系。KITTI00-02.yaml里面的Rs2c，tsinc计算得来 speed_bias_k是速度+bg+ba共9个数 time_pair是时间间距对 gwomegaw是gravity in local world frame in m/s^2，KITTI00-02.yaml里的gw值 q_n_aw_babw是四种噪声，由KITTI00-02.yaml里na，nw，acc_bias_var，gyro_bias_var四个组成 pred_T_skp1_to_w是输出值，predicted IMU pose at t(k+1) of image indexed k+1 pred_speed_kp1输出值，速度偏差 剩下两个没用到

程序开始： 1，确定初始值，初始位姿的translation是r0=t{TWS} ，初始位姿的旋转部分是CWS0=C{TWS} ，再把位姿转换成四元qWS0=q{TWS} ：

Eigen::Matrix<double, 3,1> r_0(T_sk_to_w.topRightCorner<3, 1>()); Eigen::Matrix<double,3,3> C_WS_0(T_sk_to_w.topLeftCorner<3, 3>()); Eigen::Quaternion<double> q_WS_0(C_WS_0);

2，定义积分初值，四元数积分：Δq=(1,0,0,0) ，旋转矩阵积分：∫C=0(3,3) ，旋转矩阵双重积分：∬C=0(3,3) ，加速度积分：∫a=(0,0,0) ，加速度双重积分：∬a=(0,0,0)：

Eigen::Quaterniond Delta_q(1,0,0,0); Eigen::Matrix3d C_integral = Eigen::Matrix3d::Zero(); Eigen::Matrix3d C_doubleintegral = Eigen::Matrix3d::Zero(); Eigen::Vector3d acc_integral = Eigen::Vector3d::Zero(); Eigen::Vector3d acc_doubleintegral = Eigen::Vector3d::Zero();

3，子雅各比矩阵初始化 角速度对角速度偏置偏导：dα/dbg=0(3,3) 速度对角速度偏置偏导：dv/dbg=0(3,3) 位移对角速度偏置偏导：dp/dbg=0(3,3)

// sub-Jacobians Eigen::Matrix3d dalpha_db_g = Eigen::Matrix3d::Zero(); Eigen::Matrix3d dv_db_g = Eigen::Matrix3d::Zero(); Eigen::Matrix3d dp_db_g = Eigen::Matrix3d::Zero();

4，increament 变量 δx 偏导矩阵初始化： Pδ=I(15,15)

// the Jacobian of the increment (w/o biases) Eigen::Matrix<double,15,15> P_delta = Eigen::Matrix<double,15,15>::Zero();

从最开始到当前次积分的时间间隔：Δt=0

double Delta_t = 0;

首次执行标志：

bool hasStarted = false;

for (int it=0;it

Eigen::Vector3d omega_S_0 =measurements[it].block<3,1>(4,0);//角速度,Block of size (3,1), starting at (4,0) Eigen::Vector3d acc_S_0 = measurements[it].block<3,1>(1,0);//线加速度 Eigen::Vector3d omega_S_1 = measurements[it+1].block<3,1>(4,0); Eigen::Vector3d acc_S_1 = measurements[it+1].block<3,1>(1,0);

求平均角速度：

ave_omega_S=ave_omega_S+omega_S_0; ave_omega_S=ave_omega_S/(it+1);

求出距离下一个测量值的时间间隔，当 end 小于 nexttime 时，end 处 IMU 的测量值通过插值得到，同样对于输入初始时刻 IMU 的测量值通过插值得到

double nexttime; if ((it + 1) == (measurements.size()-1)) { nexttime = end; } else nexttime =measurements [it + 1][0]; double dt = (nexttime - time); if ( end < nexttime) {// double interval = (nexttime - measurements[it][0]); nexttime = end; dt = (nexttime - time); const double r = dt / interval; omega_S_1 = ((1.0 - r) * omega_S_0 + r * omega_S_1).eval(); 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 -measurements[it][0]); omega_S_0 = (r * omega_S_0 + (1.0 - r) * omega_S_1).eval();//求开始是加权的角速度和线加速度 acc_S_0 = (r * acc_S_0 + (1.0 - r) * acc_S_1).eval(); } // ensure integrity double sigma_g_c = q_n_aw_babw(3);//Gyroscope noise density. double sigma_a_c = q_n_aw_babw(1);//Acc noise density

6，由角速度测量值和时间间隔积分得到四元数，四元数积分：dq， 角速度设为时间 t0 和 t1 平均值： Sω=0.5∗(Sω0+Sω1)−bg dqv=sin(||12Sω dt||)∗(12Sω dt) dqw=cos(||12Sω dt||) dq=(dqv,dqw) 当前次四元数积分：Δq1=Δq∗dq

// actual propagation // orientation: Eigen::Quaterniond dq; const Eigen::Vector3d omega_S_true = (0.5*(omega_S_0+omega_S_1) - speed_bias_k.segment<3>(3));//w const double theta_half = omega_S_true.norm() * 0.5 * dt; const double sinc_theta_half = ode(theta_half);//sin(x)/x 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;

7，四元数转化成旋转矩阵：C=M{Δq} ，C1=M{Δq1} ，加速度设为时间 t0 和 t1 平均值：Sa=0.5∗(Sa0+Sa1)−ba

// rotation matrix integral: 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) - speed_bias_k.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; // rotation matrix double integral: 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;

8，

// Jacobian parts dalpha_db_g += dt*C_1; const Eigen::Matrix3d cross_1 = dq.inverse().toRotationMatrix()*cross +okvis::kinematics::rightJacobian(omega_S_true*dt)*dt; const Eigen::Matrix3d acc_S_x = crossMx(acc_S_true); Eigen::Matrix3d dv_db_g_1 = dv_db_g + 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1); dp_db_g += dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);

9，变量前推更新

// memory shift Delta_q = Delta_q_1; C_integral = C_integral_1; acc_integral = acc_integral_1; cross = cross_1; dv_db_g = dv_db_g_1; time = nexttime; interval_time=Delta_t; last_dt=dt; ++i;

for循环结束

10，输出系统状态量更新

// actual propagation output: const Eigen::Vector3d g_W = gwomegaw.head<3>(); const Eigen::Vector3d t=r_0+speed_bias_k.head<3>()*Delta_t+ C_WS_0*(acc_doubleintegral)+0.5*g_W*Delta_t*Delta_t; const Eigen::Quaterniond q=q_WS_0*Delta_q; (*pred_T_skp1_to_w)=rt_to_T(t,q.toRotationMatrix());