OKVIS中imu代码应用在ORB_SLAM2中的解读

版权声明:如果读者有疑问请留言或者联系博主邮箱kylefan15@gmail.com https://blog.csdn.net/fk1174/article/details/61617447

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());
展开阅读全文

没有更多推荐了,返回首页