009 gtsam/examples/ImuFactorsExample.cpp

说明：

/**
 * @file ImuFactorsExample
 * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
 * navigation code.
 * @author Garrett (ghemann@gmail.com), Luca Carlone
 */

/**
 * Example of use of the imuFactors (imuFactor and combinedImuFactor) in
 * conjunction with GPS
 *  - imuFactor is used by default. You can test combinedImuFactor by
 *  appending a `-c` flag at the end (see below for example command).
 *  - we read IMU and GPS data from a CSV file, with the following format:
 *  A row starting with "i" is the first initial position formatted with
 *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
 *  A row starting with "0" is an imu measurement
 *  (body frame - Forward, Right, Down)
 *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
 *  A row starting with "1" is a gps correction formatted with
 *  N, E, D, qX, qY, qZ, qW
 * Note that for GPS correction, we're only using the position not the
 * rotation. The rotation is provided in the file for ground truth comparison.
 *
 *  See usage: ./ImuFactorsExample --help
 */
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

一、预先定义

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

  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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

在此程序中使用的是PreintegratedCombinedMeasurements
见002 gtsam/examples CombinedImuFactorsExample.cpp
factor使用的是ImuFactor

二、main函数

2.1 读数据和初始设置

  string data_filename, output_filename;

  bool use_isam = false;

  po::variables_map var_map = parseOptions(argc, argv);

  data_filename = findExampleDataFile(var_map["data_csv_path"].as<string>());
  output_filename = var_map["output_filename"].as<string>();
  use_isam = var_map["use_isam"].as<bool>();

  ISAM2* isam2 = 0;
  if (use_isam) {
    printf("Using ISAM2\n");
    ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.01;
    parameters.relinearizeSkip = 1;
    isam2 = new ISAM2(parameters);

  } else {
    printf("Using Levenberg Marquardt Optimizer\n");
  }

  // Set up output file for plotting errors
  FILE* fp_out = fopen(output_filename.c_str(), "w+");
  fprintf(fp_out,
          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
          "gt_qy,gt_qz,gt_qw\n");

  // Begin parsing the CSV file.  Input the first line for initialization.
  // From there, we'll iterate through the file and we'll preintegrate the IMU
  // or add in the GPS given the input.
  ifstream file(data_filename.c_str());
  string value;

  // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
  Vector10 initial_state;
  getline(file, value, ',');  // i
  for (int i = 0; i < 9; i++) {
    getline(file, value, ',');
    initial_state(i) = stof(value.c_str());
  }
  getline(file, value, '\n');
  initial_state(9) = stof(value.c_str());
  cout << "initial state:\n" << initial_state.transpose() << "\n\n";
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

2.2 进行先验的设置

  // Assemble initial quaternion through GTSAM constructor
  // ::quaternion(w,x,y,z);
  // step 1.1 进行初始的位姿、速度、bias设置
  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
  // step 1.2 将初始值插入到Values中
  Values initial_values;
  int correction_count = 0;
  initial_values.insert(X(correction_count), prior_pose);
  initial_values.insert(V(correction_count), prior_velocity);
  initial_values.insert(B(correction_count), prior_imu_bias);

  // Assemble prior noise model and add it the graph.`
  // step 1-3 设置先验的噪声
  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);

  // Add all prior factors (pose, velocity, bias) to the graph.
  // step 1-4 将所有的先验因子加入到图中
  NonlinearFactorGraph* graph = new NonlinearFactorGraph();
  graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
  graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
  graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);

  auto p = imuParams();
  // step 1-5 设置积分类型
  std::shared_ptr<PreintegrationType> preintegrated =
      std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);

  assert(preintegrated);

  // Store previous state for imu integration and latest predicted outcome.
  // step 1-6 存储imu预积分的先前状态和传播状态
  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.
  // step 1-7 错误跟踪设置
  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.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

2.3 根据数据进行迭代

2.3.1 for loop

while (file.good()) 
{
	/*......*/
}
1
2
3
4

2.3.2 get value

    // Parse out first value
    getline(file, value, ',');
    int type = stoi(value.c_str());

    if (type == 0) {  // IMU measurement
      Vector6 imu;
      for (int i = 0; i < 5; ++i) {
        getline(file, value, ',');
        imu(i) = stof(value.c_str());
      }
      getline(file, value, '\n');
      imu(5) = stof(value.c_str());

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

    } else if (type == 1) {  // GPS measurement
      Vector7 gps;
      for (int i = 0; i < 6; ++i) {
        getline(file, value, ',');
        gps(i) = stof(value.c_str());
      }
      getline(file, value, '\n');
      gps(6) = stof(value.c_str());

      correction_count++;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

      // Adding IMU factor and GPS factor and optimizing.
      auto preint_imu =
          dynamic_cast<const PreintegratedImuMeasurements&>(*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);
      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));

      auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
      GPSFactor gps_factor(X(correction_count),
                           Point3(gps(0),   // N,
                                  gps(1),   // E,
                                  gps(2)),  // D,
                           correction_noise);
      graph->add(gps_factor);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

2.3.3 执行优化

imu传播插入初值

      // Now optimize and compare results.
      prop_state = preintegrated->predict(prev_state, prev_bias);
      initial_values.insert(X(correction_count), prop_state.pose());
      initial_values.insert(V(correction_count), prop_state.v());
      initial_values.insert(B(correction_count), prev_bias);
1
2
3
4
5

根据不同的优化方法执行优化

      if (use_isam) {
        isam2->update(*graph, initial_values);
        isam2->update();
        result = isam2->calculateEstimate();

        // reset the graph
        graph->resize(0);
        initial_values.clear();

      } else {
        LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
        result = optimizer.optimize();
      }
1
2
3
4
5
6
7
8
9
10
11
12
13

2.3.4 优化后初值设置

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

      // Reset the preintegration object.
      // step 2 重置预积分和bias
      preintegrated->resetIntegrationAndSetBias(prev_bias);
1
2
3
4
5
6
7
8
9

2.3.4 report error

      // Print out the position and orientation error for comparison.
      Vector3 gtsam_position = prev_state.pose().translation();
      Vector3 position_error = gtsam_position - gps.head<3>();
      current_position_error = position_error.norm();

      Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
      Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
      Quaternion quat_error = gtsam_quat * gps_quat.inverse();
      quat_error.normalize();
      Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
                                quat_error.z() * 2);
      current_orientation_error = euler_angle_error.norm();

      // display statistics
      cout << "Position error:" << current_position_error << "\t "
           << "Angular error:" << current_orientation_error << "\n";

      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
              output_time, gtsam_position(0), gtsam_position(1),
              gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
              gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
              gps_quat.y(), gps_quat.z(), gps_quat.w());

      output_time += 1.0;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24