From 344a36c568c7fdd22d256dd57afa38ccd5701019 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Jun 2024 16:11:07 -0700 Subject: [PATCH 01/16] Added clarification --- gtsam/navigation/AHRSFactor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 39969a8f3..14548eafd 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -54,8 +54,10 @@ void PreintegratedAhrsMeasurements::integrateMeasurement( PreintegratedRotation::integrateMeasurement(measuredOmega, biasHat_, deltaT, &D_incrR_integratedOmega, &Fr); - // first order uncertainty propagation - // the deltaT allows to pass from continuous time noise to discrete time noise + // First order uncertainty propagation + // The deltaT allows to pass from continuous time noise to discrete time + // noise. Comparing with the IMUFactor.cpp implementation, the latter is an + // approximation for C * (wCov / dt) * C.transpose(), with C \approx I * dt. preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT; } From e81b38114c27fea973226575d5af1351967a6fcc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Jun 2024 16:11:24 -0700 Subject: [PATCH 02/16] Ignore clangd cache --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index e3f7613fe..6c8ac97b5 100644 --- a/.gitignore +++ b/.gitignore @@ -19,3 +19,4 @@ CMakeLists.txt.user* xcode/ /Dockerfile /python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb +.cache/ From c18191d98df6fcc717d62ba7043b5d35d78713bb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Jun 2024 16:11:55 -0700 Subject: [PATCH 03/16] Cleaned up tests --- gtsam/navigation/tests/testAHRSFactor.cpp | 397 ++++++++-------------- 1 file changed, 148 insertions(+), 249 deletions(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 89fdd4f71..5f192d9c7 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -18,12 +18,17 @@ * @author Varun Agrawal */ -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -32,33 +37,21 @@ using namespace std; using namespace gtsam; // Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::V; using symbol_shorthand::B; +using symbol_shorthand::X; -Vector3 kZeroOmegaCoriolis(0,0,0); +Vector3 kZeroOmegaCoriolis(0, 0, 0); // Define covariance matrices -double accNoiseVar = 0.01; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +double gyroNoiseVar = 0.01; +const Matrix3 kMeasuredOmegaCovariance = gyroNoiseVar * I_3x3; //****************************************************************************** namespace { -Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i, - const Rot3 rot_j, const Vector3& bias) { - return factor.evaluateError(rot_i, rot_j, bias); -} - -Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, - const Rot3 rot_j, const Vector3& bias) { - return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3)); -} - -PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements( - const Vector3& bias, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3::Zero()) { - PreintegratedAhrsMeasurements result(bias, I_3x3); +PreintegratedAhrsMeasurements integrateMeasurements( + const Vector3& biasHat, const list& measuredOmegas, + const list& deltaTs) { + PreintegratedAhrsMeasurements result(biasHat, I_3x3); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); @@ -68,79 +61,59 @@ PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements( return result; } - -Rot3 evaluatePreintegratedMeasurementsRotation( - const Vector3& bias, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3::Zero()) { - return Rot3( - evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, - initialRotationRate).deltaRij()); -} - -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, - const double deltaT) { - return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); -} - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { - return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); -} -} +} // namespace //****************************************************************************** -TEST( AHRSFactor, PreintegratedAhrsMeasurements ) { +TEST(AHRSFactor, PreintegratedAhrsMeasurements) { // Linearization point - Vector3 bias(0,0,0); ///< Current estimate of angular rate bias + Vector3 biasHat(0, 0, 0); ///< Current estimate of angular rate bias // Measurements Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; // Expected preintegrated values - Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT1(0.5); + Rot3 expectedDeltaR1 = Rot3::Roll(0.5 * M_PI / 100.0); // Actual preintegrated values - PreintegratedAhrsMeasurements actual1(bias, Z_3x3); + PreintegratedAhrsMeasurements actual1(biasHat, kMeasuredOmegaCovariance); actual1.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); + DOUBLES_EQUAL(deltaT, actual1.deltaTij(), 1e-6); + + // Check the covariance + Matrix3 expectedMeasCov = kMeasuredOmegaCovariance * deltaT; + EXPECT(assert_equal(expectedMeasCov, actual1.preintMeasCov(), 1e-6)); // Integrate again - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT2(1); + Rot3 expectedDeltaR2 = Rot3::Roll(2.0 * 0.5 * M_PI / 100.0); // Actual preintegrated values PreintegratedAhrsMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); + DOUBLES_EQUAL(deltaT * 2, actual2.deltaTij(), 1e-6); } //****************************************************************************** -TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { - Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4; +TEST(AHRSFactor, PreintegratedAhrsMeasurementsConstructor) { + Matrix3 gyroscopeCovariance = I_3x3 * 0.4; Vector3 omegaCoriolis(0.1, 0.5, 0.9); PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis); - Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias + Vector3 bias(1.0, 2.0, 3.0); ///< Current estimate of angular rate bias Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); double deltaTij = 0.02; - Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; - Matrix3 preintMeasCov = Matrix3::Ones()*0.2; + Matrix3 delRdelBiasOmega = I_3x3 * 0.5; + Matrix3 preintMeasCov = I_3x3 * 0.2; PreintegratedAhrsMeasurements actualPim( - std::make_shared(params), - bias, - deltaTij, - deltaRij, - delRdelBiasOmega, - preintMeasCov); + std::make_shared(params), bias, deltaTij, + deltaRij, delRdelBiasOmega, preintMeasCov); EXPECT(assert_equal(gyroscopeCovariance, - actualPim.p().getGyroscopeCovariance(), 1e-6)); - EXPECT(assert_equal(omegaCoriolis, - *(actualPim.p().getOmegaCoriolis()), 1e-6)); + actualPim.p().getGyroscopeCovariance(), 1e-6)); + EXPECT( + assert_equal(omegaCoriolis, *(actualPim.p().getOmegaCoriolis()), 1e-6)); EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6)); DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6); EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6)); @@ -151,198 +124,148 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point - Vector3 bias(0.,0.,0.); // Bias - Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); - Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); + Vector3 bias(0., 0., 0.); // Bias + Rot3 Ri(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); + Rot3 Rj(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); // Measurements - Vector3 measuredOmega; - measuredOmega << M_PI / 100, 0, 0; + Vector3 measuredOmega(M_PI / 100, 0, 0); double deltaT = 1.0; - PreintegratedAhrsMeasurements pim(bias, Z_3x3); + PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance); pim.integrateMeasurement(measuredOmega, deltaT); // Create factor AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {}); - Vector3 errorActual = factor.evaluateError(x1, x2, bias); - - // Expected error - Vector3 errorExpected(3); - errorExpected << 0, 0, 0; + // Check value + Vector3 errorActual = factor.evaluateError(Ri, Rj, bias); + Vector3 errorExpected(0, 0, 0); EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6)); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); - Matrix H2e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); - Matrix H3e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); - Matrix RH2e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); - - // Actual Jacobians - Matrix H1a, H2a, H3a; - (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); - - // rotations - EXPECT(assert_equal(RH1e, H1a, 1e-5)); - // 1e-5 needs to be added only when using quaternions for rotations - - EXPECT(assert_equal(H2e, H2a, 1e-5)); - - // rotations - EXPECT(assert_equal(RH2e, H2a, 1e-5)); - // 1e-5 needs to be added only when using quaternions for rotations - - EXPECT(assert_equal(H3e, H3a, 1e-5)); - // 1e-5 needs to be added only when using quaternions for rotations + // Check Derivatives + Values values; + values.insert(X(1), Ri); + values.insert(X(2), Rj); + values.insert(B(1), bias); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } /* ************************************************************************* */ TEST(AHRSFactor, ErrorWithBiases) { // Linearization point - Vector3 bias(0, 0, 0.3); - Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); - Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); + Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); + Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); double deltaT = 1.0; - - PreintegratedAhrsMeasurements pim(Vector3(0,0,0), - Z_3x3); + PreintegratedAhrsMeasurements pim(Vector3(0, 0, 0), kMeasuredOmegaCovariance); pim.integrateMeasurement(measuredOmega, deltaT); // Create factor AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); - Vector errorActual = factor.evaluateError(x1, x2, bias); - - // Expected error - Vector errorExpected(3); - errorExpected << 0, 0, 0; + // Check value + Vector3 errorExpected(0, 0, 0); + Vector3 errorActual = factor.evaluateError(Ri, Rj, bias); EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); - Matrix H2e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); - Matrix H3e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); - Matrix RH2e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); - Matrix RH3e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a; - (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); + // Check Derivatives + Values values; + values.insert(X(1), Ri); + values.insert(X(2), Rj); + values.insert(B(1), bias); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } //****************************************************************************** -TEST( AHRSFactor, PartialDerivativeExpmap ) { +TEST(AHRSFactor, PartialDerivativeExpmap) { // Linearization point - Vector3 biasOmega(0,0,0); + Vector3 biasOmega(0, 0, 0); // Measurements - Vector3 measuredOmega; - measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega(0.1, 0, 0); double deltaT = 0.5; + auto f = [&](const Vector3& biasOmega) { + return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); + }; + // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11( - std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega); + Matrix expectedH = numericalDerivative11(f, biasOmega); - const Matrix3 Jr = Rot3::ExpmapDerivative( - (measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = + Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualH = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); + EXPECT(assert_equal(expectedH, actualH, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations - } //****************************************************************************** -TEST( AHRSFactor, PartialDerivativeLogmap ) { +TEST(AHRSFactor, PartialDerivativeLogmap) { // Linearization point - Vector3 thetahat; - 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; - deltatheta << 0, 0, 0; + auto f = [thetaHat](const Vector3 deltaTheta) { + return Rot3::Logmap( + Rot3::Expmap(thetaHat).compose(Rot3::Expmap(deltaTheta))); + }; // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11( - std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta); + Vector3 deltaTheta(0, 0, 0); + Matrix expectedH = numericalDerivative11(f, deltaTheta); - const Vector3 x = thetahat; // parametrization of so(3) - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = x.norm(); - const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X - + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X - * X; + const Vector3 x = thetaHat; // parametrization of so(3) + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + double norm = x.norm(); + const Matrix3 actualH = + I_3x3 + 0.5 * X + + (1 / (norm * norm) - (1 + cos(norm)) / (2 * norm * sin(norm))) * X * X; // Compare Jacobians - EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); - + EXPECT(assert_equal(expectedH, actualH)); } //****************************************************************************** -TEST( AHRSFactor, fistOrderExponential ) { +TEST(AHRSFactor, fistOrderExponential) { // Linearization point - Vector3 biasOmega(0,0,0); + Vector3 biasOmega(0, 0, 0); // Measurements - Vector3 measuredOmega; - measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega(0.1, 0, 0); double deltaT = 1.0; // change w.r.t. linearization point double alpha = 0.0; - Vector3 deltabiasOmega; - deltabiasOmega << alpha, alpha, alpha; + Vector3 deltaBiasOmega(alpha, alpha, alpha); - const Matrix3 Jr = Rot3::ExpmapDerivative( - (measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = + Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); - Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 delRdelBiasOmega = + -Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap( - (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = + Rot3::Expmap((measuredOmega - biasOmega - deltaBiasOmega) * deltaT) + .matrix(); const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = hatRot - * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + const Matrix3 actualRot = + hatRot * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix(); // Compare Jacobians EXPECT(assert_equal(expectedRot, actualRot)); } //****************************************************************************** -TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { +TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) { // Linearization point - Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias + Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); @@ -361,98 +284,76 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Actual preintegrated values PreintegratedAhrsMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, - Vector3(M_PI / 100.0, 0.0, 0.0)); + integrateMeasurements(bias, measuredOmegas, deltaTs); + + auto f = [&](const Vector3& bias) { + return integrateMeasurements(bias, measuredOmegas, deltaTs).deltaRij(); + }; // Compute numerical derivatives - Matrix expectedDelRdelBias = - numericalDerivative11( - std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1, - measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias); + Matrix expectedDelRdelBias = numericalDerivative11(f, bias); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians - EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, + preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } -#include -#include - //****************************************************************************** -TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { - +TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 bias(0, 0, 0.3); - Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); - Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); + Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); + Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); double deltaT = 1.0; const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), - Point3(1, 0, 0)); + Point3(1, 0, 0)); - PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredOmegaCovariance); pim.integrateMeasurement(measuredOmega, deltaT); // Check preintegrated covariance - EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov())); + EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov())); // Create factor AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); - Matrix H2e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); - Matrix H3e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); - Matrix RH2e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); - Matrix RH3e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a; - (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); + // Check Derivatives + Values values; + values.insert(X(1), Ri); + values.insert(X(2), Rj); + values.insert(B(1), bias); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } + //****************************************************************************** -TEST (AHRSFactor, predictTest) { - Vector3 bias(0,0,0); +TEST(AHRSFactor, predictTest) { + Vector3 bias(0, 0, 0); // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0; + Vector3 measuredOmega(0, 0, M_PI / 10.0); double deltaT = 0.2; - PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance); for (int i = 0; i < 1000; ++i) { pim.integrateMeasurement(measuredOmega, deltaT); } // Check preintegrated covariance - Matrix expectedMeasCov(3,3); - expectedMeasCov = 200*kMeasuredAccCovariance; + Matrix expectedMeasCov(3, 3); + expectedMeasCov = 200 * kMeasuredOmegaCovariance; EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov())); AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); // Predict Rot3 x; - Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0); + Rot3 expectedRot = Rot3::Ypr(20 * M_PI, 0, 0); Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); @@ -462,29 +363,27 @@ TEST (AHRSFactor, predictTest) { // Actual Jacobians Matrix H; - (void) pim.predict(bias,H); + (void)pim.predict(bias, H); EXPECT(assert_equal(expectedH, H, 1e-8)); } //****************************************************************************** -#include -#include -TEST (AHRSFactor, graphTest) { +TEST(AHRSFactor, graphTest) { // linearization point - Rot3 x1(Rot3::RzRyRx(0, 0, 0)); - Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0)); - Vector3 bias(0,0,0); + Rot3 Ri(Rot3::RzRyRx(0, 0, 0)); + Rot3 Rj(Rot3::RzRyRx(0, M_PI / 4, 0)); + Vector3 bias(0, 0, 0); // PreIntegrator Vector3 biasHat(0, 0, 0); - PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(biasHat, kMeasuredOmegaCovariance); // Pre-integrate measurements Vector3 measuredOmega(0, M_PI / 20, 0); double deltaT = 1; // Create Factor - noiseModel::Base::shared_ptr model = // + noiseModel::Base::shared_ptr model = // noiseModel::Gaussian::Covariance(pim.preintMeasCov()); NonlinearFactorGraph graph; Values values; @@ -492,10 +391,10 @@ TEST (AHRSFactor, graphTest) { pim.integrateMeasurement(measuredOmega, deltaT); } - // pim.print("Pre integrated measurementes"); + // pim.print("Pre integrated measurements"); AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); - values.insert(X(1), x1); - values.insert(X(2), x2); + values.insert(X(1), Ri); + values.insert(X(2), Rj); values.insert(B(1), bias); graph.push_back(factor); LevenbergMarquardtOptimizer optimizer(graph, values); From d72f31fbc629dd60e5b61b9a2149d6f232867e1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 00:25:33 -0700 Subject: [PATCH 04/16] Actually use displacement in test --- gtsam/navigation/tests/testAHRSFactor.cpp | 32 ++++++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 5f192d9c7..9d512d595 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -30,6 +30,7 @@ #include #include +#include #include using namespace std::placeholders; @@ -300,6 +301,29 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) { // 1e-3 needs to be added only when using quaternions for rotations } +//****************************************************************************** +// TEST(AHRSFactor, PimWithBodyDisplacement) { +// Vector3 bias(0, 0, 0.3); +// Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); +// double deltaT = 1.0; + +// auto p = std::make_shared(); +// p->gyroscopeCovariance = kMeasuredOmegaCovariance; +// p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0)); +// PreintegratedAhrsMeasurements pim(p, Vector3::Zero()); + +// pim.integrateMeasurement(measuredOmega, deltaT); + +// Vector3 biasOmegaIncr(0.01, 0.0, 0.0); +// Matrix3 actualH; +// pim.biascorrectedDeltaRij(biasOmegaIncr, actualH); + +// // Numerical derivative using a lambda function: +// auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); }; +// Matrix3 expectedH = numericalDerivative11(f, bias); +// EXPECT(assert_equal(expectedH, actualH)); +// } + //****************************************************************************** TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 bias(0, 0, 0.3); @@ -312,10 +336,10 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); double deltaT = 1.0; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), - Point3(1, 0, 0)); - - PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredOmegaCovariance); + auto p = std::make_shared(); + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(1, 2, 3)), Point3(1, 0, 0)); + PreintegratedAhrsMeasurements pim(p, Vector3::Zero()); pim.integrateMeasurement(measuredOmega, deltaT); From d290f8326841552847fd711073e9b2708f6c6e75 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 00:25:56 -0700 Subject: [PATCH 05/16] Test incrementalRotation --- gtsam/navigation/PreintegratedRotation.cpp | 10 +- gtsam/navigation/PreintegratedRotation.h | 36 ++++-- .../tests/testPreintegratedRotation.cpp | 122 ++++++++++++++++++ 3 files changed, 154 insertions(+), 14 deletions(-) create mode 100644 gtsam/navigation/tests/testPreintegratedRotation.cpp diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index a9d4a28bb..cd8f2b64c 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -89,13 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! } -void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, +void PreintegratedRotation::integrateMeasurement( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { Matrix3 D_incrR_integratedOmega; const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); + D_incrR_integratedOmega); // If asked, pass first derivative as well if (optional_D_incrR_integratedOmega) { @@ -108,8 +108,8 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = + incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index c80399f14..ed7b6f29c 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -65,7 +65,6 @@ struct GTSAM_EXPORT PreintegratedRotationParams { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); @@ -159,15 +158,34 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Main functionality /// @{ - /// Take the gyro measurement, correct it using the (constant) bias estimate - /// and possibly the sensor pose, and then integrate it forward in time to yield - /// an incremental rotation. - Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + /** + * @brief Take the gyro measurement, correct it using the (constant) bias + * estimate and possibly the sensor pose, and then integrate it forward in + * time to yield an incremental rotation. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param biasHat The bias estimate + * @param deltaT The time interval + * @param D_incrR_integratedOmega Jacobian of the incremental rotation w.r.t. + * delta_T * (measuredOmega - biasHat), possibly rotated by body_R_sensor. + * @return The incremental rotation + */ + Rot3 incrementalRotation( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const; - /// Calculate an incremental rotation given the gyro measurement and a time interval, - /// and update both deltaTij_ and deltaRij_. - void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + /** + * @brief Calculate an incremental rotation given the gyro measurement and a + * time interval, and update both deltaTij_ and deltaRij_. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param biasHat The bias estimate + * @param deltaT The time interval + * @param D_incrR_integratedOmega Optional Jacobian of the incremental + * rotation w.r.t. the integrated angular velocity + * @param F Optional Jacobian of the incremental rotation w.r.t. the bias + * estimate + */ + void integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega = {}, OptionalJacobian<3, 3> F = {}); diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp new file mode 100644 index 000000000..e264a6929 --- /dev/null +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPreintegratedRotation.cpp + * @brief Unit test for PreintegratedRotation + * @author Frank Dellaert + */ + +#include +#include +#include + +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +using namespace gtsam; + +//****************************************************************************** +namespace simple_roll { +auto p = std::make_shared(); +PreintegratedRotation pim(p); +const double roll = 0.1; +const Vector3 measuredOmega(roll, 0, 0); +const Vector3 biasHat(0, 0, 0); +const double deltaT = 0.5; +} // namespace simple_roll + +//****************************************************************************** +TEST(PreintegratedRotation, incrementalRotation) { + using namespace simple_roll; + + // Check the value. + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, + D_incrR_integratedOmega); + Rot3 expected = Rot3::Roll(roll * deltaT); + EXPECT(assert_equal(expected, incrR, 1e-9)); + + // Lambda for numerical derivative: + auto f = [&](const Vector3& x, const Vector3& y) { + return pim.incrementalRotation(x, y, deltaT, {}); + }; + + // NOTE(frank): these derivatives as computed by the function violate the + // "Jacobian contract". We should refactor this. It's not clear that the + // deltaT factor is actually understood in calling code. + + // Check derivative with respect to measuredOmega + EXPECT(assert_equal( + numericalDerivative21(f, measuredOmega, biasHat), + deltaT * D_incrR_integratedOmega)); + + // Check derivative with respect to biasHat + EXPECT(assert_equal( + numericalDerivative22(f, measuredOmega, biasHat), + -deltaT * D_incrR_integratedOmega)); +} + +//****************************************************************************** +static std::shared_ptr paramsWithTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); + return p; +} + +namespace roll_in_rotated_frame { +auto p = paramsWithTransform(); +PreintegratedRotation pim(p); +const double roll = 0.1; +const Vector3 measuredOmega(roll, 0, 0); +const Vector3 biasHat(0, 0, 0); +const double deltaT = 0.5; +} // namespace roll_in_rotated_frame + +//****************************************************************************** +TEST(PreintegratedRotation, incrementalRotationWithTransform) { + using namespace roll_in_rotated_frame; + + // Check the value. + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, + D_incrR_integratedOmega); + Rot3 expected = Rot3::Pitch(roll * deltaT); + EXPECT(assert_equal(expected, incrR, 1e-9)); + + // Lambda for numerical derivative: + auto f = [&](const Vector3& x, const Vector3& y) { + return pim.incrementalRotation(x, y, deltaT, {}); + }; + + // NOTE(frank): Here, once again, the derivatives are weird, as they do not + // take the rotation into account. They *are* the derivatives of the rotated + // omegas, but not the derivatives with respect to the function arguments. + + // Check derivative with respect to measuredOmega + EXPECT(assert_equal( + numericalDerivative21(f, measuredOmega, biasHat), + deltaT * D_incrR_integratedOmega)); + + // Check derivative with respect to biasHat + EXPECT(assert_equal( + numericalDerivative22(f, measuredOmega, biasHat), + -deltaT * D_incrR_integratedOmega)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From 2dad81d671247c5b7d2ba8ad5688c16108193bd4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 08:59:48 -0700 Subject: [PATCH 06/16] Function object with sane Jacobian --- gtsam/navigation/PreintegratedRotation.cpp | 21 ++--- gtsam/navigation/PreintegratedRotation.h | 48 ++++++++--- .../tests/testPreintegratedRotation.cpp | 81 +++++++++---------- 3 files changed, 83 insertions(+), 67 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index cd8f2b64c..720593558 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -68,17 +68,15 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other, && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } -Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const { - +Rot3 PreintegratedRotation::IncrementalRotation::operator()( + const Vector3& bias, OptionalJacobian<3, 3> H_bias) const { // First we compensate the measurements for the bias - Vector3 correctedOmega = measuredOmega - biasHat; + Vector3 correctedOmega = measuredOmega - bias; // Then compensate for sensor-body displacement: we express the quantities // (originally in the IMU frame) into the body frame - if (p_->body_P_sensor) { - Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix(); + if (body_P_sensor) { + const Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); // rotation rate vector in the body frame correctedOmega = body_R_sensor * correctedOmega; } @@ -86,7 +84,10 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; - return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !! + if (H_bias) + *H_bias *= -deltaT; // Correct so accurately reflects bias derivative + return incrR; } void PreintegratedRotation::integrateMeasurement( @@ -94,8 +95,8 @@ void PreintegratedRotation::integrateMeasurement( OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); + IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + const Rot3 incrR = f(biasHat, D_incrR_integratedOmega); // If asked, pass first derivative as well if (optional_D_incrR_integratedOmega) { diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index ed7b6f29c..d5f930063 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -21,9 +21,9 @@ #pragma once -#include #include #include +#include namespace gtsam { @@ -159,19 +159,25 @@ class GTSAM_EXPORT PreintegratedRotation { /// @{ /** - * @brief Take the gyro measurement, correct it using the (constant) bias - * estimate and possibly the sensor pose, and then integrate it forward in - * time to yield an incremental rotation. + * @brief Function object for incremental rotation. * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param biasHat The bias estimate - * @param deltaT The time interval - * @param D_incrR_integratedOmega Jacobian of the incremental rotation w.r.t. - * delta_T * (measuredOmega - biasHat), possibly rotated by body_R_sensor. - * @return The incremental rotation + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. */ - Rot3 incrementalRotation( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& body_P_sensor; + + /** + * @brief Integrate angular velocity, but corrected by bias. + * @param bias The bias estimate + * @param H_bias Jacobian of the rotation w.r.t. bias. + * @return The incremental rotation + */ + Rot3 operator()(const Vector3& biasHat, + OptionalJacobian<3, 3> H_bias = {}) const; + }; /** * @brief Calculate an incremental rotation given the gyro measurement and a @@ -198,6 +204,24 @@ class GTSAM_EXPORT PreintegratedRotation { /// @} + /// @name Deprecated API + /// @{ + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 + /// @deprecated: use IncrementalRotation functor with sane Jacobian + inline Rot3 GTSAM_DEPRECATED incrementalRotation( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const { + IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + Rot3 incrR = f(biasHat, D_incrR_integratedOmega); + // Backwards compatible "weird" Jacobian, no longer used. + if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; + return incrR; + } +#endif + + /// @} + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index e264a6929..89b714534 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -30,40 +30,36 @@ using namespace gtsam; namespace simple_roll { auto p = std::make_shared(); PreintegratedRotation pim(p); -const double roll = 0.1; -const Vector3 measuredOmega(roll, 0, 0); -const Vector3 biasHat(0, 0, 0); +const double omega = 0.1; +const Vector3 measuredOmega(omega, 0, 0); +const Vector3 bias(0, 0, 0); const double deltaT = 0.5; } // namespace simple_roll //****************************************************************************** -TEST(PreintegratedRotation, incrementalRotation) { +TEST(PreintegratedRotation, IncrementalRotation) { using namespace simple_roll; // Check the value. - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); - Rot3 expected = Rot3::Roll(roll * deltaT); + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + const Rot3 incrR = f(bias, H_bias); + Rot3 expected = Rot3::Roll(omega * deltaT); EXPECT(assert_equal(expected, incrR, 1e-9)); - // Lambda for numerical derivative: - auto f = [&](const Vector3& x, const Vector3& y) { + // Check the derivative: + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + + // Ephemeral test for deprecated Jacobian: + Matrix3 D_incrR_integratedOmega; + (void)pim.incrementalRotation(measuredOmega, bias, deltaT, + D_incrR_integratedOmega); + auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - - // NOTE(frank): these derivatives as computed by the function violate the - // "Jacobian contract". We should refactor this. It's not clear that the - // deltaT factor is actually understood in calling code. - - // Check derivative with respect to measuredOmega EXPECT(assert_equal( - numericalDerivative21(f, measuredOmega, biasHat), - deltaT * D_incrR_integratedOmega)); - - // Check derivative with respect to biasHat - EXPECT(assert_equal( - numericalDerivative22(f, measuredOmega, biasHat), + numericalDerivative22(g, measuredOmega, bias), -deltaT * D_incrR_integratedOmega)); } @@ -77,40 +73,35 @@ static std::shared_ptr paramsWithTransform() { namespace roll_in_rotated_frame { auto p = paramsWithTransform(); PreintegratedRotation pim(p); -const double roll = 0.1; -const Vector3 measuredOmega(roll, 0, 0); -const Vector3 biasHat(0, 0, 0); +const double omega = 0.1; +const Vector3 measuredOmega(omega, 0, 0); +const Vector3 bias(0, 0, 0); const double deltaT = 0.5; } // namespace roll_in_rotated_frame //****************************************************************************** -TEST(PreintegratedRotation, incrementalRotationWithTransform) { +TEST(PreintegratedRotation, IncrementalRotationWithTransform) { using namespace roll_in_rotated_frame; // Check the value. - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); - Rot3 expected = Rot3::Pitch(roll * deltaT); - EXPECT(assert_equal(expected, incrR, 1e-9)); + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + Rot3 expected = Rot3::Pitch(omega * deltaT); + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); - // Lambda for numerical derivative: - auto f = [&](const Vector3& x, const Vector3& y) { + // Check the derivative: + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + + // Ephemeral test for deprecated Jacobian: + Matrix3 D_incrR_integratedOmega; + (void)pim.incrementalRotation(measuredOmega, bias, deltaT, + D_incrR_integratedOmega); + auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - - // NOTE(frank): Here, once again, the derivatives are weird, as they do not - // take the rotation into account. They *are* the derivatives of the rotated - // omegas, but not the derivatives with respect to the function arguments. - - // Check derivative with respect to measuredOmega EXPECT(assert_equal( - numericalDerivative21(f, measuredOmega, biasHat), - deltaT * D_incrR_integratedOmega)); - - // Check derivative with respect to biasHat - EXPECT(assert_equal( - numericalDerivative22(f, measuredOmega, biasHat), + numericalDerivative22(g, measuredOmega, bias), -deltaT * D_incrR_integratedOmega)); } From b531df7004a7cfe156142271972d87a734873f5f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 10:07:57 -0700 Subject: [PATCH 07/16] Fixed derivative with transform, deprecated integrateMeasurement --- gtsam/navigation/AHRSFactor.cpp | 7 +- gtsam/navigation/PreintegratedRotation.cpp | 45 ++++-- gtsam/navigation/PreintegratedRotation.h | 26 ++-- .../tests/testPreintegratedRotation.cpp | 139 ++++++++++++++---- 4 files changed, 155 insertions(+), 62 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 14548eafd..f23e41ec8 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -49,10 +49,9 @@ void PreintegratedAhrsMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedAhrsMeasurements::integrateMeasurement( const Vector3& measuredOmega, double deltaT) { - - Matrix3 D_incrR_integratedOmega, Fr; - PreintegratedRotation::integrateMeasurement(measuredOmega, - biasHat_, deltaT, &D_incrR_integratedOmega, &Fr); + Matrix3 Fr; + PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_, + deltaT, &Fr); // First order uncertainty propagation // The deltaT allows to pass from continuous time noise to discrete time diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 720593558..bbc607800 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -74,34 +74,32 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()( Vector3 correctedOmega = measuredOmega - bias; // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame + // (originally in the IMU frame) into the body frame. If Jacobian is + // requested, the rotation matrix is obtained as `rotate` Jacobian. + Matrix3 body_R_sensor; if (body_P_sensor) { - const Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); // rotation rate vector in the body frame - correctedOmega = body_R_sensor * correctedOmega; + correctedOmega = body_P_sensor->rotation().rotate( + correctedOmega, {}, H_bias ? &body_R_sensor : nullptr); } // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !! - if (H_bias) + if (H_bias) { *H_bias *= -deltaT; // Correct so accurately reflects bias derivative + if (body_P_sensor) *H_bias *= body_R_sensor; + } return incrR; } -void PreintegratedRotation::integrateMeasurement( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, +void PreintegratedRotation::integrateGyroMeasurement( + const Vector3& measuredOmega, const Vector3& bias, double deltaT, OptionalJacobian<3, 3> F) { - Matrix3 D_incrR_integratedOmega; + Matrix3 H_bias; IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - const Rot3 incrR = f(biasHat, D_incrR_integratedOmega); - - // If asked, pass first derivative as well - if (optional_D_incrR_integratedOmega) { - *optional_D_incrR_integratedOmega << D_incrR_integratedOmega; - } + const Rot3 incrR = f(bias, H_bias); // Update deltaTij and rotation deltaTij_ += deltaT; @@ -109,8 +107,23 @@ void PreintegratedRotation::integrateMeasurement( // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = - incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias; +} + +// deprecated! +void PreintegratedRotation::integrateMeasurement( + const Vector3& measuredOmega, const Vector3& bias, double deltaT, + OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, + OptionalJacobian<3, 3> F) { + integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + + // If asked, pass obsolete Jacobians as well + if (optional_D_incrR_integratedOmega) { + Matrix3 H_bias; + IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + const Rot3 incrR = f(bias, H_bias); + *optional_D_incrR_integratedOmega << H_bias / -deltaT; + } } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index d5f930063..b87f4208b 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -175,7 +175,7 @@ class GTSAM_EXPORT PreintegratedRotation { * @param H_bias Jacobian of the rotation w.r.t. bias. * @return The incremental rotation */ - Rot3 operator()(const Vector3& biasHat, + Rot3 operator()(const Vector3& bias, OptionalJacobian<3, 3> H_bias = {}) const; }; @@ -183,17 +183,13 @@ class GTSAM_EXPORT PreintegratedRotation { * @brief Calculate an incremental rotation given the gyro measurement and a * time interval, and update both deltaTij_ and deltaRij_. * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param biasHat The bias estimate + * @param bias The bias estimate * @param deltaT The time interval - * @param D_incrR_integratedOmega Optional Jacobian of the incremental - * rotation w.r.t. the integrated angular velocity - * @param F Optional Jacobian of the incremental rotation w.r.t. the bias - * estimate + * @param F Jacobian of internal compose, used in AhrsFactor. */ - void integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega = {}, - OptionalJacobian<3, 3> F = {}); + void integrateGyroMeasurement(const Vector3& measuredOmega, + const Vector3& bias, double deltaT, + OptionalJacobian<3, 3> F = {}); /// Return a bias corrected version of the integrated rotation, with optional Jacobian Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, @@ -210,14 +206,20 @@ class GTSAM_EXPORT PreintegratedRotation { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 /// @deprecated: use IncrementalRotation functor with sane Jacobian inline Rot3 GTSAM_DEPRECATED incrementalRotation( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + const Vector3& measuredOmega, const Vector3& bias, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const { IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - Rot3 incrR = f(biasHat, D_incrR_integratedOmega); + Rot3 incrR = f(bias, D_incrR_integratedOmega); // Backwards compatible "weird" Jacobian, no longer used. if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; return incrR; } + + /// @deprecated: returned hard-to-understand Jacobian D_incrR_integratedOmega. + void GTSAM_DEPRECATED integrateMeasurement( + const Vector3& measuredOmega, const Vector3& bias, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F); + #endif /// @} diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 89b714534..8e7e4e9e5 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -27,18 +27,28 @@ using namespace gtsam; //****************************************************************************** -namespace simple_roll { -auto p = std::make_shared(); -PreintegratedRotation pim(p); +// Example where gyro measures small rotation about x-axis, with bias. +namespace biased_x_rotation { const double omega = 0.1; -const Vector3 measuredOmega(omega, 0, 0); -const Vector3 bias(0, 0, 0); +const Vector3 trueOmega(omega, 0, 0); +const Vector3 bias(1, 2, 3); +const Vector3 measuredOmega = trueOmega + bias; const double deltaT = 0.5; -} // namespace simple_roll +} // namespace biased_x_rotation + +// Create params where x and y axes are exchanged. +static std::shared_ptr paramsWithTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); + return p; +} //****************************************************************************** -TEST(PreintegratedRotation, IncrementalRotation) { - using namespace simple_roll; +TEST(PreintegratedRotation, integrateMeasurement) { + // Example where IMU is identical to body frame, then omega is roll + using namespace biased_x_rotation; + auto p = std::make_shared(); + PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; @@ -51,6 +61,32 @@ TEST(PreintegratedRotation, IncrementalRotation) { // Check the derivative: EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + // Check value of deltaRij() after integration. + Matrix3 F; + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); +} + +//****************************************************************************** +TEST(PreintegratedRotation, Deprecated) { + // Example where IMU is identical to body frame, then omega is roll + using namespace biased_x_rotation; + auto p = std::make_shared(); + PreintegratedRotation pim(p); + + // Check the value. + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + Rot3 expected = Rot3::Roll(omega * deltaT); + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); + // Ephemeral test for deprecated Jacobian: Matrix3 D_incrR_integratedOmega; (void)pim.incrementalRotation(measuredOmega, bias, deltaT, @@ -58,30 +94,32 @@ TEST(PreintegratedRotation, IncrementalRotation) { auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - EXPECT(assert_equal( - numericalDerivative22(g, measuredOmega, bias), - -deltaT * D_incrR_integratedOmega)); -} + const Matrix3 oldJacobian = + numericalDerivative22(g, measuredOmega, bias); + EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); -//****************************************************************************** -static std::shared_ptr paramsWithTransform() { - auto p = std::make_shared(); - p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); - return p; -} + // Check deprecated version. + Matrix3 D_incrR_integratedOmega2, F; + pim.integrateMeasurement(measuredOmega, bias, deltaT, + D_incrR_integratedOmega2, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); -namespace roll_in_rotated_frame { -auto p = paramsWithTransform(); -PreintegratedRotation pim(p); -const double omega = 0.1; -const Vector3 measuredOmega(omega, 0, 0); -const Vector3 bias(0, 0, 0); -const double deltaT = 0.5; -} // namespace roll_in_rotated_frame + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Check that deprecated Jacobian is correct. + EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); +} //****************************************************************************** TEST(PreintegratedRotation, IncrementalRotationWithTransform) { - using namespace roll_in_rotated_frame; + // Example where IMU is rotated, so measured omega indicates pitch. + using namespace biased_x_rotation; + auto p = paramsWithTransform(); + PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; @@ -93,6 +131,32 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) { // Check the derivative: EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + // Check value of deltaRij() after integration. + Matrix3 F; + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); +} + +//****************************************************************************** +TEST(PreintegratedRotation, DeprecatedWithTransform) { + // Example where IMU is rotated, so measured omega indicates pitch. + using namespace biased_x_rotation; + auto p = paramsWithTransform(); + PreintegratedRotation pim(p); + + // Check the value. + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + Rot3 expected = Rot3::Pitch(omega * deltaT); + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); + // Ephemeral test for deprecated Jacobian: Matrix3 D_incrR_integratedOmega; (void)pim.incrementalRotation(measuredOmega, bias, deltaT, @@ -100,9 +164,24 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) { auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - EXPECT(assert_equal( - numericalDerivative22(g, measuredOmega, bias), - -deltaT * D_incrR_integratedOmega)); + const Matrix3 oldJacobian = + numericalDerivative22(g, measuredOmega, bias); + EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); + + // Check deprecated version. + Matrix3 D_incrR_integratedOmega2, F; + pim.integrateMeasurement(measuredOmega, bias, deltaT, + D_incrR_integratedOmega2, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Check that deprecated Jacobian is correct. + EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); } //****************************************************************************** From 1ac286f97a6fb482c26d4a9c9d042c4fd065e74d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 10:10:01 -0700 Subject: [PATCH 08/16] Removed (passing) tests of deprecated --- .../tests/testPreintegratedRotation.cpp | 86 +------------------ 1 file changed, 2 insertions(+), 84 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 8e7e4e9e5..bc51bf9d3 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -44,7 +44,7 @@ static std::shared_ptr paramsWithTransform() { } //****************************************************************************** -TEST(PreintegratedRotation, integrateMeasurement) { +TEST(PreintegratedRotation, integrateGyroMeasurement) { // Example where IMU is identical to body frame, then omega is roll using namespace biased_x_rotation; auto p = std::make_shared(); @@ -74,48 +74,7 @@ TEST(PreintegratedRotation, integrateMeasurement) { } //****************************************************************************** -TEST(PreintegratedRotation, Deprecated) { - // Example where IMU is identical to body frame, then omega is roll - using namespace biased_x_rotation; - auto p = std::make_shared(); - PreintegratedRotation pim(p); - - // Check the value. - Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; - Rot3 expected = Rot3::Roll(omega * deltaT); - EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); - - // Ephemeral test for deprecated Jacobian: - Matrix3 D_incrR_integratedOmega; - (void)pim.incrementalRotation(measuredOmega, bias, deltaT, - D_incrR_integratedOmega); - auto g = [&](const Vector3& x, const Vector3& y) { - return pim.incrementalRotation(x, y, deltaT, {}); - }; - const Matrix3 oldJacobian = - numericalDerivative22(g, measuredOmega, bias); - EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); - - // Check deprecated version. - Matrix3 D_incrR_integratedOmega2, F; - pim.integrateMeasurement(measuredOmega, bias, deltaT, - D_incrR_integratedOmega2, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); - - // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); - - // Check that deprecated Jacobian is correct. - EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); - - // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); -} - -//****************************************************************************** -TEST(PreintegratedRotation, IncrementalRotationWithTransform) { +TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Example where IMU is rotated, so measured omega indicates pitch. using namespace biased_x_rotation; auto p = paramsWithTransform(); @@ -143,47 +102,6 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) { EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); } -//****************************************************************************** -TEST(PreintegratedRotation, DeprecatedWithTransform) { - // Example where IMU is rotated, so measured omega indicates pitch. - using namespace biased_x_rotation; - auto p = paramsWithTransform(); - PreintegratedRotation pim(p); - - // Check the value. - Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; - Rot3 expected = Rot3::Pitch(omega * deltaT); - EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); - - // Ephemeral test for deprecated Jacobian: - Matrix3 D_incrR_integratedOmega; - (void)pim.incrementalRotation(measuredOmega, bias, deltaT, - D_incrR_integratedOmega); - auto g = [&](const Vector3& x, const Vector3& y) { - return pim.incrementalRotation(x, y, deltaT, {}); - }; - const Matrix3 oldJacobian = - numericalDerivative22(g, measuredOmega, bias); - EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); - - // Check deprecated version. - Matrix3 D_incrR_integratedOmega2, F; - pim.integrateMeasurement(measuredOmega, bias, deltaT, - D_incrR_integratedOmega2, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); - - // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); - - // Check that deprecated Jacobian is correct. - EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); - - // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); -} - //****************************************************************************** int main() { TestResult tr; From 0a3fdcc3a4b43e04e09fc6535f5c5625982e9303 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 12:24:45 -0700 Subject: [PATCH 09/16] biascorrectedDeltaRij tests --- gtsam/navigation/PreintegratedRotation.cpp | 10 +-- gtsam/navigation/PreintegratedRotation.h | 69 ++++++++++--------- gtsam/navigation/tests/testAHRSFactor.cpp | 23 ------- .../tests/testPreintegratedRotation.cpp | 30 ++++++++ 4 files changed, 71 insertions(+), 61 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index bbc607800..b10d8f772 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -95,11 +95,11 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()( } void PreintegratedRotation::integrateGyroMeasurement( - const Vector3& measuredOmega, const Vector3& bias, double deltaT, + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> F) { Matrix3 H_bias; IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - const Rot3 incrR = f(bias, H_bias); + const Rot3 incrR = f(biasHat, H_bias); // Update deltaTij and rotation deltaTij_ += deltaT; @@ -112,16 +112,16 @@ void PreintegratedRotation::integrateGyroMeasurement( // deprecated! void PreintegratedRotation::integrateMeasurement( - const Vector3& measuredOmega, const Vector3& bias, double deltaT, + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { - integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + integrateGyroMeasurement(measuredOmega, biasHat, deltaT, F); // If asked, pass obsolete Jacobians as well if (optional_D_incrR_integratedOmega) { Matrix3 H_bias; IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - const Rot3 incrR = f(bias, H_bias); + const Rot3 incrR = f(biasHat, H_bias); *optional_D_incrR_integratedOmega << H_bias / -deltaT; } } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b87f4208b..a36674062 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -135,18 +135,10 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Access instance variables /// @{ - const std::shared_ptr& params() const { - return p_; - } - const double& deltaTij() const { - return deltaTij_; - } - const Rot3& deltaRij() const { - return deltaRij_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } + const std::shared_ptr& params() const { return p_; } + const double& deltaTij() const { return deltaTij_; } + const Rot3& deltaRij() const { return deltaRij_; } + const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; } /// @} /// @name Testable @@ -158,6 +150,35 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Main functionality /// @{ + /** + * @brief Calculate an incremental rotation given the gyro measurement and a + * time interval, and update both deltaTij_ and deltaRij_. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param bias The biasHat estimate + * @param deltaT The time interval + * @param F Jacobian of internal compose, used in AhrsFactor. + */ + void integrateGyroMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> F = {}); + + /** + * @brief Return a bias corrected version of the integrated rotation. + * @param biasOmegaIncr An increment with respect to biasHat used above. + * @param H Jacobian of the correction w.r.t. the bias increment. + * @note The *key* functionality of this class used in optimizing the bias. + */ + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H = {}) const; + + /// Integrate coriolis correction in body frame rot_i + Vector3 integrateCoriolis(const Rot3& rot_i) const; + + /// @} + + /// @name Internal, exposed for testing only + /// @{ + /** * @brief Function object for incremental rotation. * @param measuredOmega The measured angular velocity (as given by the sensor) @@ -179,25 +200,6 @@ class GTSAM_EXPORT PreintegratedRotation { OptionalJacobian<3, 3> H_bias = {}) const; }; - /** - * @brief Calculate an incremental rotation given the gyro measurement and a - * time interval, and update both deltaTij_ and deltaRij_. - * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param bias The bias estimate - * @param deltaT The time interval - * @param F Jacobian of internal compose, used in AhrsFactor. - */ - void integrateGyroMeasurement(const Vector3& measuredOmega, - const Vector3& bias, double deltaT, - OptionalJacobian<3, 3> F = {}); - - /// Return a bias corrected version of the integrated rotation, with optional Jacobian - Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, - OptionalJacobian<3, 3> H = {}) const; - - /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i) const; - /// @} /// @name Deprecated API @@ -215,9 +217,10 @@ class GTSAM_EXPORT PreintegratedRotation { return incrR; } - /// @deprecated: returned hard-to-understand Jacobian D_incrR_integratedOmega. + /// @deprecated: use integrateGyroMeasurement from now on + /// @note this returned hard-to-understand Jacobian D_incrR_integratedOmega. void GTSAM_DEPRECATED integrateMeasurement( - const Vector3& measuredOmega, const Vector3& bias, double deltaT, + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F); #endif diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 9d512d595..95674ce78 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -301,29 +301,6 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) { // 1e-3 needs to be added only when using quaternions for rotations } -//****************************************************************************** -// TEST(AHRSFactor, PimWithBodyDisplacement) { -// Vector3 bias(0, 0, 0.3); -// Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); -// double deltaT = 1.0; - -// auto p = std::make_shared(); -// p->gyroscopeCovariance = kMeasuredOmegaCovariance; -// p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0)); -// PreintegratedAhrsMeasurements pim(p, Vector3::Zero()); - -// pim.integrateMeasurement(measuredOmega, deltaT); - -// Vector3 biasOmegaIncr(0.01, 0.0, 0.0); -// Matrix3 actualH; -// pim.biascorrectedDeltaRij(biasOmegaIncr, actualH); - -// // Numerical derivative using a lambda function: -// auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); }; -// Matrix3 expectedH = numericalDerivative11(f, bias); -// EXPECT(assert_equal(expectedH, actualH)); -// } - //****************************************************************************** TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 bias(0, 0, 0.3); diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index bc51bf9d3..b6486d030 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -71,6 +71,22 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { // Make sure delRdelBiasOmega is H_bias after integration. EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + + // Check if we make a correction to the bias, the value and Jacobian are + // correct. Note that the bias is subtracted from the measurement, and the + // integration time is taken into account, so we expect -deltaT*delta change. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); + EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)); + + // TODO(frank): again the derivative is not the *sane* one! + // auto g = [&](const Vector3& increment) { + // return pim.biascorrectedDeltaRij(increment, {}); + // }; + // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); } //****************************************************************************** @@ -100,6 +116,20 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Make sure delRdelBiasOmega is H_bias after integration. EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + + // Check the bias correction in same way, but will now yield pitch change. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)); + EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)); + + // TODO(frank): again the derivative is not the *sane* one! + // auto g = [&](const Vector3& increment) { + // return pim.biascorrectedDeltaRij(increment, {}); + // }; + // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); } //****************************************************************************** From ca50c82badf579f9b4d0dbc28f3e125b9a08f6fb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 13:51:24 -0700 Subject: [PATCH 10/16] CompareWithPreintegratedRotation --- .../tests/testManifoldPreintegration.cpp | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index f04663943..4016240cf 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -109,6 +109,59 @@ TEST(ManifoldPreintegration, computeError) { EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); } +/* ************************************************************************* */ +TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { + // Create a PreintegratedRotation object + auto p = std::make_shared(); + PreintegratedRotation pim(p); + + // Integrate a single measurement + const double omega = 0.1; + const Vector3 trueOmega(omega, 0, 0); + const Vector3 bias(1, 2, 3); + const Vector3 measuredOmega = trueOmega + bias; + const double deltaT = 0.5; + + // Check the accumulated rotation. + Rot3 expected = Rot3::Roll(omega * deltaT); + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Now do the same for a ManifoldPreintegration object + imuBias::ConstantBias biasHat {Z_3x1, bias}; + ManifoldPreintegration manifoldPim(testing::Params(), biasHat); + manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT); + EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9)); + + // Check their internal Jacobians are the same: + EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega())); + + // Check PreintegratedRotation::biascorrectedDeltaRij. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); + const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT); + EXPECT(assert_equal(expected2, corrected, 1e-9)); + + // Check ManifoldPreintegration::biasCorrectedDelta. + imuBias::ConstantBias bias_i {Z_3x1, bias + biasOmegaIncr}; + Matrix96 H2; + Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2); + Vector9 expected3; + expected3 << Rot3::Logmap(expected2), Z_3x1, Z_3x1; + EXPECT(assert_equal(expected3, biasCorrected, 1e-9)); + + // Check that this one is sane: + auto g = [&](const Vector3& increment) { + return manifoldPim.biasCorrectedDelta({Z_3x1, bias + increment}, {}); + }; + EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), + H2.rightCols<3>(), + 1e-4)); // NOTE(frank): reduced tolerance +} + /* ************************************************************************* */ int main() { TestResult tr; From e47b4e5b2cfa02457840d314325652e2120dd590 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 14:34:04 -0700 Subject: [PATCH 11/16] 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); From fccf3cedd9d8cdcdf34dfb4af6e3f061a2ecda99 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 15:10:12 -0700 Subject: [PATCH 12/16] Make sure compiles without deprecated --- gtsam/navigation/PreintegratedRotation.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index b10d8f772..374a09359 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -110,7 +110,7 @@ void PreintegratedRotation::integrateGyroMeasurement( delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias; } -// deprecated! +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 void PreintegratedRotation::integrateMeasurement( const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, @@ -125,6 +125,7 @@ void PreintegratedRotation::integrateMeasurement( *optional_D_incrR_integratedOmega << H_bias / -deltaT; } } +#endif Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, OptionalJacobian<3, 3> H) const { From 1bb26f86afd26b6a786bf2748c1f939bde4f6173 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 21:34:37 -0700 Subject: [PATCH 13/16] Attempt to resolve Windows linking --- gtsam/navigation/PreintegratedRotation.cpp | 6 ++- gtsam/navigation/PreintegratedRotation.h | 50 +++++++++---------- .../tests/testPreintegratedRotation.cpp | 6 +-- 3 files changed, 30 insertions(+), 32 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 374a09359..ebf98bc15 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -68,7 +68,8 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other, && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } -Rot3 PreintegratedRotation::IncrementalRotation::operator()( +namespace internal { +Rot3 IncrementalRotation::operator()( const Vector3& bias, OptionalJacobian<3, 3> H_bias) const { // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - bias; @@ -93,12 +94,13 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()( } return incrR; } +} // namespace internal void PreintegratedRotation::integrateGyroMeasurement( const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> F) { Matrix3 H_bias; - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; const Rot3 incrR = f(biasHat, H_bias); // Update deltaTij and rotation diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index a36674062..3ff3eb3f1 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -176,32 +176,6 @@ class GTSAM_EXPORT PreintegratedRotation { /// @} - /// @name Internal, exposed for testing only - /// @{ - - /** - * @brief Function object for incremental rotation. - * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param deltaT The time interval over which the rotation is integrated. - * @param body_P_sensor Optional transform between body and IMU. - */ - struct IncrementalRotation { - const Vector3& measuredOmega; - const double deltaT; - const std::optional& body_P_sensor; - - /** - * @brief Integrate angular velocity, but corrected by bias. - * @param bias The bias estimate - * @param H_bias Jacobian of the rotation w.r.t. bias. - * @return The incremental rotation - */ - Rot3 operator()(const Vector3& bias, - OptionalJacobian<3, 3> H_bias = {}) const; - }; - - /// @} - /// @name Deprecated API /// @{ @@ -250,4 +224,28 @@ class GTSAM_EXPORT PreintegratedRotation { template <> struct traits : public Testable {}; +namespace internal { +/** + * @brief Function object for incremental rotation. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. + */ +struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& body_P_sensor; + + /** + * @brief Integrate angular velocity, but corrected by bias. + * @param bias The bias estimate + * @param H_bias Jacobian of the rotation w.r.t. bias. + * @return The incremental rotation + */ + Rot3 operator()(const Vector3& bias, + OptionalJacobian<3, 3> H_bias = {}) const; +}; + +} // namespace internal + } /// namespace gtsam diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index b6486d030..1e27ca1e4 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -52,8 +52,7 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { // Check the value. Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; + internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; const Rot3 incrR = f(bias, H_bias); Rot3 expected = Rot3::Roll(omega * deltaT); EXPECT(assert_equal(expected, incrR, 1e-9)); @@ -98,8 +97,7 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Check the value. Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; + internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; Rot3 expected = Rot3::Pitch(omega * deltaT); EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); From ab73b956e0209f31d8df92d1e9eff645a8176a37 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 21:47:15 -0700 Subject: [PATCH 14/16] Compile deprecated sections --- gtsam/navigation/PreintegratedRotation.cpp | 2 +- gtsam/navigation/PreintegratedRotation.h | 50 +++++++++++----------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index ebf98bc15..04e201a34 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -122,7 +122,7 @@ void PreintegratedRotation::integrateMeasurement( // If asked, pass obsolete Jacobians as well if (optional_D_incrR_integratedOmega) { Matrix3 H_bias; - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; const Rot3 incrR = f(biasHat, H_bias); *optional_D_incrR_integratedOmega << H_bias / -deltaT; } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 3ff3eb3f1..146a47a19 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -27,6 +27,30 @@ namespace gtsam { +namespace internal { +/** + * @brief Function object for incremental rotation. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. + */ +struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& body_P_sensor; + + /** + * @brief Integrate angular velocity, but corrected by bias. + * @param bias The bias estimate + * @param H_bias Jacobian of the rotation w.r.t. bias. + * @return The incremental rotation + */ + Rot3 operator()(const Vector3& bias, + OptionalJacobian<3, 3> H_bias = {}) const; +}; + +} // namespace internal + /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor struct GTSAM_EXPORT PreintegratedRotationParams { @@ -184,7 +208,7 @@ class GTSAM_EXPORT PreintegratedRotation { inline Rot3 GTSAM_DEPRECATED incrementalRotation( const Vector3& measuredOmega, const Vector3& bias, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const { - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; Rot3 incrR = f(bias, D_incrR_integratedOmega); // Backwards compatible "weird" Jacobian, no longer used. if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; @@ -224,28 +248,4 @@ class GTSAM_EXPORT PreintegratedRotation { template <> struct traits : public Testable {}; -namespace internal { -/** - * @brief Function object for incremental rotation. - * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param deltaT The time interval over which the rotation is integrated. - * @param body_P_sensor Optional transform between body and IMU. - */ -struct IncrementalRotation { - const Vector3& measuredOmega; - const double deltaT; - const std::optional& body_P_sensor; - - /** - * @brief Integrate angular velocity, but corrected by bias. - * @param bias The bias estimate - * @param H_bias Jacobian of the rotation w.r.t. bias. - * @return The incremental rotation - */ - Rot3 operator()(const Vector3& bias, - OptionalJacobian<3, 3> H_bias = {}) const; -}; - -} // namespace internal - } /// namespace gtsam From f2bc3640cf671c1a09995ff709ae09e49c4653f5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 23:16:38 -0700 Subject: [PATCH 15/16] Add GTSAM_EXPORT --- gtsam/navigation/PreintegratedRotation.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 146a47a19..5c36da05b 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -24,6 +24,7 @@ #include #include #include +#include "gtsam/dllexport.h" namespace gtsam { @@ -34,7 +35,7 @@ namespace internal { * @param deltaT The time interval over which the rotation is integrated. * @param body_P_sensor Optional transform between body and IMU. */ -struct IncrementalRotation { +struct GTSAM_EXPORT IncrementalRotation { const Vector3& measuredOmega; const double deltaT; const std::optional& body_P_sensor; From a90dbc7fc5031e519dd27aab4ebaa1cd6f4177e2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 25 Jul 2024 09:58:00 -0500 Subject: [PATCH 16/16] Fix comments --- gtsam/navigation/PreintegratedRotation.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 5c36da05b..49b1aa4c1 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -181,7 +181,7 @@ class GTSAM_EXPORT PreintegratedRotation { * @param measuredOmega The measured angular velocity (as given by the sensor) * @param bias The biasHat estimate * @param deltaT The time interval - * @param F Jacobian of internal compose, used in AhrsFactor. + * @param F optional Jacobian of internal compose, used in AhrsFactor. */ void integrateGyroMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, @@ -190,7 +190,7 @@ class GTSAM_EXPORT PreintegratedRotation { /** * @brief Return a bias corrected version of the integrated rotation. * @param biasOmegaIncr An increment with respect to biasHat used above. - * @param H Jacobian of the correction w.r.t. the bias increment. + * @param H optional Jacobian of the correction w.r.t. the bias increment. * @note The *key* functionality of this class used in optimizing the bias. */ Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,