Adds unit test for imu preintegration of a single step

tests/ImuMeasurement.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <tests/Measurement.h>
+
+namespace drs {
+
+///
+/// \brief Contains data from the IMU mesaurements.
+///
+class ImuMeasurement : public Measurement {
+ public:
+  enum Name { BODY = 0, RF_FOOT = 1, RH_FOOT = 2 };
+
+  Name name;               ///< Unique string identifier
+  Eigen::Vector3d I_a_WI;  ///< Raw acceleration from the IMU (m/s/s)
+  Eigen::Vector3d I_w_WI;  ///< Raw angular velocity from the IMU (rad/s)
+
+  virtual ~ImuMeasurement() override {}
+  ImuMeasurement();
+  friend std::ostream& operator<<(std::ostream& stream, const ImuMeasurement& meas);
+};
+
+}  // namespace drs

tests/Measurement.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <string>
+
+namespace drs {
+
+///
+/// \brief This is the base class for all measurement types.
+///
+class Measurement {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  uint64_t dt;      ///< Time since the last message of this type (nanoseconds).
+  uint64_t time;    ///< ROS time message recieved (nanoseconds).
+  std::string type; ///< The type of message (to enable dynamic/static casting).
+
+  virtual ~Measurement() {}
+  Measurement();
+  Measurement(std::string _type);
+};
+
+} // namespace drs

tests/testImuPreintegration.cpp

@@ -0,0 +1,155 @@
+/**
+ * @file   testImuPreintegration.cpp
+ * @brief  Unit tests for IMU Preintegration
+ * @author Russell Buchanan
+ **/
+
+#include <tests/ImuMeasurement.h>
+
+#include <fstream>
+#include <iostream>
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
+
+namespace drs {
+
+Measurement::Measurement() : dt(0), time(0), type("UNDEFINED") {}
+
+Measurement::Measurement(std::string _type) : dt(0), time(0), type(_type) {}
+
+ImuMeasurement::ImuMeasurement() : I_a_WI{0, 0, 0}, I_w_WI{0, 0, 0} { type = "ImuMeasurement"; }
+
+std::ostream& operator<<(std::ostream& stream, const ImuMeasurement& meas) {
+  stream << "IMU Measurement at time = " << meas.time << " : \n"
+         << "dt    : " << meas.dt << "\n"
+         << "I_a_WI: " << meas.I_a_WI.transpose() << "\n"
+         << "I_w_WI: " << meas.I_w_WI.transpose() << "\n";
+  return stream;
+}
+
+}  // namespace drs
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+/// \brief Uses the GTSAM library to perform IMU preintegration on an acceleration input.
+///
+TEST(GtsamLibraryTests, LoadedSimulationData) {
+  Eigen::Vector3d finalPos;
+
+  std::vector<drs::ImuMeasurement> imuMeasurements;
+
+  double accNoiseSigma = 0.001249;
+  double accBiasRwSigma = 0.000106;
+  double gyrNoiseSigma = 0.000208;
+  double gyrBiasRwSigma = 0.000004;
+  double integrationCovariance = 1e-8;
+  double biasAccOmegaInt = 1e-5;
+
+  double gravity = 9.81;
+  double rate = 400.0;  // Hz
+
+  /// @todo Update directory to correct location
+  std::string inFileString = "/home/russell/imu_data.csv";
+  std::ofstream outputFile;
+  outputFile.open("/home/russell/gtsam_output.csv", std::ofstream::out);
+  std::ifstream inputFile(inFileString);
+  std::string line;
+  while (std::getline(inputFile, line)) {
+    std::stringstream ss(line);
+    std::string str;
+    std::vector<double> results;
+    while (getline(ss, str, ',')) {
+      results.push_back(std::atof(str.c_str()));
+    }
+    drs::ImuMeasurement measurement;
+    measurement.dt = static_cast<uint64_t>(1e9 * (1 / rate));
+    measurement.time = results[2];
+    measurement.I_a_WI = {results[29], results[30], results[31]};
+    measurement.I_w_WI = {results[17], results[18], results[19]};
+    imuMeasurements.push_back(measurement);
+
+    // std::cout << "IMU measurement " << measurement << std::endl;
+  }
+
+  // Assume a Z-up navigation (assuming we are performing optimization in the IMU frame).
+  boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> imuPreintegratedParams =
+      gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedU(gravity);
+  imuPreintegratedParams->accelerometerCovariance = I_3x3 * pow(accNoiseSigma, 2);
+  imuPreintegratedParams->biasAccCovariance = I_3x3 * pow(accBiasRwSigma, 2);
+  imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2);
+  imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2);
+  imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance;
+  imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt;
+
+  // Initial state
+  gtsam::Pose3 priorPose;
+  gtsam::Vector3 priorVelocity;
+  gtsam::imuBias::ConstantBias priorImuBias;
+  gtsam::PreintegratedCombinedMeasurements imuPreintegrated;
+  Eigen::Vector3d position;
+  Eigen::Vector3d velocity;
+  gtsam::NavState propState;
+
+  gtsam::NavState initialNavState(priorPose, priorVelocity);
+
+  // Bias estimated by my Algorithm
+  priorImuBias =
+      gtsam::imuBias::ConstantBias(Eigen::Vector3d(-0.0314648, 0.0219921, 6.95945e-05),
+                                   Eigen::Vector3d(4.88581e-08, -1.04971e-09, -0.000122868));
+
+  // zero bias
+  // priorImuBias = gtsam::imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
+  //                                             Eigen::Vector3d(0,0,0));
+
+  imuPreintegrated = gtsam::PreintegratedCombinedMeasurements(imuPreintegratedParams, priorImuBias);
+
+  // Put header row in output csv
+  outputFile << "X Position,"
+             << "Y Position,"
+             << "Z Position,"
+             << "X Velocity,"
+             << "Y Velocity,"
+             << "Z Velocity,"
+             << "\n";
+
+  for (int n = 1; n < imuMeasurements.size(); n++) { //start at 1 to skip header
+    // integrate
+    imuPreintegrated.integrateMeasurement(imuMeasurements[n].I_a_WI, imuMeasurements[n].I_w_WI,
+                                          1 / rate);
+    // predict
+    propState = imuPreintegrated.predict(initialNavState, priorImuBias);
+    position = propState.pose().translation();
+    velocity = propState.velocity();
+    // std::cout << "IMU Position " << position.transpose() << std::endl;
+    // std::cout << "IMU Velocity " << velocity.transpose() << std::endl;
+
+    // Write to csv
+    outputFile << std::to_string(position.x()) << "," << std::to_string(position.y()) << ","
+               << std::to_string(position.z()) << "," << std::to_string(velocity.x()) << ","
+               << std::to_string(velocity.y()) << "," << std::to_string(velocity.z()) << ","
+               << "\n";
+  }
+
+  outputFile.close();
+
+  gtsam::Vector3 rotation = propState.pose().rotation().rpy();
+
+  // Dont have ground truth for x and y position yet
+  // DOUBLES_EQUAL(0.1, position[0], 1e-2);
+  // DOUBLES_EQUAL(0.1, position[1], 1e-2);
+  DOUBLES_EQUAL(0.0, position[2], 1e-2);
+
+  DOUBLES_EQUAL(0.0, rotation[0], 1e-2);
+  DOUBLES_EQUAL(0.0, rotation[1], 1e-2);
+  DOUBLES_EQUAL(0.0, rotation[2], 1e-2);
+}
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */