128 lines
4.2 KiB
C++
128 lines
4.2 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 <CppUnitLite/TestHarness.h>
|
|
#include <gtsam/base/Testable.h>
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
#include <gtsam/navigation/CombinedImuFactor.h>
|
|
#include <gtsam/slam/dataset.h>
|
|
#include <tests/ImuMeasurement.h>
|
|
|
|
#include <fstream>
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
/**
|
|
* \brief Uses the GTSAM library to perform IMU preintegration on an
|
|
* acceleration input.
|
|
*/
|
|
TEST(TestImuPreintegration, LoadedSimulationData) {
|
|
Vector3 finalPos(0, 0, 0);
|
|
|
|
vector<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
|
|
|
|
string inFileString = findExampleDataFile("quadraped_imu_data.csv");
|
|
ifstream inputFile(inFileString);
|
|
string line;
|
|
while (getline(inputFile, line)) {
|
|
stringstream ss(line);
|
|
string str;
|
|
vector<double> results;
|
|
while (getline(ss, str, ',')) {
|
|
results.push_back(atof(str.c_str()));
|
|
}
|
|
ImuMeasurement measurement;
|
|
measurement.dt = static_cast<size_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);
|
|
}
|
|
|
|
// Assume a Z-up navigation (assuming we are performing optimization in the
|
|
// IMU frame).
|
|
auto 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);
|
|
imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance;
|
|
imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt;
|
|
|
|
// Initial state
|
|
Pose3 priorPose;
|
|
Vector3 priorVelocity(0, 0, 0);
|
|
imuBias::ConstantBias priorImuBias;
|
|
PreintegratedCombinedMeasurements imuPreintegrated;
|
|
Vector3 position(0, 0, 0);
|
|
Vector3 velocity(0, 0, 0);
|
|
NavState propState;
|
|
|
|
NavState initialNavState(priorPose, priorVelocity);
|
|
|
|
// Assume zero bias for simulated data
|
|
priorImuBias =
|
|
imuBias::ConstantBias(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
|
|
|
|
imuPreintegrated =
|
|
PreintegratedCombinedMeasurements(imuPreintegratedParams, priorImuBias);
|
|
|
|
// 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);
|
|
// predict
|
|
propState = imuPreintegrated.predict(initialNavState, priorImuBias);
|
|
position = propState.pose().translation();
|
|
velocity = propState.velocity();
|
|
}
|
|
|
|
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);
|
|
}
|
|
/* ************************************************************************* */
|