From bb662f0cb439205b6b3fb05ee87c21b274a7b0b2 Mon Sep 17 00:00:00 2001
From: Russell Buchanan <russell@robots.ox.ac.uk>
Date: Tue, 19 Jan 2021 10:49:42 +0000
Subject: [PATCH] clean up

---
 tests/testImuPreintegration.cpp | 31 +++----------------------------
 1 file changed, 3 insertions(+), 28 deletions(-)

diff --git a/tests/testImuPreintegration.cpp b/tests/testImuPreintegration.cpp
index 43b3461ee..7975ff794 100644
--- a/tests/testImuPreintegration.cpp
+++ b/tests/testImuPreintegration.cpp
@@ -49,8 +49,6 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
   double rate = 400.0;  // Hz
 
   string inFileString = findExampleDataFile("quadraped_imu_data.csv");
-  ofstream outputFile;
-  outputFile.open("imu_preint_output.csv", ios::out);
   ifstream inputFile(inFileString);
   string line;
   while (getline(inputFile, line)) {
@@ -95,27 +93,13 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
 
   NavState initialNavState(priorPose, priorVelocity);
 
-  // Bias estimated by my Algorithm
-  priorImuBias = imuBias::ConstantBias(
-      Eigen::Vector3d(-0.0314648, 0.0219921, 6.95945e-05),
-      Eigen::Vector3d(4.88581e-08, -1.04971e-09, -0.000122868));
-
-  // zero bias
-  // priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
-  //                                             Eigen::Vector3d(0,0,0));
+  // Assume zero bias for simulated data
+  priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
+                                              Eigen::Vector3d(0,0,0));
 
   imuPreintegrated = PreintegratedCombinedMeasurements(
       imuPreintegratedParams, priorImuBias);
 
-  // Put header row in output csv
-  outputFile << "X Position,"
-             << "Y Position,"
-             << "Z Position,"
-             << "X Velocity,"
-             << "Y Velocity,"
-             << "Z Velocity,"
-             << "\n";
-
   // start at 1 to skip header
   for (size_t n = 1; n < imuMeasurements.size(); n++) {
     // integrate
@@ -127,17 +111,8 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
     velocity = propState.velocity();
     // cout << "IMU Position " << position.transpose() << endl;
     // cout << "IMU Velocity " << velocity.transpose() << endl;
-
-    // Write to csv
-    outputFile << to_string(position.x()) << "," << to_string(position.y())
-               << "," << to_string(position.z()) << ","
-               << to_string(velocity.x()) << "," << to_string(velocity.y())
-               << "," << to_string(velocity.z()) << ","
-               << "\n";
   }
 
-  outputFile.close();
-
   Vector3 rotation = propState.pose().rotation().rpy();
 
   // Dont have ground truth for x and y position yet