1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file testImuPreintegration.cpp
14 * @brief Unit tests for IMU Preintegration
15 * @author Russell Buchanan
16 **/
17
18#include <CppUnitLite/TestHarness.h>
19#include <gtsam/base/Testable.h>
20#include <gtsam/base/numericalDerivative.h>
21#include <gtsam/navigation/CombinedImuFactor.h>
22#include <gtsam/slam/dataset.h>
23#include <tests/ImuMeasurement.h>
24
25#include <fstream>
26#include <iostream>
27
28using namespace std;
29using namespace gtsam;
30
31/* ************************************************************************* */
32/**
33 * \brief Uses the GTSAM library to perform IMU preintegration on an
34 * acceleration input.
35 */
36TEST(TestImuPreintegration, LoadedSimulationData) {
37 Vector3 finalPos(0, 0, 0);
38
39 vector<ImuMeasurement> imuMeasurements;
40
41 double accNoiseSigma = 0.001249;
42 double accBiasRwSigma = 0.000106;
43 double gyrNoiseSigma = 0.000208;
44 double gyrBiasRwSigma = 0.000004;
45 double integrationCovariance = 1e-8;
46
47 double gravity = 9.81;
48 double rate = 400.0; // Hz
49
50 string inFileString = findExampleDataFile(name: "quadraped_imu_data.csv");
51 ifstream inputFile(inFileString);
52 string line;
53 while (getline(is&: inputFile, str&: line)) {
54 stringstream ss(line);
55 string str;
56 vector<double> results;
57 while (getline(in&: ss, str&: str, delim: ',')) {
58 results.push_back(x: atof(nptr: str.c_str()));
59 }
60 ImuMeasurement measurement;
61 measurement.dt = static_cast<size_t>(1e9 * (1 / rate));
62 measurement.time = results[2];
63 measurement.I_a_WI = {results[29], results[30], results[31]};
64 measurement.I_w_WI = {results[17], results[18], results[19]};
65 imuMeasurements.push_back(x: measurement);
66 }
67
68 // Assume a Z-up navigation (assuming we are performing optimization in the
69 // IMU frame).
70 auto imuPreintegratedParams =
71 PreintegratedCombinedMeasurements::Params::MakeSharedU(g: gravity);
72 imuPreintegratedParams->accelerometerCovariance =
73 I_3x3 * pow(x: accNoiseSigma, y: 2);
74 imuPreintegratedParams->biasAccCovariance = I_3x3 * pow(x: accBiasRwSigma, y: 2);
75 imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(x: gyrNoiseSigma, y: 2);
76 imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(x: gyrBiasRwSigma, y: 2);
77 imuPreintegratedParams->integrationCovariance = I_3x3 * integrationCovariance;
78
79 // Initial state
80 Pose3 priorPose;
81 Vector3 priorVelocity(0, 0, 0);
82 imuBias::ConstantBias priorImuBias;
83 PreintegratedCombinedMeasurements imuPreintegrated;
84 Vector3 position(0, 0, 0);
85 Vector3 velocity(0, 0, 0);
86 NavState propState;
87
88 NavState initialNavState(priorPose, priorVelocity);
89
90 // Assume zero bias for simulated data
91 priorImuBias =
92 imuBias::ConstantBias(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
93
94 imuPreintegrated =
95 PreintegratedCombinedMeasurements(imuPreintegratedParams, priorImuBias);
96
97 // start at 1 to skip header
98 for (size_t n = 1; n < imuMeasurements.size(); n++) {
99 // integrate
100 imuPreintegrated.integrateMeasurement(measuredAcc: imuMeasurements[n].I_a_WI,
101 measuredOmega: imuMeasurements[n].I_w_WI, dt: 1 / rate);
102 // predict
103 propState = imuPreintegrated.predict(state_i: initialNavState, bias_i: priorImuBias);
104 position = propState.pose().translation();
105 velocity = propState.velocity();
106 }
107
108 Vector3 rotation = propState.pose().rotation().rpy();
109
110 // Dont have ground truth for x and y position yet
111 // DOUBLES_EQUAL(0.1, position[0], 1e-2);
112 // DOUBLES_EQUAL(0.1, position[1], 1e-2);
113 DOUBLES_EQUAL(0.0, position[2], 1e-2);
114
115 DOUBLES_EQUAL(0.0, rotation[0], 1e-2);
116 DOUBLES_EQUAL(0.0, rotation[1], 1e-2);
117 DOUBLES_EQUAL(0.0, rotation[2], 1e-2);
118}
119
120/* ************************************************************************* */
121int main() {
122 TestResult tr;
123 return TestRegistry::runAllTests(result&: tr);
124}
125/* ************************************************************************* */
126