Skip to content

Commit 182580b

Browse files
dellaertdellaert
committed
Make tests fail :-)
1 parent 23ed14c commit 182580b

File tree

3 files changed

+55
-43
lines changed

3 files changed

+55
-43
lines changed

‎gtsam/navigation/tests/imuFactorTesting.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,8 @@ namespace {
102102
using CT = PreintegratedCombinedMeasurementsT<T>; \
103103
testGroup##testName##Helper<PM, CM>(result_, this->name_); \
104104
testGroup##testName##Helper<PT, CT>(result_, this->name_); \
105+
using PG = PreintegratedImuMeasurementsG; \
106+
testGroup##testName##Helper<PG, CT>(result_, this->name_); \
105107
} \
106108
template <class PIM, class CombinedPIM> \
107109
void testGroup##testName##Helper(TestResult& result_, \

‎gtsam/navigation/tests/testCombinedImuFactor.cpp‎

Lines changed: 47 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include <gtsam/geometry/Pose3.h>
2626
#include <gtsam/inference/Symbol.h>
2727
#include <gtsam/navigation/CombinedImuFactor.h>
28+
#include <gtsam/navigation/GalileanImuFactor.h>
2829
#include <gtsam/navigation/ImuBias.h>
2930
#include <gtsam/navigation/ImuFactor.h>
3031
#include <gtsam/navigation/ScenarioRunner.h>
@@ -50,9 +51,13 @@ static std::shared_ptr<PreintegratedCombinedMeasurements::Params> Params(
5051
} // namespace combined
5152

5253
/* ************************************************************************* */
53-
TEST_PIM(CombinedImuFactor, PreintegratedMeasurements ) {
54+
TEST_PIM(CombinedImuFactor, PreintegratedMeasurements) {
5455
// Linearization point
55-
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
56+
Bias bias(
57+
Vector3(0, 0, 0),
58+
Vector3(
59+
0, 0,
60+
0)); ///< Current estimate of acceleration and angular rate biases
5661

5762
// Measurements
5863
Vector3 measuredAcc(0.1, 0.0, 0.0);
@@ -76,19 +81,18 @@ TEST_PIM(CombinedImuFactor, PreintegratedMeasurements ) {
7681
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
7782
}
7883

79-
8084
/* ************************************************************************* */
81-
TEST_PIM(CombinedImuFactor, ErrorWithBiases ) {
82-
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
83-
Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
85+
TEST_PIM(CombinedImuFactor, ErrorWithBiases) {
86+
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
87+
Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
8488
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
8589
Vector3 v1(0.5, 0.0, 0.0);
8690
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
87-
Point3(5.5, 1.0, -50.0));
91+
Point3(5.5, 1.0, -50.0));
8892
Vector3 v2(0.5, 0.0, 0.0);
8993

9094
auto p = combined::Params();
91-
p->omegaCoriolis = Vector3(0,0.1,0.1);
95+
p->omegaCoriolis = Vector3(0, 0.1, 0.1);
9296
PIM pim(p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
9397

9498
// Measurements
@@ -101,29 +105,30 @@ TEST_PIM(CombinedImuFactor, ErrorWithBiases ) {
101105

102106
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
103107

104-
CombinedPIM combined_pim(p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
108+
CombinedPIM combined_pim(
109+
p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
105110

106111
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
107112

108113
// Create factor
109114
ImuFactorT<PIM> imuFactor(X(1), V(1), X(2), V(2), B(1), pim);
110115

111-
CombinedImuFactorT<CombinedPIM> combinedFactor(X(1), V(1), X(2), V(2), B(1), B(2),
112-
combined_pim);
116+
CombinedImuFactorT<CombinedPIM> combinedFactor(X(1), V(1), X(2), V(2), B(1),
117+
B(2), combined_pim);
113118

114119
Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
115-
Vector errorActual = combinedFactor.evaluateError(x1, v1, x2, v2, bias,
116-
bias2);
120+
Vector errorActual =
121+
combinedFactor.evaluateError(x1, v1, x2, v2, bias, bias2);
117122
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
118123

119124
// Expected Jacobians
120125
Matrix H1e, H2e, H3e, H4e, H5e;
121-
(void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
126+
(void)imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
122127

123128
// Actual Jacobians
124129
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
125-
(void) combinedFactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
126-
H3a, H4a, H5a, H6a);
130+
(void)combinedFactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a,
131+
H4a, H5a, H6a);
127132

128133
EXPECT(assert_equal(H1e, H1a.topRows(9)));
129134
EXPECT(assert_equal(H2e, H2a.topRows(9)));
@@ -148,9 +153,11 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
148153
PreintegratedCombinedMeasurementsT<TangentPreintegration> pim(p);
149154
testing::integrateMeasurements(measurements, &pim);
150155

151-
EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
156+
EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(
157+
preintegrated, Z_3x1, Z_3x1),
152158
pim.preintegrated_H_biasAcc()));
153-
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
159+
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(
160+
preintegrated, Z_3x1, Z_3x1),
154161
pim.preintegrated_H_biasOmega(), 1e-3));
155162
}
156163

@@ -173,7 +180,8 @@ TEST_PIM(CombinedImuFactor, PredictPositionAndVelocity) {
173180
// Create factor
174181
const noiseModel::Gaussian::shared_ptr combinedmodel =
175182
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
176-
const CombinedImuFactorT<CombinedPIM> Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
183+
const CombinedImuFactorT<CombinedPIM> Combinedfactor(X(1), V(1), X(2), V(2),
184+
B(1), B(2), pim);
177185

178186
// Predict
179187
const NavState actual = pim.predict(NavState(), bias);
@@ -185,16 +193,17 @@ TEST_PIM(CombinedImuFactor, PredictPositionAndVelocity) {
185193

186194
/* ************************************************************************* */
187195
TEST_PIM(CombinedImuFactor, PredictRotation) {
188-
const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
196+
const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
189197
auto p = combined::Params();
190198
CombinedPIM pim(p, bias);
191-
const Vector3 measuredAcc = - kGravityAlongNavZDown;
199+
const Vector3 measuredAcc = -kGravityAlongNavZDown;
192200
const Vector3 measuredOmega(0, 0, M_PI / 10.0);
193201
const double deltaT = 0.01;
194202
const double tol = 1e-4;
195203
for (int i = 0; i < 100; ++i)
196204
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
197-
const CombinedImuFactorT<CombinedPIM> Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
205+
const CombinedImuFactorT<CombinedPIM> Combinedfactor(X(1), V(1), X(2), V(2),
206+
B(1), B(2), pim);
198207

199208
// Predict
200209
const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
@@ -226,20 +235,20 @@ TEST_PIM(CombinedImuFactor, CheckCovariance) {
226235
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
227236

228237
Eigen::Matrix<double, 15, 15> expected;
229-
expected << 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
230-
0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
231-
0, 0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
232-
0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, 0, //
233-
0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, //
234-
0, 0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, //
235-
0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, 0, //
236-
0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, //
237-
0, 0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, //
238-
0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, 0, //
239-
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, //
240-
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, //
241-
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, //
242-
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, //
238+
expected << 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
239+
0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
240+
0, 0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
241+
0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, 0, //
242+
0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, //
243+
0, 0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, //
244+
0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, 0, //
245+
0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, //
246+
0, 0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, //
247+
0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, 0, //
248+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, //
249+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, //
250+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, //
251+
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, //
243252
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5.;
244253

245254
// regression
@@ -282,8 +291,8 @@ TEST_PIM(CombinedImuFactor, SameCovariance) {
282291
cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
283292

284293
// Assert if the noise covariance
285-
EXPECT(assert_equal(pim.preintMeasCov(),
286-
cpim.preintMeasCov().block(0, 0, 9, 9)));
294+
EXPECT(assert_equal<Matrix9>(
295+
pim.preintMeasCov(), Matrix9(cpim.preintMeasCov().block(0, 0, 9, 9))));
287296
}
288297

289298
/* ************************************************************************* */

‎gtsam/navigation/tests/testImuFactor.cpp‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -20,16 +20,17 @@
2020

2121
// #define ENABLE_TIMING // uncomment for timing results
2222

23+
#include <CppUnitLite/TestHarness.h>
24+
#include <gtsam/base/TestableAssertions.h>
25+
#include <gtsam/base/numericalDerivative.h>
26+
#include <gtsam/geometry/Pose3.h>
27+
#include <gtsam/linear/Sampler.h>
28+
#include <gtsam/navigation/GalileanImuFactor.h>
2329
#include <gtsam/navigation/ImuFactor.h>
2430
#include <gtsam/navigation/ScenarioRunner.h>
25-
#include <gtsam/geometry/Pose3.h>
2631
#include <gtsam/nonlinear/Values.h>
2732
#include <gtsam/nonlinear/factorTesting.h>
28-
#include <gtsam/linear/Sampler.h>
29-
#include <gtsam/base/TestableAssertions.h>
30-
#include <gtsam/base/numericalDerivative.h>
3133

32-
#include <CppUnitLite/TestHarness.h>
3334
#include <list>
3435

3536
#include "imuFactorTesting.h"

0 commit comments

Comments
 (0)