From e47b4e5b2cfa02457840d314325652e2120dd590 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 14:34:04 -0700 Subject: [PATCH] Add a strong end-to-end test --- gtsam/navigation/tests/testAHRSFactor.cpp | 106 ++++++++++++++++++---- gtsam/navigation/tests/testImuFactor.cpp | 48 +++++----- 2 files changed, 113 insertions(+), 41 deletions(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 95674ce78..570e531d7 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -29,9 +29,12 @@ #include #include #include +#include #include #include +#include +#include "gtsam/nonlinear/LevenbergMarquardtParams.h" using namespace std::placeholders; using namespace std; @@ -39,7 +42,7 @@ using namespace gtsam; // Convenience for named keys using symbol_shorthand::B; -using symbol_shorthand::X; +using symbol_shorthand::R; Vector3 kZeroOmegaCoriolis(0, 0, 0); @@ -136,7 +139,7 @@ TEST(AHRSFactor, Error) { pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {}); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis, {}); // Check value Vector3 errorActual = factor.evaluateError(Ri, Rj, bias); @@ -145,8 +148,8 @@ TEST(AHRSFactor, Error) { // Check Derivatives Values values; - values.insert(X(1), Ri); - values.insert(X(2), Rj); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } @@ -165,7 +168,7 @@ TEST(AHRSFactor, ErrorWithBiases) { pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis); // Check value Vector3 errorExpected(0, 0, 0); @@ -174,8 +177,8 @@ TEST(AHRSFactor, ErrorWithBiases) { // Check Derivatives Values values; - values.insert(X(1), Ri); - values.insert(X(2), Rj); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } @@ -324,12 +327,12 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov())); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis); + AHRSFactor factor(R(1), R(2), B(1), pim, omegaCoriolis); // Check Derivatives Values values; - values.insert(X(1), Ri); - values.insert(X(2), Rj); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } @@ -350,7 +353,7 @@ TEST(AHRSFactor, predictTest) { expectedMeasCov = 200 * kMeasuredOmegaCovariance; EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov())); - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis); // Predict Rot3 x; @@ -368,7 +371,6 @@ TEST(AHRSFactor, predictTest) { EXPECT(assert_equal(expectedH, H, 1e-8)); } //****************************************************************************** - TEST(AHRSFactor, graphTest) { // linearization point Rot3 Ri(Rot3::RzRyRx(0, 0, 0)); @@ -393,15 +395,87 @@ TEST(AHRSFactor, graphTest) { } // pim.print("Pre integrated measurements"); - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); - values.insert(X(1), Ri); - values.insert(X(2), Rj); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); graph.push_back(factor); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0)); - EXPECT(assert_equal(expectedRot, result.at(X(2)))); + EXPECT(assert_equal(expectedRot, result.at(R(2)))); +} + +/* ************************************************************************* */ +TEST(AHRSFactor, bodyPSensorWithBias) { + using noiseModel::Diagonal; + + int numRotations = 10; + const Vector3 noiseBetweenBiasSigma(3.0e-6, 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); + + // Measurements in the sensor frame: + const double omega = 0.1; + const Vector3 realOmega(omega, 0, 0); + const Vector3 realBias(1, 2, 3); // large ! + const Vector3 measuredOmega = realOmega + realBias; + + auto p = std::make_shared(); + p->body_P_sensor = Pose3(Rot3::Yaw(M_PI_2), Point3(0, 0, 0)); + p->gyroscopeCovariance = 1e-8 * I_3x3; + double deltaT = 0.005; + + // Specify noise values on priors + const Vector3 priorNoisePoseSigmas(0.001, 0.001, 0.001); + const Vector3 priorNoiseBiasSigmas(0.5e-1, 0.5e-1, 0.5e-1); + SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); + + // Create a factor graph with priors on initial pose, velocity and bias + NonlinearFactorGraph graph; + Values values; + + graph.addPrior(R(0), Rot3(), priorNoisePose); + values.insert(R(0), Rot3()); + + // The key to this test is that we specify the bias, in the sensor frame, as + // known a priori. We also create factors below that encode our assumption + // that this bias is constant over time In theory, after optimization, we + // should recover that same bias estimate + graph.addPrior(B(0), realBias, priorNoiseBias); + values.insert(B(0), realBias); + + // Now add IMU factors and bias noise models + const Vector3 zeroBias(0, 0, 0); + for (int i = 1; i < numRotations; i++) { + PreintegratedAhrsMeasurements pim(p, realBias); + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredOmega, deltaT); + + // Create factors + graph.emplace_shared(R(i - 1), R(i), B(i - 1), pim); + graph.emplace_shared >(B(i - 1), B(i), zeroBias, + biasNoiseModel); + + values.insert(R(i), Rot3()); + values.insert(B(i), realBias); + } + + // Finally, optimize, and get bias at last time step + LevenbergMarquardtParams params; + // params.setVerbosityLM("SUMMARY"); + Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); + const Vector3 biasActual = result.at(B(numRotations - 1)); + + // Bias should be a self-fulfilling prophesy: + EXPECT(assert_equal(realBias, biasActual, 1e-3)); + + // Check that the successive rotations are all `omega` apart: + for (int i = 0; i < numRotations; i++) { + Rot3 expectedRot = Rot3::Pitch(omega * i); + Rot3 actualRot = result.at(R(i)); + EXPECT(assert_equal(expectedRot, actualRot, 1e-3)); + } } //****************************************************************************** diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 3c8f426cb..d4bc763ee 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -410,33 +410,33 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) { const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualDelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9)); } /* ************************************************************************* */ TEST(ImuFactor, PartialDerivativeLogmap) { // Linearization point - Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias + Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias // Measurements - Vector3 deltatheta(0, 0, 0); + Vector3 deltaTheta(0, 0, 0); - auto evaluateLogRotation = [=](const Vector3 deltatheta) { + auto evaluateLogRotation = [=](const Vector3 delta) { return Rot3::Logmap( - Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); + Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta))); }; // Compute numerical derivatives - Matrix expectedDelFdeltheta = - numericalDerivative11(evaluateLogRotation, deltatheta); + Matrix expectedDelFdelTheta = + numericalDerivative11(evaluateLogRotation, deltaTheta); - Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); + Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat); // Compare Jacobians - EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); + EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta)); } /* ************************************************************************* */ @@ -450,8 +450,8 @@ TEST(ImuFactor, fistOrderExponential) { // change w.r.t. linearization point double alpha = 0.0; - Vector3 deltabiasOmega; - deltabiasOmega << alpha, alpha, alpha; + Vector3 deltaBiasOmega; + deltaBiasOmega << alpha, alpha, alpha; const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); @@ -459,13 +459,12 @@ TEST(ImuFactor, fistOrderExponential) { Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign const Matrix expectedRot = Rot3::Expmap( - (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + (measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix(); const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); const Matrix3 actualRot = hatRot - * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); - // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix(); // This is a first order expansion so the equality is only an approximation EXPECT(assert_equal(expectedRot, actualRot)); @@ -728,7 +727,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { using noiseModel::Diagonal; typedef Bias Bias; - int numFactors = 10; + int numPoses = 10; Vector6 noiseBetweenBiasSigma; noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, 3.0e-6, 3.0e-6); @@ -761,7 +760,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); Vector3 zeroVel(0, 0, 0); - // Create a factor graph with priors on initial pose, vlocity and bias + // Create a factor graph with priors on initial pose, velocity and bias NonlinearFactorGraph graph; Values values; @@ -780,9 +779,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { // Now add IMU factors and bias noise models Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - for (int i = 1; i < numFactors; i++) { - PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p, - priorBias); + for (int i = 1; i < numPoses; i++) { + PreintegratedImuMeasurements pim(p, priorBias); for (int j = 0; j < 200; ++j) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -796,8 +794,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { } // Finally, optimize, and get bias at last time step - Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); - Bias biasActual = results.at(B(numFactors - 1)); + Values result = LevenbergMarquardtOptimizer(graph, values).optimize(); + Bias biasActual = result.at(B(numPoses - 1)); // And compare it with expected value (our prior) Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); @@ -851,11 +849,11 @@ struct ImuFactorMergeTest { actual_pim02.preintegrated(), tol)); EXPECT(assert_equal(pim02_expected, actual_pim02, tol)); - ImuFactor::shared_ptr factor01 = + auto factor01 = std::make_shared(X(0), V(0), X(1), V(1), B(0), pim01); - ImuFactor::shared_ptr factor12 = + auto factor12 = std::make_shared(X(1), V(1), X(2), V(2), B(0), pim12); - ImuFactor::shared_ptr factor02_expected = std::make_shared( + auto factor02_expected = std::make_shared( X(0), V(0), X(2), V(2), B(0), pim02_expected); ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);