12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'regression_test' into feature/cleanup_ImuFactor 

Conflicts:
	gtsam/navigation/tests/testImuFactor.cpp
release/4.3a0
dellaert 2015-07-27 16:17:05 +02:00
parent 1ac790da1c 11d0ad0d78
commit b1ec4d5876
1 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -830,6 +830,19 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Matrix expected(9,9);
expected <<
0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,//
0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,//
0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,//
0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,//
0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,//
0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,//
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0290687976, 0.0, 0.0,//
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0290687976, 0.0,//
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03;
EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7));
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis);
@ -935,6 +948,19 @@ TEST(ImuFactor, PredictArbitrary) {
for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Matrix expected(9,9);
expected << //
0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, -0.0144839494, -0.0454268484, 0.00489577218,//
0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, 0.044978128, -0.0149428917, 0.00839301168, //
-0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, 0.0100471195, 0.00609093775, 0.000448720395, //
0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, -0.0409843415, -0.13554868, 0.00879031682, //
0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, 0.134423822, -0.0471183681, 0.0162199769, //
-0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764, 0.0383280513, 0.0247643646, 0.00112485862,//
-0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, 0.0299999995, 0.0, 0.0, //
-0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, 0.0, 0.0299999995, 0.0, //
0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, 0.0, 0.0, 0.0299999995;
EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7));
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kZeroOmegaCoriolis);