diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h index fae2400b5..3247b56ee 100644 --- a/gtsam/navigation/tests/imuFactorTesting.h +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -35,6 +35,15 @@ static const Bias kZeroBiasHat, kZeroBias; static const Vector3 kZeroOmegaCoriolis(0, 0, 0); static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); +static const double kGravity = 10; +static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); + +// Realistic MEMS white noise characteristics. Angular and velocity random walk +// expressed in degrees respectively m/s per sqrt(hr). +auto radians = [](double t) { return t * M_PI / 180; }; +static const double kGyroSigma = radians(0.5) / 60; // 0.5 degree ARW +static const double kAccelSigma = 0.1 / 60; // 10 cm VRW + namespace testing { struct ImuMeasurement { diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 44ec48eda..743fc9baf 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -34,10 +34,21 @@ #include "imuFactorTesting.h" +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} + /* ************************************************************************* */ TEST( CombinedImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); @@ -45,7 +56,7 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) { double deltaT = 0.5; double tol = 1e-6; - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + auto p = testing::Params(); // Actual preintegrated values PreintegratedImuMeasurements expected1(p, bias); @@ -63,18 +74,18 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) { /* ************************************************************************* */ TEST( CombinedImuFactor, ErrorWithBiases ) { - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) + Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(0.5, 0.0, 0.0); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); Vector3 v2(0.5, 0.0, 0.0); - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + auto p = testing::Params(); p->omegaCoriolis = Vector3(0,0.1,0.1); PreintegratedImuMeasurements pim( - p, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); // Measurements Vector3 measuredOmega; @@ -87,7 +98,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); PreintegratedCombinedMeasurements combined_pim(p, - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -122,7 +133,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { /* ************************************************************************* */ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + auto p = testing::Params(); testing::SomeMeasurements measurements; boost::function preintegrated = @@ -144,16 +155,14 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { /* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity) { - imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + auto p = testing::Params(); // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3; - Vector3 measuredAcc; - measuredAcc << 0, 1.1, -9.81; - double deltaT = 0.001; + const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3; + const Vector3 measuredAcc(0, 1.1, -kGravity); + const double deltaT = 0.001; PreintegratedCombinedMeasurements pim(p, bias); @@ -161,40 +170,36 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - noiseModel::Gaussian::shared_ptr combinedmodel = + const noiseModel::Gaussian::shared_ptr combinedmodel = noiseModel::Gaussian::Covariance(pim.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); + const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); // Predict - NavState actual = pim.predict(NavState(), bias); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; - expectedVelocity << 0, 1, 0; + const NavState actual = pim.predict(NavState(), bias); + const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + const Vector3 expectedVelocity(0, 1, 0); EXPECT(assert_equal(expectedPose, actual.pose())); - EXPECT( - assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); } /* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + auto p = testing::Params(); PreintegratedCombinedMeasurements pim(p, bias); - Vector3 measuredAcc; - measuredAcc << 0, 0, -9.81; - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0; - double deltaT = 0.001; - double tol = 1e-4; + const Vector3 measuredAcc = - kGravityAlongNavZDown; + const Vector3 measuredOmega(0, 0, M_PI / 10.0); + const double deltaT = 0.001; + const double tol = 1e-4; for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); + const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); // Predict - Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; - Vector3 v(0, 0, 0), v2; - NavState actual = pim.predict(NavState(x, v), bias); - Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; + const Vector3 v(0, 0, 0), v2; + const NavState actual = pim.predict(NavState(x, v), bias); + const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, actual.pose(), tol)); } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index da9ed6a75..fb15e3b2f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -33,8 +33,16 @@ #include "imuFactorTesting.h" -static const double kGravity = 10; -static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} /* ************************************************************************* */ namespace { @@ -47,21 +55,6 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); } -// Define covariance matrices -/* ************************************************************************* */ -static const double kGyroSigma = 0.02; -static const double kAccelerometerSigma = 0.1; - -// Create default parameters with Z-down and above noise paramaters -static boost::shared_ptr defaultParams() { - auto p = PreintegrationParams::MakeSharedD(kGravity); - p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; - p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma - * I_3x3; - p->integrationCovariance = 0.0001 * I_3x3; - return p; -} - } // namespace /* ************************************************************************* */ @@ -78,7 +71,7 @@ TEST(ImuFactor, Accelerating) { Vector3(a, 0, 0)); const double T = 3.0; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -102,7 +95,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { double expectedDeltaT1(0.5); // Actual pre-integrated values - PreintegratedImuMeasurements actual1(defaultParams()); + PreintegratedImuMeasurements actual1(testing::Params()); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()))); @@ -169,7 +162,7 @@ static const NavState state2(x2, v2); /* ************************************************************************* */ TEST(ImuFactor, PreintegrationBaseMethods) { using namespace common; - auto p = defaultParams(); + auto p = testing::Params(); p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); p->use2ndOrderCoriolis = true; @@ -203,7 +196,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) { /* ************************************************************************* */ TEST(ImuFactor, ErrorAndJacobians) { using namespace common; - PreintegratedImuMeasurements pim(defaultParams()); + PreintegratedImuMeasurements pim(testing::Params()); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias))); @@ -260,7 +253,7 @@ TEST(ImuFactor, ErrorAndJacobians) { Matrix cov = pim.preintMeasCov(); Matrix R = RtR(cov.inverse()); Vector whitened = R * expectedError; - EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-5)); + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-4)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); @@ -282,7 +275,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - auto p = defaultParams(); + auto p = testing::Params(); p->omegaCoriolis = kNonZeroOmegaCoriolis; Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); @@ -328,7 +321,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - auto p = defaultParams(); + auto p = testing::Params(); p->omegaCoriolis = kNonZeroOmegaCoriolis; p->use2ndOrderCoriolis = true; @@ -438,13 +431,13 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { boost::function preintegrated = [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(defaultParams(), Bias(a, w)); + PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.preintegrated(); }; // Actual pre-integrated values - PreintegratedImuMeasurements pim(defaultParams()); + PreintegratedImuMeasurements pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), @@ -470,7 +463,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { const AcceleratingScenario scenario(nRb, p1, v1, a, Vector3(0, 0, M_PI / 10.0 + 0.3)); - auto p = defaultParams(); + auto p = testing::Params(); p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)), Point3(0.1, 0.05, 0.01)); p->omegaCoriolis = kNonZeroOmegaCoriolis; @@ -562,7 +555,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) { measuredAcc << 0, 1, -kGravity; double deltaT = 0.001; - PreintegratedImuMeasurements pim(defaultParams(), + PreintegratedImuMeasurements pim(testing::Params(), Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); for (int i = 0; i < 1000; ++i) @@ -590,7 +583,7 @@ TEST(ImuFactor, PredictRotation) { measuredAcc << 0, 0, -kGravity; double deltaT = 0.001; - PreintegratedImuMeasurements pim(defaultParams(), + PreintegratedImuMeasurements pim(testing::Params(), Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); for (int i = 0; i < 1000; ++i) @@ -614,7 +607,7 @@ TEST(ImuFactor, PredictArbitrary) { Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10)); const double T = 3.0; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); // // PreintegratedImuMeasurements pim = runner.integrate(T); // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); @@ -629,7 +622,7 @@ TEST(ImuFactor, PredictArbitrary) { Vector3 measuredOmega = runner.actualAngularVelocity(0); Vector3 measuredAcc = runner.actualSpecificForce(0); - auto p = defaultParams(); + auto p = testing::Params(); p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise PreintegratedImuMeasurements pim(p, biasHat); Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); @@ -662,7 +655,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Rotate sensor (z-down) to body (same as navigation) i.e. z-up - auto p = defaultParams(); + auto p = testing::Params(); p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0)); @@ -717,7 +710,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { // table exerts an equal and opposite force w.r.t gravity Vector3 measuredAcc(0, 0, -kGravity); - auto p = defaultParams(); + auto p = testing::Params(); p->n_gravity = Vector3(0, 0, -kGravity); p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); p->accelerometerCovariance = 1e-7 * I_3x3; @@ -826,7 +819,7 @@ struct ImuFactorMergeTest { } auto actual_pim02 = ImuFactor::Merge(pim01, pim12); EXPECT(assert_equal(expected_pim02.preintegrated(), actual_pim02.preintegrated(), tol)); -// EXPECT(assert_equal(expected_pim02, actual_pim02, tol)); + EXPECT(assert_equal(expected_pim02, actual_pim02, tol)); ImuFactor::shared_ptr factor_01 = boost::make_shared(X(0), V(0), X(1), V(1), B(0), pim01); diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index e9b611ef0..17b34c0d3 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -21,30 +21,28 @@ #include #include -using namespace std; -using namespace gtsam; +#include "imuFactorTesting.h" static const double kDt = 0.1; -static const double kGyroSigma = 0.02; -static const double kAccelSigma = 0.1; - -// Create default parameters with Z-down and above noise parameters -static boost::shared_ptr defaultParams() { - auto p = PreintegrationParams::MakeSharedD(10); - p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; - p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; - p->integrationCovariance = 0.0000001 * I_3x3; - return p; -} - Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); } +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} + /* ************************************************************************* */ TEST(PreintegrationBase, UpdateEstimate1) { - PreintegrationBase pim(defaultParams()); + PreintegrationBase pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta.setZero(); @@ -58,7 +56,7 @@ TEST(PreintegrationBase, UpdateEstimate1) { /* ************************************************************************* */ TEST(PreintegrationBase, UpdateEstimate2) { - PreintegrationBase pim(defaultParams()); + PreintegrationBase pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; @@ -73,7 +71,7 @@ TEST(PreintegrationBase, UpdateEstimate2) { /* ************************************************************************* */ TEST(PreintegrationBase, computeError) { - PreintegrationBase pim(defaultParams()); + PreintegrationBase pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; Matrix9 aH1, aH2; diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index c2fdb963d..a700f0979 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -34,7 +34,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); // Create default parameters with Z-up and above noise parameters -static boost::shared_ptr defaultParams() { +static boost::shared_ptr testing::Params() { auto p = PreintegrationParams::MakeSharedU(10); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; @@ -51,7 +51,7 @@ TEST(ScenarioRunner, Spin) { const Vector3 W(0, 0, w), V(0, 0, 0); const ConstantTwistScenario scenario(W, V); - auto p = defaultParams(); + auto p = testing::Params(); ScenarioRunner runner(&scenario, p, kDt); const double T = 2 * kDt; // seconds @@ -80,7 +80,7 @@ ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(&scenario, testing::Params(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -94,7 +94,7 @@ TEST(ScenarioRunner, Forward) { /* ************************************************************************* */ TEST(ScenarioRunner, ForwardWithBias) { using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(&scenario, testing::Params(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T, kNonZeroBias); @@ -108,7 +108,7 @@ TEST(ScenarioRunner, Circle) { const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(&scenario, testing::Params(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -126,7 +126,7 @@ TEST(ScenarioRunner, Loop) { const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(&scenario, testing::Params(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -157,7 +157,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating) { using namespace accelerating; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -170,7 +170,7 @@ TEST(ScenarioRunner, Accelerating) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias) { using namespace accelerating; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -185,7 +185,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -216,7 +216,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating2) { using namespace accelerating2; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -229,7 +229,7 @@ TEST(ScenarioRunner, Accelerating2) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias2) { using namespace accelerating2; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -244,7 +244,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -276,7 +276,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating3) { using namespace accelerating3; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -289,7 +289,7 @@ TEST(ScenarioRunner, Accelerating3) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias3) { using namespace accelerating3; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -304,7 +304,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -337,7 +337,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating4) { using namespace accelerating4; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -350,7 +350,7 @@ TEST(ScenarioRunner, Accelerating4) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias4) { using namespace accelerating4; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(&scenario, testing::Params(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -365,7 +365,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(&scenario, testing::Params(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));