From 5d95d66077f7d459d41897cd94377af7a084b3a3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jan 2016 11:00:25 -0800 Subject: [PATCH] Simplifying bias tests --- gtsam/navigation/tests/imuFactorTesting.h | 67 +++++++++++++ .../tests/testCombinedImuFactor.cpp | 93 +++++-------------- gtsam/navigation/tests/testImuFactor.cpp | 86 ++++------------- 3 files changed, 110 insertions(+), 136 deletions(-) create mode 100644 gtsam/navigation/tests/imuFactorTesting.h diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h new file mode 100644 index 000000000..fae2400b5 --- /dev/null +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 imuFactorTesting.h + * @brief Common testing infrastructure + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +using namespace std; +using namespace gtsam; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + +typedef imuBias::ConstantBias Bias; +static const Vector3 Z_3x1 = Vector3::Zero(); +static const Bias kZeroBiasHat, kZeroBias; + +static const Vector3 kZeroOmegaCoriolis(0, 0, 0); +static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); + +namespace testing { + +struct ImuMeasurement { + ImuMeasurement(const Vector3& acc, const Vector3& gyro, double dt) + : acc(acc), gyro(gyro), dt(dt) {} + const Vector3 acc, gyro; + const double dt; +}; + +template +void integrateMeasurements(const vector& measurements, + PIM* pim) { + for (const auto& m : measurements) + pim->integrateMeasurement(m.acc, m.gyro, m.dt); +} + +struct SomeMeasurements : vector { + SomeMeasurements() { + reserve(102); + const double dt = 0.01, pi100 = M_PI / 100; + emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt); + emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt); + for (int i = 1; i < 100; i++) { + emplace_back(Vector3(0.05, 0.09, 0.01), + Vector3(pi100, pi100 * 3, 2 * pi100), dt); + } + } +}; + +} // namespace testing diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c25086eea..8ed0f751f 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -32,17 +32,7 @@ #include #include -using namespace std; -using namespace gtsam; - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::V; -using symbol_shorthand::B; - -typedef imuBias::ConstantBias Bias; -static const Vector3 Z_3x1 = Vector3::Zero(); -static const Bias kZeroBiasHat, kZeroBias; +#include "imuFactorTesting.h" namespace { @@ -50,34 +40,22 @@ namespace { // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { + const imuBias::ConstantBias& bias) { auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); - PreintegratedCombinedMeasurements result(p, bias); - - list::const_iterator itAcc = measuredAccs.begin(); - list::const_iterator itOmega = measuredOmegas.begin(); - list::const_iterator itDeltaT = deltaTs.begin(); - for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { - result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); - } - return result; + PreintegratedCombinedMeasurements pim(p, bias); + integrateMeasurements(testing::SomeMeasurements(), &pim); + return pim; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaPij(); + const imuBias::ConstantBias& bias) { + return evaluatePreintegratedMeasurements(bias).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaVij(); + const imuBias::ConstantBias& bias) { + return evaluatePreintegratedMeasurements(bias).deltaVij(); } - } /* ************************************************************************* */ @@ -167,64 +145,39 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { } /* ************************************************************************* */ -TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { - // Linearization point - imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); - - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - +TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { // Actual preintegrated values PreintegratedCombinedMeasurements pim = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs); + evaluatePreintegratedMeasurements(kZeroBiasHat); // Check derivative of rotation - boost::function theta = [=]( - const Vector3& a, const Vector3& w) { - return evaluatePreintegratedMeasurements( - Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta(); - }; + boost::function theta = + [=](const Vector3& a, const Vector3& w) { + return evaluatePreintegratedMeasurements(Bias(a, w)).theta(); + }; EXPECT( assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3))); EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1), pim.delRdelBiasOmega(), 1e-7)); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBias = + numericalDerivative11( + evaluatePreintegratedMeasurementsPosition, kZeroBiasHat); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBias = + numericalDerivative11( + &evaluatePreintegratedMeasurementsVelocity, kZeroBiasHat); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega(),1e-8)); + EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega(), 1e-8)); EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega(),1e-8)); + EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega(), 1e-8)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 0829fbc37..80c7f6502 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -12,17 +12,18 @@ /** * @file testImuFactor.cpp * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts, Frank Dellaert + * @author Luca Carlone + * @author Frank Dellaert + * @author Richard Roberts + * @author Stephen Williams */ #include -#include #include #include #include #include #include -#include #include #include @@ -31,22 +32,10 @@ #include #include -using namespace std; -using namespace gtsam; - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::V; -using symbol_shorthand::B; - -typedef imuBias::ConstantBias Bias; -static const Vector3 Z_3x1 = Vector3::Zero(); -static const Bias kZeroBiasHat, kZeroBias; +#include "imuFactorTesting.h" static const double kGravity = 10; static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); -static const Vector3 kZeroOmegaCoriolis(0, 0, 0); -static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); /* ************************************************************************* */ namespace { @@ -78,31 +67,18 @@ static boost::shared_ptr defaultParams() { // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ PreintegratedImuMeasurements evaluatePreintegratedMeasurements( - const Bias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - PreintegratedImuMeasurements result(defaultParams(), bias); - - list::const_iterator itAcc = measuredAccs.begin(); - list::const_iterator itOmega = measuredOmegas.begin(); - list::const_iterator itDeltaT = deltaTs.begin(); - for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { - result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); - } - return result; + const Bias& bias) { + PreintegratedImuMeasurements pim(defaultParams(), bias); + integrateMeasurements(testing::SomeMeasurements(), &pim); + return pim; } -Vector3 evaluatePreintegratedMeasurementsPosition( - const Bias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaPij(); +Vector3 evaluatePreintegratedMeasurementsPosition(const Bias& bias) { + return evaluatePreintegratedMeasurements(bias).deltaPij(); } -Vector3 evaluatePreintegratedMeasurementsVelocity( - const Bias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaVij(); +Vector3 evaluatePreintegratedMeasurementsVelocity(const Bias& bias) { + return evaluatePreintegratedMeasurements(bias).deltaVij(); } Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, @@ -478,33 +454,15 @@ TEST(ImuFactor, fistOrderExponential) { /* ************************************************************************* */ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - // Actual pre-integrated values PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, - deltaTs); + evaluatePreintegratedMeasurements(kZeroBias); // Check derivative of rotation - boost::function theta = [=]( - const Vector3& a, const Vector3& w) { - return evaluatePreintegratedMeasurements( - Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta(); - }; + boost::function theta = + [=](const Vector3& a, const Vector3& w) { + return evaluatePreintegratedMeasurements(Bias(a, w)).theta(); + }; EXPECT( assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3))); EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1), @@ -512,9 +470,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { // Check derivative of translation Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), - kZeroBias); + &evaluatePreintegratedMeasurementsPosition, kZeroBias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); @@ -523,9 +479,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { // Check derivative of velocity Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), - kZeroBias); + &evaluatePreintegratedMeasurementsVelocity, kZeroBias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));