diff --git a/tests/testImuPreintegration.cpp b/tests/testImuPreintegration.cpp index 6e3b6396d..43b3461ee 100644 --- a/tests/testImuPreintegration.cpp +++ b/tests/testImuPreintegration.cpp @@ -1,46 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * 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 testImuPreintegration.cpp * @brief Unit tests for IMU Preintegration * @author Russell Buchanan **/ +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include - -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 std; using namespace gtsam; /* ************************************************************************* */ -/// \brief Uses the GTSAM library to perform IMU preintegration on an acceleration input. -/// -TEST(GtsamLibraryTests, LoadedSimulationData) { +/** + * \brief Uses the GTSAM library to perform IMU preintegration on an + * acceleration input. + */ +TEST(TestImuPreintegration, LoadedSimulationData) { Eigen::Vector3d finalPos; - std::vector imuMeasurements; + vector imuMeasurements; double accNoiseSigma = 0.001249; double accBiasRwSigma = 0.000106; @@ -52,33 +48,36 @@ TEST(GtsamLibraryTests, LoadedSimulationData) { 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 results; + 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)) { + stringstream ss(line); + string str; + vector results; while (getline(ss, str, ',')) { - results.push_back(std::atof(str.c_str())); + results.push_back(atof(str.c_str())); } - drs::ImuMeasurement measurement; - measurement.dt = static_cast(1e9 * (1 / rate)); + ImuMeasurement measurement; + measurement.dt = static_cast(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; + // cout << "IMU measurement " << measurement << endl; } - // Assume a Z-up navigation (assuming we are performing optimization in the IMU frame). - boost::shared_ptr imuPreintegratedParams = - gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedU(gravity); - imuPreintegratedParams->accelerometerCovariance = I_3x3 * pow(accNoiseSigma, 2); + // Assume a Z-up navigation (assuming we are performing optimization in the + // IMU frame). + boost::shared_ptr + imuPreintegratedParams = + 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); @@ -86,26 +85,27 @@ TEST(GtsamLibraryTests, LoadedSimulationData) { imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt; // Initial state - gtsam::Pose3 priorPose; - gtsam::Vector3 priorVelocity; - gtsam::imuBias::ConstantBias priorImuBias; - gtsam::PreintegratedCombinedMeasurements imuPreintegrated; + Pose3 priorPose; + Vector3 priorVelocity; + imuBias::ConstantBias priorImuBias; + PreintegratedCombinedMeasurements imuPreintegrated; Eigen::Vector3d position; Eigen::Vector3d velocity; - gtsam::NavState propState; + NavState propState; - gtsam::NavState initialNavState(priorPose, priorVelocity); + 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)); + 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 = gtsam::imuBias::ConstantBias(Eigen::Vector3d(0,0,0), + // priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0), // Eigen::Vector3d(0,0,0)); - imuPreintegrated = gtsam::PreintegratedCombinedMeasurements(imuPreintegratedParams, priorImuBias); + imuPreintegrated = PreintegratedCombinedMeasurements( + imuPreintegratedParams, priorImuBias); // Put header row in output csv outputFile << "X Position," @@ -116,27 +116,29 @@ TEST(GtsamLibraryTests, LoadedSimulationData) { << "Z Velocity," << "\n"; - for (int n = 1; n < imuMeasurements.size(); n++) { //start at 1 to skip header + // start at 1 to skip header + for (size_t n = 1; n < imuMeasurements.size(); n++) { // integrate - imuPreintegrated.integrateMeasurement(imuMeasurements[n].I_a_WI, imuMeasurements[n].I_w_WI, - 1 / rate); + 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; + // cout << "IMU Position " << position.transpose() << endl; + // cout << "IMU Velocity " << velocity.transpose() << 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()) << "," + 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(); - gtsam::Vector3 rotation = propState.pose().rotation().rpy(); + Vector3 rotation = propState.pose().rotation().rpy(); // Dont have ground truth for x and y position yet // DOUBLES_EQUAL(0.1, position[0], 1e-2); @@ -147,6 +149,7 @@ TEST(GtsamLibraryTests, LoadedSimulationData) { DOUBLES_EQUAL(0.0, rotation[1], 1e-2); DOUBLES_EQUAL(0.0, rotation[2], 1e-2); } + /* ************************************************************************* */ int main() { TestResult tr;