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

Cleaned up a test, added loop case

release/4.3a0
Frank Dellaert 2016-01-31 16:19:16 -08:00
parent fc500eea33
commit 7dce902f2f
1 changed files with 30 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -88,55 +88,51 @@ TEST(ImuFactor, PreintegratedMeasurements) {
double deltaT = 0.5; double deltaT = 0.5;
// Expected pre-integrated values // Expected pre-integrated values
Vector3 expectedDeltaP1; Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0);
expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0);
Vector3 expectedDeltaV1(0.05, 0.0, 0.0); Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT1(0.5);
// Actual pre-integrated values // Actual pre-integrated values
PreintegratedImuMeasurements actual1(testing::Params()); PreintegratedImuMeasurements actual(testing::Params());
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Z_3x1, actual.theta()));
EXPECT(assert_equal(Z_3x1, actual.deltaPij()));
EXPECT(assert_equal(Z_3x1, actual.deltaVij()));
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()))); actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()))); EXPECT(assert_equal(expectedDeltaR1, actual.theta()));
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()))); EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij()));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);
// Check derivatives of computeError // Check derivatives of computeError
Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
NavState x1, x2 = actual1.predict(x1, bias); NavState x1, x2 = actual.predict(x1, bias);
{ {
Matrix9 aH1, aH2; Matrix9 aH1, aH2;
Matrix96 aH3; Matrix96 aH3;
actual1.computeError(x1, x2, bias, aH1, aH2, aH3); actual.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&, boost::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
const Bias&)> f = boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3,
boost::bind(&PreintegrationBase::computeError, actual1, _1, _2, _3,
boost::none, boost::none, boost::none); boost::none, boost::none, boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
} }
// Integrate again // Integrate again
Vector3 expectedDeltaP2; Vector3 expectedDeltaR2(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; Vector3 expectedDeltaP2(0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0);
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) +
+ expectedDeltaR1.matrix() * measuredAcc * 0.5; Rot3::Expmap(expectedDeltaR1) * measuredAcc * 0.5;
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT2(1);
// Actual pre-integrated values // Actual pre-integrated values
PreintegratedImuMeasurements actual2 = actual1; actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR2, actual.theta()));
EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij()));
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()))); EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()))); DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij())));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Common linearization point and measurements for tests // Common linearization point and measurements for tests
@ -802,7 +798,7 @@ struct ImuFactorMergeTest {
PreintegratedImuMeasurements pim12(p_, bias12); PreintegratedImuMeasurements pim12(p_, bias12);
PreintegratedImuMeasurements pim02_expected(p_, bias01); PreintegratedImuMeasurements pim02_expected(p_, bias01);
double deltaT = 0.05; double deltaT = 0.01;
ScenarioRunner runner(&scenario, p_, deltaT); ScenarioRunner runner(&scenario, p_, deltaT);
// TODO(frank) can this loop just go into runner ? // TODO(frank) can this loop just go into runner ?
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
@ -837,8 +833,8 @@ struct ImuFactorMergeTest {
void TestScenarios(TestResult& result_, const std::string& name_, void TestScenarios(TestResult& result_, const std::string& name_,
const imuBias::ConstantBias& bias01, const imuBias::ConstantBias& bias01,
const imuBias::ConstantBias& bias12, double tol) { const imuBias::ConstantBias& bias12, double tol) {
for (auto scenario : {forward_}) for (auto scenario : {forward_, loop_})
EXPECT_LONGS_EQUAL(0,TestScenario(result_, name_, scenario, bias01, bias12, tol)); TestScenario(result_, name_, scenario, bias01, bias12, tol);
} }
}; };
@ -847,7 +843,8 @@ struct ImuFactorMergeTest {
// an exact answer. // an exact answer.
TEST(ImuFactor, MergeZeroBias) { TEST(ImuFactor, MergeZeroBias) {
ImuFactorMergeTest mergeTest; ImuFactorMergeTest mergeTest;
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-5); // TODO(frank): not too happy with large tolerance (needed for loop case)
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-3);
} }
//// Test case with different biases where we expect there to be some variation. //// Test case with different biases where we expect there to be some variation.