diff --git a/tests/ImuMeasurement.h b/tests/ImuMeasurement.h new file mode 100644 index 000000000..5ac3d54be --- /dev/null +++ b/tests/ImuMeasurement.h @@ -0,0 +1,23 @@ +#pragma once + +#include + +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 diff --git a/tests/Measurement.h b/tests/Measurement.h new file mode 100644 index 000000000..e26c5d918 --- /dev/null +++ b/tests/Measurement.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +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 diff --git a/tests/testImuPreintegration.cpp b/tests/testImuPreintegration.cpp new file mode 100644 index 000000000..6e3b6396d --- /dev/null +++ b/tests/testImuPreintegration.cpp @@ -0,0 +1,155 @@ +/** + * @file testImuPreintegration.cpp + * @brief Unit tests for IMU Preintegration + * @author Russell Buchanan + **/ + +#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 gtsam; + +/* ************************************************************************* */ +/// \brief Uses the GTSAM library to perform IMU preintegration on an acceleration input. +/// +TEST(GtsamLibraryTests, LoadedSimulationData) { + Eigen::Vector3d finalPos; + + std::vector 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 results; + while (getline(ss, str, ',')) { + results.push_back(std::atof(str.c_str())); + } + drs::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; + } + + // 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); + 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); +} +/* ************************************************************************* */