redoPreintegration 和 propagation 定義類似

// Propagates pose, speeds and biases with given IMU measurements.
int ImuError::redoPreintegration(const okvis::kinematics::Transformation& /*T_WS*/,
                                 const okvis::SpeedAndBias & speedAndBiases) const

int ImuError::propagation(const okvis::ImuMeasurementDeque & imuMeasurements,
                          const okvis::ImuParameters & imuParams,
                          okvis::kinematics::Transformation& T_WS,
                          okvis::SpeedAndBias & speedAndBiases,
                          const okvis::Time & t_start,
                          const okvis::Time & t_end, covariance_t* covariance,
                          jacobian_t* jacobian){
  // now the propagation
  okvis::Time time = t_start;
  okvis::Time end = t_end;

  // sanity check:
  assert(imuMeasurements.front().timeStamp<=time);
  if (!(imuMeasurements.back().timeStamp >= end))
    return -1;  // nothing to do...

// initial condition
Eigen::Vector3d r_0 = T_WS.r();
Eigen::Quaterniond q_WS_0 = T_WS.q();
Eigen::Matrix3d C_WS_0 = T_WS.C();

propagation 初值賦值：
位姿 translation 部分：r0=t{TWS}
位姿轉換成四元數：qWS0=q{TWS}
位姿旋轉部分：CWS0=C{TWS}

// increments (initialise with identity)
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();

積分初值:
四元數積分：Δq=(1,0,0,0)
旋轉矩陣積分：∫C=0(3,3)
旋轉矩陣雙重積分：∬C=0(3,3)
加速度積分：∫a=(0,0,0)
加速度雙重積分：∬a=(0,0,0)

// cross matrix accumulatrion
Eigen::Matrix3d cross = Eigen::Matrix3d::Zero();

Mcross=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();

子雅各比矩陣初始化
角速度對角速度偏置偏導：dαdbg=0(3,3)
速度對角速度偏置偏導：dvdbg=0(3,3)
位移對角速度偏置偏導：dpdbg=0(3,3)

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

increament 變量 δx 偏導矩陣初始化： Pδ=I(15,15)

double Delta_t = 0;

從最開始到當前次積分的時間間隔：Δt=0

for

// hasStarted 第一次執行標識
bool hasStarted = false;
int i = 0;
for (okvis::ImuMeasurementDeque::const_iterator it = imuMeasurements.begin();
    it != imuMeasurements.end(); ++it) {

    Eigen::Vector3d omega_S_0 = it->measurement.gyroscopes;
    Eigen::Vector3d acc_S_0 = it->measurement.accelerometers;
    Eigen::Vector3d omega_S_1 = (it + 1)->measurement.gyroscopes;
    Eigen::Vector3d acc_S_1 = (it + 1)->measurement.accelerometers;

it 個角速度測量：Sω0
it 個加速度測量：Sa0
it+1 個IMU 測量：Sω1
it+1 個加速度測量：Sa1

    // time delta
    okvis::Time nexttime;
    if ((it + 1) == imuMeasurements.end()) {
      nexttime = t_end;
    } else
      nexttime = (it + 1)->timeStamp;
    double dt = (nexttime - time).toSec();

    // 當 end 小於 nexttime 時，end 處 IMU 的測量值通過插值得到
    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();
      acc_S_1 = ((1.0 - r) * acc_S_0 + r * acc_S_1).eval();
    }

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

    // 同樣對於輸入初始時刻 IMU 的測量值通過插值得到
    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();
      acc_S_0 = (r * acc_S_0 + (1.0 - r) * acc_S_1).eval();
    }

    // ensure integrity
    double sigma_g_c = imuParams.sigma_g_c;
    double sigma_a_c = imuParams.sigma_a_c;

t0 到 t1 時間間隔：dt
Δt=Δt+dt
從配置文件中讀取的 gyro noise density [rad/s/sqrt(Hz)]：σgc
從配置文件中讀取的 accelerometer noise density [m/s^2/sqrt(Hz)]：σac

    // 讀入的數據超過設定的最大值，不確定度乘 100
    if (fabs(omega_S_0[0]) > imuParams.g_max
        || fabs(omega_S_0[1]) > imuParams.g_max
        || fabs(omega_S_0[2]) > imuParams.g_max
        || fabs(omega_S_1[0]) > imuParams.g_max
        || fabs(omega_S_1[1]) > imuParams.g_max
        || fabs(omega_S_1[2]) > imuParams.g_max) {
      sigma_g_c *= 100;
      LOG(WARNING) << "gyr saturation";
    }

    if (fabs(acc_S_0[0]) > imuParams.a_max || fabs(acc_S_0[1]) > imuParams.a_max
        || fabs(acc_S_0[2]) > imuParams.a_max
        || fabs(acc_S_1[0]) > imuParams.a_max
        || fabs(acc_S_1[1]) > imuParams.a_max
        || fabs(acc_S_1[2]) > imuParams.a_max) {
      sigma_a_c *= 100;
      LOG(WARNING) << "acc saturation";
    }

    //由角速度測量值和時間間隔積分得到四元數
    // actual propagation
    // orientation:
    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;

四元數積分：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

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

四元數轉化成旋轉矩陣：C=M{Δq}
四元數轉化成旋轉矩陣：C1=M{Δq1}
加速度設爲時間 t0 和 t1 平均值：Sa=0.5∗(Sa0+Sa1)−ba
∫′C=∫C+0.5∗(C+C1)∗dt
∫′a=∫a+0.5∗(C+C1)∗Sa∗dt
∬C=∬C+∫C∗dt+0.25∗(C+C1)∗dt∗dt
∬a=∬a+∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt

    // 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 = okvis::kinematics::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);

dαdbg=dαdbg+C1∗dt
Mcross1=C{dq−1}∗Mcross+Jr{Sω∗dt}∗dt
dvdbg′=dvdbg+0.5∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1)
dpdbg=dpdbg+dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1)

covariance propagation

    // covariance propagation
    if (covariance) {
      Eigen::Matrix<double,15,15> F_delta = Eigen::Matrix<double,15,15>::Identity();
      // transform
      F_delta.block<3,3>(0,3) = -okvis::kinematics::crossMx(acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt);
      F_delta.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*dt;
      F_delta.block<3,3>(0,9) = dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);
      F_delta.block<3,3>(0,12) = -C_integral*dt + 0.25*(C + C_1)*dt*dt;
      F_delta.block<3,3>(3,9) = -dt*C_1;
      F_delta.block<3,3>(6,3) = -okvis::kinematics::crossMx(0.5*(C + C_1)*acc_S_true*dt);
      F_delta.block<3,3>(6,9) = 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);
      F_delta.block<3,3>(6,12) = -0.5*(C + C_1)*dt;
      P_delta = F_delta*P_delta*F_delta.transpose();

Fδ=I(15,15)
Fδ(0:2,3:5)=−[∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt]×
Fδ(0:2,6:8)=I(3,3)∗dt
Fδ(0:2,9:11)=dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1)
Fδ(3:5,9:11)=−dt∗C1
Fδ(6:8,3:5)=−[0.5∗(C+C1)∗Sa∗dt]×
Fδ(6:8,9:11)=0.5∗dt∗(C∗[aS]×∗Mcross+C1∗[aS]×∗Mcross1)
Fδ(6:8,12:15)=−0.5∗(C+C1)∗dt

F δ = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ 00000 - [\int a * d t + 0.25 * (C + C 1) * S a * d t * d t] \times 0 - [0.5 * (C + C 1) * S a * d t] \times 00 I (3, 3) * d t 0000 d t * d v d b g + 0.25 * d t * d t * (C * [S a] \times * M c r o s s + C 1 * [S a] \times * M c r o s s 1) - d t * C 1 0.5 * d t * (C * [a S] \times * M c r o s s + C 1 * [a S] \times * M c r o s s 1) 00 00 - 0.5 * (C + C 1) * d t 00 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

Pδ=Fδ∗Pδ∗FTδ

      // add noise. Note that transformations with rotation matrices can be ignored, since the noise is isotropic.
      //F_tot = F_delta*F_tot;
      const double sigma2_dalpha = dt * sigma_g_c * sigma_g_c;
      P_delta(3,3) += sigma2_dalpha;
      P_delta(4,4) += sigma2_dalpha;
      P_delta(5,5) += sigma2_dalpha;
      const double sigma2_v = dt * sigma_a_c * imuParams.sigma_a_c;
      P_delta(6,6) += sigma2_v;
      P_delta(7,7) += sigma2_v;
      P_delta(8,8) += sigma2_v;
      const double sigma2_p = 0.5*dt*dt*sigma2_v;
      P_delta(0,0) += sigma2_p;
      P_delta(1,1) += sigma2_p;
      P_delta(2,2) += sigma2_p;
      const double sigma2_b_g = dt * imuParams.sigma_gw_c * imuParams.sigma_gw_c;
      P_delta(9,9)   += sigma2_b_g;
      P_delta(10,10) += sigma2_b_g;
      P_delta(11,11) += sigma2_b_g;
      const double sigma2_b_a = dt * imuParams.sigma_aw_c * imuParams.sigma_aw_c;
      P_delta(12,12) += sigma2_b_a;
      P_delta(13,13) += sigma2_b_a;
      P_delta(14,14) += sigma2_b_a;
    }

gyro noise density：σgc
accelerometer noise density：σac
gyro drift noise density：σgwc
accelerometer drift noise density：σawc
σ2dα=dt∗σgc∗σgc
σ2v=dt∗σac∗σac
σ2p=0.5∗dt∗dt∗σ2v
σ2bg=dt∗σgwc∗σgwc
σ2ba=dt∗σawc∗σawc

P δ = P δ + ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ σ 2 p * I (3, 3) 0000 0 σ 2 d α * I (3, 3) 000 00 σ 2 v * I (3, 3) 00 000 σ 2 b g * I (3, 3) 0 0000 σ 2 b a * I (3, 3) ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

end covariance propagation

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

    ++i;

    if (nexttime == t_end)
      break;

  }

Δq=Δq1
∫C=∫′C
∫a=∫′a
Mcross=Mcorss1
dvdbg=dvdbg′

end for

  // actual propagation output:
  const Eigen::Vector3d g_W = imuParams.g * Eigen::Vector3d(0, 0, 6371009).normalized();
  T_WS.set(r_0+speedAndBiases.head<3>()*Delta_t
             + C_WS_0*(acc_doubleintegral/*-C_doubleintegral*speedAndBiases.segment<3>(6)*/)
             - 0.5*g_W*Delta_t*Delta_t,
             q_WS_0*Delta_q);
  speedAndBiases.head<3>() += C_WS_0*(acc_integral/*-C_integral*speedAndBiases.segment<3>(6)*/)-g_W*Delta_t;

輸出系統狀態量更新

輸入重力加速度參數：g
gW=g∗(0,0,1)T
t{TWS}=r0+v∗Δt+CWS0∗∬a−0.5∗gW∗Δt∗Δt
q{TWS}=qWS0∗Δq
v=v+CWS0∗∫a−gW∗Δt

  // assign Jacobian, if requested
  if (jacobian) {
    Eigen::Matrix<double,15,15> & F = *jacobian;
    F.setIdentity(); // holds for all states, including d/dalpha, d/db_g, d/db_a
    F.block<3,3>(0,3) = -okvis::kinematics::crossMx(C_WS_0*acc_doubleintegral);
    F.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*Delta_t;
    F.block<3,3>(0,9) = C_WS_0*dp_db_g;
    F.block<3,3>(0,12) = -C_WS_0*C_doubleintegral;
    F.block<3,3>(3,9) = -C_WS_0*dalpha_db_g;
    F.block<3,3>(6,3) = -okvis::kinematics::crossMx(C_WS_0*acc_integral);
    F.block<3,3>(6,9) = C_WS_0*dv_db_g;
    F.block<3,3>(6,12) = -C_WS_0*C_integral;
  }

輸出雅各比矩陣

J=I(15,15)
J(0:2,3:5)=−[CWS0∗∬a]×
J(0:2,6:8)=I(3,3)∗Δt
J(0:2,9:11)=CWS0∗dpdbg
J(0:2,12:14)=−CWS0∗∬C
J(3:5,9:11)=−CWS0∗dαdbg
J(6:8,3:5)=−[CWS0∗∫a]×
J(6:8,9:11)=CWS0∗dvdbg
J(6:8,12:14)=−CWS0∗∫C

J = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ 00000 - [C W S 0 * \iint a] \times 0 - [C W S 0 * \int a] \times 00 I (3, 3) * Δ t 0000 C W S 0 * d p d b g - C W S 0 * d α d b g C W S 0 * d v d b g 00 - C W S 0 * \iint C 0 - C W S 0 * \int C 00 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

  // overall covariance, if requested
  if (covariance) {
    Eigen::Matrix<double,15,15> & P = *covariance;
    // transform from local increments to actual states
    Eigen::Matrix<double,15,15> T = Eigen::Matrix<double,15,15>::Identity();
    T.topLeftCorner<3,3>() = C_WS_0;
    T.block<3,3>(3,3) = C_WS_0;
    T.block<3,3>(6,6) = C_WS_0;
    P = T * P_delta * T.transpose();
  }
  return i;
}

輸出方差矩陣

T=I(15,15)

T = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ C W S 0 0000 0 C W S 0 000 00 C W S 0 00 0000000000 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

P=T∗Pδ∗TT

OKVIS 中的 propagation 代碼公式版

for

covariance propagation

end covariance propagation

end for

輸出系統狀態量更新

輸出雅各比矩陣

輸出方差矩陣

ElasticFusion 解析

ElasticFusion 中的 Randomized Ferns 重定位/迴環檢測論文和代碼解析

KinectFusion 中的 ICP 算法 GPU 代碼解讀

DBoW2 迴環檢測/重定位算法解析

PCL KinectFusion 編譯安裝教程

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結