Merge pull request borglab#365 from borglab/imu-examples

Reworked IMU examples

examples/CombinedImuFactorsExample.cpp

@@ -0,0 +1,303 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file CombinedImuFactorsExample
+ * @brief Test example for using GTSAM ImuCombinedFactor
+ * navigation code.
+ * @author Varun Agrawal
+ */
+
+/**
+ * Example of use of the CombinedImuFactor in
+ * conjunction with GPS
+ *  - 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
+ *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
+ *  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: ./CombinedImuFactorsExample --help
+ */
+
+#include <boost/program_options.hpp>
+
+// GTSAM related includes.
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/GPSFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/dataset.h>
+
+#include <cstring>
+#include <fstream>
+#include <iostream>
+
+using namespace gtsam;
+using namespace std;
+
+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)
+
+namespace po = boost::program_options;
+
+po::variables_map parseOptions(int argc, char* argv[]) {
+  po::options_description desc;
+  desc.add_options()("help,h", "produce help message")(
+      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
+      "path to the CSV file with the IMU data")(
+      "output_filename",
+      po::value<string>()->default_value("imuFactorExampleResults.csv"),
+      "path to the result file to use")("use_isam", po::bool_switch(),
+                                        "use ISAM as the optimizer");
+
+  po::variables_map vm;
+  po::store(po::parse_command_line(argc, argv, desc), vm);
+
+  if (vm.count("help")) {
+    cout << desc << "\n";
+    exit(1);
+  }
+
+  return vm;
+}
+
+Vector10 readInitialState(ifstream& file) {
+  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());
+
+  return initial_state;
+}
+
+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;
+}
+
+int main(int argc, char* argv[]) {
+  string data_filename, output_filename;
+  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>();
+
+  // 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());
+
+  Vector10 initial_state = readInitialState(file);
+  cout << "initial state:\n" << initial_state.transpose() << "\n\n";
+
+  // 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
+
+  int index = 0;
+
+  Values initial_values;
+
+  // 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);
+
+  // 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);
+
+  // 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);
+
+  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.
+
+  // All priors have been set up, now iterate through the data file.
+  while (file.good()) {
+    // Parse out first value
+    string 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());
+
+      index++;
+
+      // 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);
+
+      // 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();
+
+      // 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);
+
+      // Print out the position and orientation error for comparison.
+      Vector3 result_position = prev_state.pose().translation();
+      Vector3 position_error = result_position - gps.head<3>();
+      current_position_error = position_error.norm();
+
+      Quaternion result_quat = prev_state.pose().rotation().toQuaternion();
+      Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
+      Quaternion quat_error = result_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"
+           << endl;
+
+      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
+              output_time, result_position(0), result_position(1),
+              result_position(2), result_quat.x(), result_quat.y(),
+              result_quat.z(), result_quat.w(), gps(0), gps(1), gps(2),
+              gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
+
+      output_time += 1.0;
+
+    } else {
+      cerr << "ERROR parsing file\n";
+      return 1;
+    }
+  }
+  fclose(fp_out);
+  cout << "Complete, results written to " << output_filename << "\n\n";
+
+  return 0;
+}