002 gtsam/examples CombinedImuFactorsExample.cpp

一、CombinedImuFactor使用例程

1.1 symbols

using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)

二、测量参数的设置

boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
  // 设置噪声-->传感器白噪声和随机游走噪声
  // We use the sensor specs to build the noise model for the IMU factor.
  double accel_noise_sigma = 0.0003924;
  double gyro_noise_sigma = 0.000205689024915;
  double accel_bias_rw_sigma = 0.004905;
  double gyro_bias_rw_sigma = 0.000001454441043;
  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
  //两个预积分的协方差->position from velocities and error in the bias used for preintegration
  Matrix33 integration_error_cov =
      I_3x3 * 1e-8;  // error committed in integrating position from velocities
  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
  Matrix66 bias_acc_omega_int =
      I_6x6 * 1e-5;  // error in the bias used for preintegration
  //重力方向MakeSharedD和MakeSharedU不同
  auto 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;

  return p;
}

三、关联

该factor关联了i和j时刻的 速度、姿态、IMU偏置
假设了在两个时刻内 imu的bias是缓慢变化的
由以下

  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous

两个变量表达

预积分协方差将用于积分的偏置估计中的噪声纳入考虑由PreintegrationCombinedParams::biasAccOmegaInt影响

PreintegratedCombinedMeasurements的协方差矩阵保留了偏置不确定性和预积分测量不确定性之间的相关性

四、main函数

4.1 初始位姿

  // Assemble initial quaternion through GTSAM constructor
  // ::Quaternion(w,x,y,z);
  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
                                         initial_state(4), initial_state(5));
  Point3 prior_point(initial_state.head<3>());
  Pose3 prior_pose(prior_rotation, prior_point);
  Vector3 prior_velocity(initial_state.tail<3>());

  imuBias::ConstantBias prior_imu_bias;  // assume zero initial bias

4.2 变量初始值

  // insert pose at initialization
  initial_values.insert(X(index), prior_pose);
  initial_values.insert(V(index), prior_velocity);
  initial_values.insert(B(index), prior_imu_bias);

4.3 先验噪声模型

  // Assemble prior noise model and add it the graph.`
  auto pose_noise_model = noiseModel::Diagonal::Sigmas(
      (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
          .finished());  // rad,rad,rad,m, m, m
  auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // m/s
  auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);

4.4 prior factors

  // Add all prior factors (pose, velocity, bias) to the graph.
  NonlinearFactorGraph graph;
  graph.addPrior<Pose3>(X(index), prior_pose, pose_noise_model);
  graph.addPrior<Vector3>(V(index), prior_velocity, velocity_noise_model);
  graph.addPrior<imuBias::ConstantBias>(B(index), prior_imu_bias,
                                        bias_noise_model);

4.5 预积分参数及其模型设置

prior_imu_bias是先验的imu偏置，由初始化得到

  auto p = imuParams();

  std::shared_ptr<PreintegrationType> preintegrated =
      std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);

  assert(preintegrated);
  // 导航状态
  // Store previous state for imu integration and latest predicted outcome.
  NavState prev_state(prior_pose, prior_velocity);
  NavState prop_state = prev_state;
  imuBias::ConstantBias prev_bias = prior_imu_bias;

  // Keep track of total error over the entire run as simple performance metric.
  double current_position_error = 0.0, current_orientation_error = 0.0;

  double output_time = 0.0;
  double dt = 0.005;  // The real system has noise, but here, results are nearly
                      // exactly the same, so keeping this for simplicity.

preintegrated：作用

1. 输入高频IMU在body坐标系下的两个测量（加速度和线速度），进行积分，得到与开始状态和结尾状态无关的帧间相对预积分测量
2. 输入起始状态和起始偏移，与帧间相对预积分测量作用，预测结尾状态
3. 输入计算得到的偏移量，利用预积分技术“刷新”预积分测量（姿态、速度的预积分测量）

4.6 向因子图中添加因子

4.6.1 读取数据时计算预积分

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

4.6.2 添加因子

      // Adding IMU factor and GPS factor and optimizing.
      auto preint_imu_combined =
          dynamic_cast<const PreintegratedCombinedMeasurements&>(
              *preintegrated);
      CombinedImuFactor imu_factor(X(index - 1), V(index - 1), X(index),
                                   V(index), B(index - 1), B(index),
                                   preint_imu_combined);
      graph.add(imu_factor);

      auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
      GPSFactor gps_factor(X(index),
                           Point3(gps(0),   // N,
                                  gps(1),   // E,
                                  gps(2)),  // D,
                           correction_noise);
      graph.add(gps_factor);

4.7 进行优化

      // Now optimize and compare results.
      prop_state = preintegrated->predict(prev_state, prev_bias);//预测当前的状态
      initial_values.insert(X(index), prop_state.pose());
      initial_values.insert(V(index), prop_state.v());
      initial_values.insert(B(index), prev_bias);
      
      LevenbergMarquardtParams params;
      params.setVerbosityLM("SUMMARY");
      LevenbergMarquardtOptimizer optimizer(graph, initial_values, params);
      Values result = optimizer.optimize();

prop_state = preintegrated->predict(prev_state, prev_bias);//预测当前的状态

是由前一时刻 t = i的测量得到t = j 时刻的测量

4.8 优化完重置bias和预积分

      // Overwrite the beginning of the preintegration for the next step.
      prev_state =
          NavState(result.at<Pose3>(X(index)), result.at<Vector3>(V(index)));
      prev_bias = result.at<imuBias::ConstantBias>(B(index));

      // Reset the preintegration object.
      preintegrated->resetIntegrationAndSetBias(prev_bias);

preintegrated->resetIntegrationAndSetBias(prev_bias); // 调用接口更新预积分测量，但不需要重新积分