1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
/* ----------------------------------------------------------------------------
* 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);
}
/* ************************************************************************* */
|
