gtsam关于imu预积分实现探究

资料

gtsam官网关于IMU预积分的资料介绍了gtsam预积分基于以下论文实现

Todd Lupton and Salah Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions”, TRO, 28(1):61-76, 2012.

同时基于流形空间进行了优化，基于以下两篇论文

Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, “Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors”, Int. Conf. on Robotics and Automation (ICRA), 2014.

Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, “IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation”, Robotics: Science and Systems (RSS), 2015.

代码分析

从main文件着手

我们从gtsam给的例程代码入手，ImuFactorsExample.cpp文件描述了IMU预积分GPS进行导航的代码。

打开输入输出文件以后，首先是基础操作初始化一个非线性因子图

// Add all prior factors (pose, velocity, bias) to the graph.
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));

例程提供了两种预积分方式PreintegratedCombinedMeasurements和PreintegratedImuMeasurements,我们按PreintegratedImuMeasurements进行分析。在初始化一个IMU预积分量之前首先初始化了预积分的基础参数，各种协方差

boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
// PreintegrationBase params:
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
// should be using 2nd order integration
// PreintegratedRotation params:
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;

初始化一个imu预积分对象

1	imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);

读取数据，如果是imu数据，就进行预积分

if (type == 0) { // IMU measurement
  Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
  for (int i=0; i<5; ++i) {
    getlie(file, value, ',');
    imu(i) = atof(value.c_str());
  }
  getline(file, value, '\n');
  imu(5) = atof(value.c_str());

  // Adding the IMU preintegration.
  imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
}

追踪imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt),可以发现函数定义为

void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
    const Vector3& measuredOmega, double dt) {
  // NOTE(frank): integrateMeasurement always needs to compute the derivatives,
  // even when not of interest to the caller. Provide scratch space here.
  Matrix9 A;
  Matrix93 B, C;
  update(measuredAcc, measuredOmega, dt, &A, &B, &C);
}

核心为update(measuredAcc, measuredOmega, dt, &A, &B, &C);，关于这个updata函数gtsam有两种实现，TangentPreintegration::update和ManifoldPreintegration::update。即基于切空间和基于流形空间的实现。选用哪种实现取决于gtsam编译安装时GTSAM_TANGENT_PREINTEGRATION.为ON还是OFF。默认为切空间；流形空间的实现基于RSS2015的文献实现，切空间的实现在gtsam自带的文档有介绍。这里我们以默认的切空间分析。

切空间的更新函数实现

void TangentPreintegration::update(const Vector3& measuredAcc,
    const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
    Matrix93* C) {
  // Correct for bias in the sensor frame
  Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
  Vector3 omega = biasHat_.correctGyroscope(measuredOmega);

  // Possibly correct for sensor pose
  Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
  if (p().body_P_sensor)
    boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
        D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);

  // Do update
  deltaTij_ += dt;
  preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);

  if (p().body_P_sensor) {
    // More complicated derivatives in case of non-trivial sensor pose
    *C *= D_correctedOmega_omega;
    if (!p().body_P_sensor->translation().isZero())
      *C += *B * D_correctedAcc_omega;
    *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
  }

  // new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc
  // where acc_H_biasAcc = -I_3x3, hence
  // new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc
  preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);

  // new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega
  // where omega_H_biasOmega = -I_3x3, hence
  // new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega
  preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}

在这个函数中，首先对加速度和角速度进行修正，减去当前其偏置值(bias),得到修正后的加速度角速度：

1 2	Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); Vector3 omega = biasHat_.correctGyroscope(measuredOmega);

body坐标系和imu坐标系之间的转换

默认情况下imu坐标系和body坐标系是一个坐标系，也可以设置不是一个坐标系。通过在初始化时设置p->body_P_sensor的值确定imu坐标系到body坐标系的转换矩阵。如果设置了两个坐标系之间的转换关系，那么需要将imu测量的imu坐标系的加速度和角速度转换为body坐标系的加速度角速度。在updata()函数的中的这几行代码就是执行这一操作：

1
2
3

if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
    D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);

代码

追踪correctMeasurementsBySensorPose()函数我们可以看到这个函数的实现：

pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
    const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
    OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc,
    OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega,
    OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega) const {
  assert(p().body_P_sensor);

  // Compensate for sensor-body displacement if needed: we express the quantities
  // (originally in the IMU frame) into the body frame
  // Equations below assume the "body" frame is the CG

  // Get sensor to body rotation matrix
  const Matrix3 bRs = p().body_P_sensor->rotation().matrix();

  // Convert angular velocity and acceleration from sensor to body frame
  Vector3 correctedAcc = bRs * unbiasedAcc;
  const Vector3 correctedOmega = bRs * unbiasedOmega;

  // Jacobians
  if (correctedAcc_H_unbiasedAcc) *correctedAcc_H_unbiasedAcc = bRs;
  if (correctedAcc_H_unbiasedOmega) *correctedAcc_H_unbiasedOmega = Z_3x3;
  if (correctedOmega_H_unbiasedOmega) *correctedOmega_H_unbiasedOmega = bRs;

  // Centrifugal acceleration
  const Vector3 b_arm = p().body_P_sensor->translation();
  if (!b_arm.isZero()) {
    // Subtract out the the centripetal acceleration from the unbiased one
    // to get linear acceleration vector in the body frame:
    const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
    const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
    correctedAcc -= body_Omega_body * b_velocity_bs;

    // Update derivative: centrifugal causes the correlation between acc and omega!!!
    if (correctedAcc_H_unbiasedOmega) {
      double wdp = correctedOmega.dot(b_arm);
      const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal();
      *correctedAcc_H_unbiasedOmega = -( diag_wdp
          + correctedOmega * b_arm.transpose()) * bRs.matrix()
          + 2 * b_arm * unbiasedOmega.transpose();
    }
  }
  return make_pair(correctedAcc, correctedOmega);
}

公式化表达

速度加速度从sensor(IMU)坐标系转换到body坐标系 \[ \omega^\prime=R_s^b \omega \] \[ a^\prime=R_s^b a \] 离心加速度 \[ v_{bs}^b=\omega^\prime\times p \] \[ a_c=\omega^\prime\times v_{bs}^b \] 将离心加速度从加速中去掉 \[ a\prime=a\prime-a_c \]

更新预积分量

下面是重头戏，更新预积分量了。

1	preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);

从函数调用可以看到，函数接收7个值，刚才修正过后的加速度，角速度，时间间隔，上一次运算结束保存的预积分量，A,B,C。追踪这个函数，我们先看一下整体实现。

代码

Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
    const Vector3& w_body, double dt, const Vector9& preintegrated,
    OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
    OptionalJacobian<9, 3> C) {
  const auto theta = preintegrated.segment<3>(0);
  const auto position = preintegrated.segment<3>(3);
  const auto velocity = preintegrated.segment<3>(6);

  // This functor allows for saving computation when exponential map and its
  // derivatives are needed at the same location in so<3>
  so3::DexpFunctor local(theta);

  // Calculate exact mean propagation
  Matrix3 w_tangent_H_theta, invH;
  const Vector3 w_tangent = // angular velocity mapped back to tangent space
      local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
  const SO3 R = local.expmap();
  const Vector3 a_nav = R * a_body;
  const double dt22 = 0.5 * dt * dt;

  Vector9 preintegratedPlus;
  preintegratedPlus << // new preintegrated vector:
      theta + w_tangent * dt, // theta
  position + velocity * dt + a_nav * dt22, // position
  velocity + a_nav * dt; // velocity

  if (A) {
    // Exact derivative of R*a with respect to theta:
    const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();

    A->setIdentity();
    A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
    A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
    A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
    A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
  }
  if (B) {
    B->block<3, 3>(0, 0) = Z_3x3;
    B->block<3, 3>(3, 0) = R * dt22;
    B->block<3, 3>(6, 0) = R * dt;
  }
  if (C) {
    C->block<3, 3>(0, 0) = invH * dt;
    C->block<3, 3>(3, 0) = Z_3x3;
    C->block<3, 3>(6, 0) = Z_3x3;
  }

  return preintegratedPlus;
}

分析

首先可以看到，预积分量的代码表达为一个九维向量：\(preintegrated=[\theta,x,v]\)。

接着，这里实例化了一套局部坐标系，关于这个local，gtsam的文档有详细介绍。参见：a new imu factor

然后是一个重要操作，借助局部坐标把角速度的切向量求取回来。

1 2	const Vector3 w_tangent = // angular velocity mapped back to tangent space local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);

切向量求取

让我们跳转进去，看看这个函数的实现

Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                  OptionalJacobian<3, 3> H2) const {
  const Matrix3 invDexp = dexp_.inverse();
  const Vector3 c = invDexp * v;
  if (H1) {
    Matrix3 D_dexpv_omega;
    applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
    *H1 = -invDexp* D_dexpv_omega;
  }
  if (H2) *H2 = invDexp;
  return c;
}

代码里的变量对应论文的公式表达

dexp_: \(H(\theta )\)，这个量是在初始化局部坐标系的时候就求好了的
invDexp：\(H(\theta )^{-1}\), 这个量就是借助dexp_这个矩阵用Eigen直接矩阵求逆得到的
v: \(\omega_k^b\)
D_dexpv_omega：用来更新噪声传播
总的来看这个函数返回值赋值给切向量可以表达为 \(\text{w\_tangent}=-H(\theta )^{-1}\omega_k^b\)

这里特别感兴趣 \(H(\theta )\)这个量的实现，找到local坐标系里面对其的求取:

DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
    : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
  if (nearZero)
    dexp_ = I_3x3 - 0.5 * W;
  else {
    a = one_minus_cos / theta;
    b = 1.0 - sin_theta / theta;
    dexp_ = I_3x3 - a * K + b * KK;
  }
}

W: \([\omega]_{\times}\)
theta: \(||\omega||\)
one_minus_cos: \(1-cos||\omega||\)
K: \(\frac{[w]_\times}{||\omega||}\)
KK: \({\frac{[w]_\times^2}{\||\omega||^2}}\)

分析可以看到其计算方式为 \[ H(\omega)= \begin{cases} I-\frac{1}{2}[\omega]_\times& ||\omega|| \approx 0\\ I-\frac{\left(1-\cos ||\omega||)\right[\omega]_\times}{||\omega||^2}+\left(1-\frac{\sin ||\omega||}{||\omega||}\right)\left(\frac{[\omega]_\times}{||\omega||}\right)^2& \text{others} \end{cases} \]

本质就是以右扰动定义的雅克比，参见下文献中的公式10.86的\(J_r(x)\)，此外注意这里的\(\omega\)是初始化local的时候传入的\(\theta\)，不是测量的角速度\(\omega\) >Gregory S. Chirikjian. “Stochastic Models, Information Theory, and Lie Groups, Volume 2” (2012).

更新预积分的测量值

1	preintegratedPlus << theta + w_tangent * dt, position + velocity * dt + a_nav * dt22, velocity + a_nav * dt;

\[ \begin{aligned} \theta_{k+1} &=\theta_{k}+H\left(\theta_{k}\right)^{-1} \omega_{k}^{b} \Delta_{t} \\ p_{k+1} &=p_{k}+v_{k} \Delta_{t}+R_{k} a_{k}^{b} \frac{\Delta_{t}^{2}}{2} \\ v_{k+1} &=v_{k}+R_{k} a_{k}^{b} \Delta_{t} \end{aligned} \]

最后，针对误差传递 \(\Sigma_{k+1}=A_{k} \Sigma_{k} A_{k}^{T}+B_{k}\Sigma_{\eta}^{a d} B_{k}+C_{k} \Sigma_{\eta}^{g d} C_{k}\)，更新A，B，C

if (A) {
  // Exact derivative of R*a with respect to theta:
  const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();

  A->setIdentity();
  A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
  A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
  A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
  A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
  B->block<3, 3>(0, 0) = Z_3x3;
  B->block<3, 3>(3, 0) = R * dt22;
  B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
  C->block<3, 3>(0, 0) = invH * dt;
  C->block<3, 3>(3, 0) = Z_3x3;
  C->block<3, 3>(6, 0) = Z_3x3;

对应公式 \[ A_{k} \approx\left[\begin{array}{ccc} I_{3 \times 3}-\frac{\Delta_{t}}{2}\left[\omega_{k}^{b}\right]_{\times} &0 & 0\\ R_{k}\left[-a_{k}^{b}\right]_{\times} H\left(\theta_{k}\right) \frac{\Delta_{t}}{2} & I_{3 \times 3} & I_{3 \times 3} \Delta_{t} \\ R_{k}\left[-a_{k}^{b}\right]_{\times} H\left(\theta_{k}\right) \Delta_{t} &0 & I_{3 \times 3} \end{array}\right] \]

\[ B_{k}=\left[\begin{array}{c} 0_{3 \times 3} \\ R_{k} \frac{\Delta_{t}}{2} \\ R_{k} \Delta_{t} \end{array}\right], \quad C_{k}=\left[\begin{array}{c} H\left(\theta_{k}\right)^{-1} \Delta_{t} \\ 0_{3 \times 3} \\ 0_{3 \times 3} \end{array}\right] \]

回到main函数

当读取到GPS的数据的时候，首先将两帧GPS数据之间的IMU预积分量添加到图优化的框架里面去。

PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
                      X(correction_count  ), V(correction_count  ),
                      B(correction_count-1),
                      *preint_imu);
graph->add(imu_factor);

此外，针对两帧GPS数据之间的IMU bias的变化量添加一个约束。

imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
                                                B(correction_count  ),
                                                zero_bias, bias_noise_model));

添加gps测量约束，然后对图进行优化。