26 using namespace gtsam;
29 static const double kDt = 1
e-2;
40 auto p = PreintegrationParams::MakeSharedU(10);
43 p->integrationCovariance = 0.0000001 * I_3x3;
47 #define EXPECT_NEAR(a, b, c) EXPECT(assert_equal(Vector(a), Vector(b), c));
59 const double T = 2 *
kDt;
68 Z_3x3,
p->gyroscopeCovariance /
kDt;
94 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
100 gttic(ForwardWithBias);
103 const double T = 0.1;
118 const double T = 0.1;
124 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
137 const double T = 0.1;
143 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
157 const double a = 0.2;
174 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
180 gttic(AcceleratingWithBias);
191 gttic(AcceleratingAndRotating);
193 const double a = 0.2;
204 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
219 const double a = 0.2;
228 gttic(Accelerating2);
236 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
242 gttic(AcceleratingWithBias2);
253 gttic(AcceleratingAndRotating2);
255 const double a = 0.2;
266 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
282 const double a = 0.2;
291 gttic(Accelerating3);
299 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
305 gttic(AcceleratingWithBias3);
316 gttic(AcceleratingAndRotating3);
318 const double a = 0.2;
329 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
346 const double a = 0.2;
355 gttic(Accelerating4);
363 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
369 gttic(AcceleratingWithBias4);
380 gttic(AcceleratingAndRotating4);
382 const double a = 0.2;
393 EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);