002 gtsam/examples CombinedImuFactorsExample.cpp

最新推荐文章于 2023-10-17 11:16:41 发布

xlsemmmmmm

最新推荐文章于 2023-10-17 11:16:41 发布

阅读量573

点赞数

CC 4.0 BY-SA版权

分类专栏：因子图优化文章标签： c++

本文链接：https://blog.csdn.net/weixin_43848456/article/details/127494546

002 CombinedImuFactorsExample.cpp

一、CombinedImuFactor使用例程
- 1.1 symbols
二、测量参数的设置
三、关联
四、main函数

一、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