开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> C++知识库 -> gtsam关于imu预积分实现探究 -> 正文阅读

[C++知识库]gtsam关于imu预积分实现探究

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预积分对象

  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),得到修正后的加速度角速度：

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

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

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

    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$

更新预积分量

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

  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

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

  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：用来更新噪声传播
总的来看这个函数返回值赋值给切向量可以表达为 $w_tangent = ? H ( θ ) ? 1 ω k b \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).

更新预积分的测量值

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