From 344a36c568c7fdd22d256dd57afa38ccd5701019 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Jun 2024 16:11:07 -0700 Subject: [PATCH 01/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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 55505377092d9479402b94fd398a4df538d19f0f Mon Sep 17 00:00:00 2001 From: Akash Patel <17132214+acxz@users.noreply.github.com> Date: Tue, 2 Jul 2024 20:31:09 -0500 Subject: [PATCH 16/38] correct Linux CI platforms follow up of https://github.com/borglab/gtsam/pull/1276 and https://github.com/borglab/gtsam/commit/42182c85ffeeeffeda8fdc4cdd7358d3cbaafe4d tidy up support matrix markdown table format --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 40b84dff8..e02aa953c 100644 --- a/README.md +++ b/README.md @@ -15,11 +15,11 @@ matrices. The current support matrix is: -| Platform | Compiler | Build Status | -|:------------:|:---------:|:-------------:| -| Ubuntu 18.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) | -| macOS | clang | ![macOS CI](https://github.com/borglab/gtsam/workflows/macOS%20CI/badge.svg) | -| Windows | MSVC | ![Windows CI](https://github.com/borglab/gtsam/workflows/Windows%20CI/badge.svg) | +| Platform | Compiler | Build Status | +|:------------------:|:---------:|:--------------------------------------------------------------------------------:| +| Ubuntu 20.04/22.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) | +| macOS | clang | ![macOS CI](https://github.com/borglab/gtsam/workflows/macOS%20CI/badge.svg) | +| Windows | MSVC | ![Windows CI](https://github.com/borglab/gtsam/workflows/Windows%20CI/badge.svg) | On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](#wrappers). From 24320deaaf7972faf33d481fac5b3aaaf4c82676 Mon Sep 17 00:00:00 2001 From: "Patel, Akash M" Date: Mon, 8 Jul 2024 22:35:49 -0500 Subject: [PATCH 17/38] modernize containers use compose spec, use single image repo with tags for various configs, create a docker hub push script, clean up/slim down container files --- containers/Containerfile | 45 +++++++ containers/README.md | 127 ++++++++++++++++++ containers/compose.yaml | 14 ++ containers/gtsam-vnc/Containerfile | 20 +++ .../gtsam-vnc}/bootstrap.sh | 0 containers/gtsam-vnc/compose.yaml | 10 ++ containers/gtsam-vnc/hub_push.sh | 19 +++ containers/hub_push.sh | 32 +++++ docker/README.md | 63 --------- docker/ubuntu-boost-tbb/Dockerfile | 19 --- docker/ubuntu-boost-tbb/build.sh | 3 - docker/ubuntu-gtsam-python-vnc/Dockerfile | 20 --- docker/ubuntu-gtsam-python-vnc/build.sh | 4 - docker/ubuntu-gtsam-python-vnc/vnc.sh | 5 - docker/ubuntu-gtsam-python/Dockerfile | 30 ----- docker/ubuntu-gtsam-python/build.sh | 3 - docker/ubuntu-gtsam/Dockerfile | 35 ----- docker/ubuntu-gtsam/build.sh | 3 - 18 files changed, 267 insertions(+), 185 deletions(-) create mode 100644 containers/Containerfile create mode 100644 containers/README.md create mode 100644 containers/compose.yaml create mode 100644 containers/gtsam-vnc/Containerfile rename {docker/ubuntu-gtsam-python-vnc => containers/gtsam-vnc}/bootstrap.sh (100%) mode change 100755 => 100644 create mode 100644 containers/gtsam-vnc/compose.yaml create mode 100644 containers/gtsam-vnc/hub_push.sh create mode 100644 containers/hub_push.sh delete mode 100644 docker/README.md delete mode 100644 docker/ubuntu-boost-tbb/Dockerfile delete mode 100755 docker/ubuntu-boost-tbb/build.sh delete mode 100644 docker/ubuntu-gtsam-python-vnc/Dockerfile delete mode 100755 docker/ubuntu-gtsam-python-vnc/build.sh delete mode 100755 docker/ubuntu-gtsam-python-vnc/vnc.sh delete mode 100644 docker/ubuntu-gtsam-python/Dockerfile delete mode 100755 docker/ubuntu-gtsam-python/build.sh delete mode 100644 docker/ubuntu-gtsam/Dockerfile delete mode 100755 docker/ubuntu-gtsam/build.sh diff --git a/containers/Containerfile b/containers/Containerfile new file mode 100644 index 000000000..6dd5226cd --- /dev/null +++ b/containers/Containerfile @@ -0,0 +1,45 @@ +# base image off ubuntu image +ARG UBUNTU_TAG=22.04 +FROM docker.io/ubuntu:${UBUNTU_TAG} + +RUN apt-get update && apt-get install -y --no-install-recommends \ +# dependencies + libboost-all-dev \ +# optional dependencies + libtbb-dev \ + python3-dev \ + python3-pip \ + python3-pyparsing \ + python3-numpy \ +# build dependencies + build-essential \ + cmake \ +# download dependencies + git \ + ca-certificates && \ + rm -rf /var/lib/apt/lists/* + +# build flags +ARG GTSAM_GIT_TAG=4.2.0 +ARG GTSAM_WITH_TBB=ON +ARG GTSAM_BUILD_PYTHON=ON +ARG CORES=4 + +# build and install gtsam +RUN mkdir -p /src/github/borglab && cd /src/github/borglab && \ + git clone https://github.com/borglab/gtsam --depth 1 --branch ${GTSAM_GIT_TAG} && \ + cd gtsam && \ + mkdir build && \ + cd build && \ + cmake \ + -DCMAKE_BUILD_TYPE=Release \ + -DGTSAM_BUILD_TESTS=OFF \ + -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB} \ + -DGTSAM_BUILD_PYTHON=${GTSAM_BUILD_PYTHON} \ + .. && \ + make -j${CORES} install && \ + if [ "${GTSAM_BUILD_PYTHON}" = "ON" ] ; then \ + make python-install; \ + fi + +CMD ["/bin/bash"] diff --git a/containers/README.md b/containers/README.md new file mode 100644 index 000000000..2d4a7c27a --- /dev/null +++ b/containers/README.md @@ -0,0 +1,127 @@ +# GTSAM Containers + +- container files to build images +- script to push images to a registry +- instructions to pull images and run containers + +## Dependencies + +- a container engine such as [`Docker Engine`](https://docs.docker.com/engine/install/) + +## Pull from Docker Hub + +Various GTSAM image configurations are available at [`docker.io/borglab/gtsam`](https://hub.docker.com/r/borglab/gtsam). Determine which [tag](https://hub.docker.com/r/borglab/gtsam/tags) you want and pull the image. + +Example for pulling an image with GTSAM compiled with TBB and Python support on top of a base Ubuntu 22.04 image. + +```bash +docker pull docker.io/borglab/gtsam:4.2.0-tbb-ON-python-ON_22.04 +``` + +[`docker.io/borglab/gtsam-vnc`](https://hub.docker.com/r/borglab/gtsam-vnc) is also provided as an image with GTSAM that will run a VNC server to connect to. + +## Using the images + +### Just GTSAM + +To start the image, execute + +```bash +docker run -it borglab/gtsam:4.2.0-tbb-ON-python-OFF_22.04 +``` + +after you will find yourself in a bash shell. + +### GTSAM with Python wrapper + +To use GTSAM via the python wrapper, similarly execute + +```bash +docker run -it borglab/gtsam:4.2.0-tbb-ON-python-ON_22.04 +``` + +and then launch `python3`: + +```bash +python3 +>>> import gtsam +>>> gtsam.Pose2(1,2,3) +(1, 2, 3) +``` + +### GTSAM with Python wrapper and VNC + +First, start the image, which will run a VNC server on port 5900: + +```bash +docker run -p 5900:5900 borglab/gtsam-vnc:4.2.0-tbb-ON-python-ON_22.04 +``` + +Then open a remote VNC X client, for example: + +#### Linux + +```bash +sudo apt-get install tigervnc-viewer +xtigervncviewer :5900 +``` + +#### Mac + +The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server. + +## Build images locally + +### Build Dependencies + +- a [Compose Spec](https://compose-spec.io/) implementation such as [docker-compose](https://docs.docker.com/compose/install/) + +### `gtsam` image + +#### `.env` file + +- `GTSAM_GIT_TAG`: [git tag from the gtsam repo](https://github.com/borglab/gtsam/tags) +- `UBUNTU_TAG`: image tag provided by [ubuntu](https://hub.docker.com/_/ubuntu/tags) to base the image off of +- `GTSAM_WITH_TBB`: to build GTSAM with TBB, set to `ON` +- `GTSAM_BUILD_PYTHON`: to build python bindings, set to `ON` +- `CORES`: number of cores to compile with + +#### Build `gtsam` image + +```bash +docker compose build +``` + +### `gtsam-vnc` image + +#### `gtsam-vnc/.env` file + +- `GTSAM_TAG`: image tag provided by [gtsam](https://hub.docker.com/r/borglab/gtsam/tags) + +#### Build `gtsam-vnc` image + +```bash +docker compose --file gtsam-vnc/compose.yaml build +``` + +## Push to Docker Hub + +Make sure you are logged in via: `docker login docker.io`. + +### `gtsam` images + +Specify the variables described in the `.env` file in the `hub_push.sh` script. +To push images to Docker Hub, run as follows: + +```bash +./hub_push.sh +``` + +### `gtsam-vnc` images + +Specify the variables described in the `gtsam-vnc/.env` file in the `gtsam-vnc/hub_push.sh` script. +To push images to Docker Hub, run as follows: + +```bash +./gtsam-vnc/hub_push.sh +``` diff --git a/containers/compose.yaml b/containers/compose.yaml new file mode 100644 index 000000000..01f9a6c07 --- /dev/null +++ b/containers/compose.yaml @@ -0,0 +1,14 @@ +services: + gtsam: + build: + args: + UBUNTU_TAG: ${UBUNTU_TAG} + GTSAM_GIT_TAG: ${GTSAM_GIT_TAG} + GTSAM_WITH_TBB: ${GTSAM_WITH_TBB} + GTSAM_BUILD_PYTHON: ${GTSAM_BUILD_PYTHON} + CORES: ${CORES} + context: . + dockerfile: Containerfile + env_file: + - .env + image: gtsam:${GTSAM_GIT_TAG}-tbb-${GTSAM_WITH_TBB}-python-${GTSAM_BUILD_PYTHON}_${UBUNTU_TAG} diff --git a/containers/gtsam-vnc/Containerfile b/containers/gtsam-vnc/Containerfile new file mode 100644 index 000000000..8814d0e48 --- /dev/null +++ b/containers/gtsam-vnc/Containerfile @@ -0,0 +1,20 @@ +# This image connects to the host X-server via VNC to provide a Graphical User Interface for interaction. + +# base image off gtsam image +ARG GTSAM_TAG=4.2.0-tbb-ON-python-ON_22.04 +FROM docker.io/borglab/gtsam:${GTSAM_TAG} + +RUN apt-get update && apt-get install -y --no-install-recommends \ +# Things needed to get a python GUI + python3-tk \ + python3-matplotlib \ +# Install a VNC X-server, Frame buffer, and windows manager + x11vnc \ + xvfb \ + fluxbox \ +# Finally, install wmctrl needed for bootstrap script + wmctrl \ + rm -rf /var/lib/apt/lists/* + +COPY bootstrap.sh / +CMD ["/bootstrap.sh"] diff --git a/docker/ubuntu-gtsam-python-vnc/bootstrap.sh b/containers/gtsam-vnc/bootstrap.sh old mode 100755 new mode 100644 similarity index 100% rename from docker/ubuntu-gtsam-python-vnc/bootstrap.sh rename to containers/gtsam-vnc/bootstrap.sh diff --git a/containers/gtsam-vnc/compose.yaml b/containers/gtsam-vnc/compose.yaml new file mode 100644 index 000000000..c27bdef85 --- /dev/null +++ b/containers/gtsam-vnc/compose.yaml @@ -0,0 +1,10 @@ +services: + gtsam_vnc: + build: + args: + GTSAM_TAG: ${GTSAM_TAG} + context: . + dockerfile: Containerfile + env_file: + - .env + image: gtsam-vnc:${GTSAM_TAG} diff --git a/containers/gtsam-vnc/hub_push.sh b/containers/gtsam-vnc/hub_push.sh new file mode 100644 index 000000000..21326d741 --- /dev/null +++ b/containers/gtsam-vnc/hub_push.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash + +# A script to push images to Docker Hub + +declare -a gtsam_tags=("4.2.0-tbb-ON-python-ON_22.04") + +for gtsam_tag in "${gtsam_tags[@]}"; do + + touch gtsam-vnc/.env + echo "GTSAM_TAG=${gtsam_tag}" > gtsam-vnc/.env + + docker compose --file gtsam-vnc/compose.yaml build + + docker tag gtsam-vnc:"${gtsam_tag}" \ + docker.io/borglab/gtsam-vnc:"${gtsam_tag}" + + docker push docker.io/borglab/gtsam-vnc:"${gtsam_tag}" + +done diff --git a/containers/hub_push.sh b/containers/hub_push.sh new file mode 100644 index 000000000..f31d4a3f4 --- /dev/null +++ b/containers/hub_push.sh @@ -0,0 +1,32 @@ +#!/usr/bin/env bash + +# A script to push images to Docker Hub + +declare -a ubuntu_tags=("22.04") +declare -a gtsam_git_tags=("4.2.0") +declare -a gtsam_with_tbb_options=("OFF" "ON") +declare -a gtsam_build_python_options=("OFF" "ON") + +for ubuntu_tag in "${ubuntu_tags[@]}"; do +for gtsam_git_tag in "${gtsam_git_tags[@]}"; do +for gtsam_with_tbb in "${gtsam_with_tbb_options[@]}"; do +for gtsam_build_python in "${gtsam_build_python_options[@]}"; do + + touch .env + echo "UBUNTU_TAG=${ubuntu_tag}" > .env + echo "GTSAM_GIT_TAG=${gtsam_git_tag}" >> .env + echo "GTSAM_WITH_TBB=${gtsam_with_tbb}" >> .env + echo "GTSAM_BUILD_PYTHON=${gtsam_build_python}" >> .env + echo "CORES=4" >> .env + + docker compose build + + docker tag gtsam:"${gtsam_git_tag}-tbb-${gtsam_with_tbb}-python-${gtsam_build_python}_${ubuntu_tag}" \ + docker.io/borglab/gtsam:"${gtsam_git_tag}-tbb-${gtsam_with_tbb}-python-${gtsam_build_python}_${ubuntu_tag}" + + docker push docker.io/borglab/gtsam:"${gtsam_git_tag}-tbb-${gtsam_with_tbb}-python-${gtsam_build_python}_${ubuntu_tag}" + +done +done +done +done diff --git a/docker/README.md b/docker/README.md deleted file mode 100644 index 37c47a27f..000000000 --- a/docker/README.md +++ /dev/null @@ -1,63 +0,0 @@ -# Instructions - -# Images on Docker Hub - -There are 4 images available on https://hub.docker.com/orgs/borglab/repositories: - -- `borglab/ubuntu-boost-tbb`: 18.06 Linux (nicknamed `bionic`) base image, with Boost and TBB installed. -- `borglab/ubuntu-gtsam`: GTSAM Release version installed in `/usr/local`. -- `borglab/ubuntu-gtsam-python`: installed GTSAM with python wrapper. -- `borglab/ubuntu-gtsam-python-vnc`: image with GTSAM+python wrapper that will run a VNC server to connect to. - -# Using the images - -## Just GTSAM - -To start the Docker image, execute -```bash -docker run -it borglab/ubuntu-gtsam:bionic -``` -after you will find yourself in a bash shell, in the directory `/usr/src/gtsam/build`. -## GTSAM with Python wrapper - -To use GTSAM via the python wrapper, similarly execute -```bash -docker run -it borglab/ubuntu-gtsam-python:bionic -``` -and then launch `python3`: -```bash -python3 ->>> import gtsam ->>> gtsam.Pose2(1,2,3) -(1, 2, 3) -``` - -## GTSAM with Python wrapper and VNC - -First, start the docker image, which will run a VNC server on port 5900: -```bash -docker run -p 5900:5900 borglab/ubuntu-gtsam-python-vnc:bionic -``` - -Then open a remote VNC X client, for example: - -### Linux -```bash -sudo apt-get install tigervnc-viewer -xtigervncviewer :5900 -``` -### Mac -The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server. - -# Re-building the images locally - -To build all docker images, in order: - -```bash -(cd ubuntu-boost-tbb && ./build.sh) -(cd ubuntu-gtsam && ./build.sh) -(cd ubuntu-gtsam-python && ./build.sh) -(cd ubuntu-gtsam-python-vnc && ./build.sh) -``` - -Note: building GTSAM can take a lot of memory because of the heavy templating. It is advisable to give Docker enough resources, e.g., 8GB, to avoid OOM errors while compiling. \ No newline at end of file diff --git a/docker/ubuntu-boost-tbb/Dockerfile b/docker/ubuntu-boost-tbb/Dockerfile deleted file mode 100644 index 9f6eea3b8..000000000 --- a/docker/ubuntu-boost-tbb/Dockerfile +++ /dev/null @@ -1,19 +0,0 @@ -# Basic Ubuntu 18.04 image with Boost and TBB installed. To be used for building further downstream packages. - -# Get the base Ubuntu image from Docker Hub -FROM ubuntu:bionic - -# Disable GUI prompts -ENV DEBIAN_FRONTEND noninteractive - -# Update apps on the base image -RUN apt-get -y update && apt-get -y install - -# Install C++ -RUN apt-get -y install build-essential apt-utils - -# Install boost and cmake -RUN apt-get -y install libboost-all-dev cmake - -# Install TBB -RUN apt-get -y install libtbb-dev diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh deleted file mode 100755 index 35b349c6a..000000000 --- a/docker/ubuntu-boost-tbb/build.sh +++ /dev/null @@ -1,3 +0,0 @@ -# Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/Dockerfile b/docker/ubuntu-gtsam-python-vnc/Dockerfile deleted file mode 100644 index 8039698c3..000000000 --- a/docker/ubuntu-gtsam-python-vnc/Dockerfile +++ /dev/null @@ -1,20 +0,0 @@ -# This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction. - -# Get the base Ubuntu/GTSAM image from Docker Hub -FROM borglab/ubuntu-gtsam-python:bionic - -# Things needed to get a python GUI -ENV DEBIAN_FRONTEND noninteractive -RUN apt install -y python-tk -RUN python3 -m pip install matplotlib - -# Install a VNC X-server, Frame buffer, and windows manager -RUN apt install -y x11vnc xvfb fluxbox - -# Finally, install wmctrl needed for bootstrap script -RUN apt install -y wmctrl - -# Copy bootstrap script and make sure it runs -COPY bootstrap.sh / - -CMD '/bootstrap.sh' diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh deleted file mode 100755 index a0bbb6a96..000000000 --- a/docker/ubuntu-gtsam-python-vnc/build.sh +++ /dev/null @@ -1,4 +0,0 @@ -# Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -# Needs to be run in docker/ubuntu-gtsam-python-vnc directory -docker build -t borglab/ubuntu-gtsam-python-vnc:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/vnc.sh b/docker/ubuntu-gtsam-python-vnc/vnc.sh deleted file mode 100755 index b749093af..000000000 --- a/docker/ubuntu-gtsam-python-vnc/vnc.sh +++ /dev/null @@ -1,5 +0,0 @@ -# After running this script, connect VNC client to 0.0.0.0:5900 -docker run -it \ - --workdir="/usr/src/gtsam" \ - -p 5900:5900 \ - borglab/ubuntu-gtsam-python-vnc:bionic \ No newline at end of file diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile deleted file mode 100644 index 4a7c4b37f..000000000 --- a/docker/ubuntu-gtsam-python/Dockerfile +++ /dev/null @@ -1,30 +0,0 @@ -# GTSAM Ubuntu image with Python wrapper support. - -# Get the base Ubuntu/GTSAM image from Docker Hub -FROM borglab/ubuntu-gtsam:bionic - -# Install pip -RUN apt-get install -y python3-pip python3-dev - -# Run cmake again, now with python toolbox on -WORKDIR /usr/src/gtsam/build -RUN cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DGTSAM_WITH_EIGEN_MKL=OFF \ - -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ - -DGTSAM_BUILD_TIMING_ALWAYS=OFF \ - -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_BUILD_PYTHON=ON \ - -DGTSAM_PYTHON_VERSION=3\ - .. - -# Build again, as ubuntu-gtsam image cleaned -RUN make -j4 install -RUN make python-install -RUN make clean - -# Needed to run python wrapper: -RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc - -# Run bash -CMD ["bash"] diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh deleted file mode 100755 index 68827074d..000000000 --- a/docker/ubuntu-gtsam-python/build.sh +++ /dev/null @@ -1,3 +0,0 @@ -# Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic . diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile deleted file mode 100644 index ce6927fe8..000000000 --- a/docker/ubuntu-gtsam/Dockerfile +++ /dev/null @@ -1,35 +0,0 @@ -# Ubuntu image with GTSAM installed. Configured with Boost and TBB support. - -# Get the base Ubuntu image from Docker Hub -FROM borglab/ubuntu-boost-tbb:bionic - -# Install git -RUN apt-get update && \ - apt-get install -y git - -# Install compiler -RUN apt-get install -y build-essential - -# Clone GTSAM (develop branch) -WORKDIR /usr/src/ -RUN git clone --single-branch --branch develop https://github.com/borglab/gtsam.git - -# Change to build directory. Will be created automatically. -WORKDIR /usr/src/gtsam/build -# Run cmake -RUN cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DGTSAM_WITH_EIGEN_MKL=OFF \ - -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ - -DGTSAM_BUILD_TIMING_ALWAYS=OFF \ - -DGTSAM_BUILD_TESTS=OFF \ - .. - -# Build -RUN make -j4 install && make clean - -# Needed to link with GTSAM -RUN echo 'export LD_LIBRARY_PATH=/usr/local/lib:LD_LIBRARY_PATH' >> /root/.bashrc - -# Run bash -CMD ["bash"] diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh deleted file mode 100755 index 790ee1575..000000000 --- a/docker/ubuntu-gtsam/build.sh +++ /dev/null @@ -1,3 +0,0 @@ -# Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t borglab/ubuntu-gtsam:bionic . From b7da0f483821a40feeb4ef1eef521726366009fa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Jul 2024 18:40:45 -0400 Subject: [PATCH 18/38] add DiscreteConditional constructor using table --- gtsam/discrete/discrete.i | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index a1731f8e5..0deeb8033 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -104,6 +104,9 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor { DiscreteConditional(const gtsam::DecisionTreeFactor& joint, const gtsam::DecisionTreeFactor& marginal, const gtsam::Ordering& orderedKeys); + DiscreteConditional(const gtsam::DiscreteKey& key, + const gtsam::DiscreteKeys& parents, + const std::vector& table); // Standard interface double logNormalizationConstant() const; From 0a7db4129098004e2263561250ba00f6380f7de2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Jul 2024 18:54:25 -0400 Subject: [PATCH 19/38] print method for DiscreteKeys --- gtsam/discrete/discrete.i | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 0deeb8033..5eacb3634 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -14,6 +14,9 @@ class DiscreteKeys { bool empty() const; gtsam::DiscreteKey at(size_t n) const; void push_back(const gtsam::DiscreteKey& point_pair); + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; }; // DiscreteValues is added in specializations/discrete.h as a std::map @@ -162,7 +165,6 @@ virtual class DiscreteDistribution : gtsam::DiscreteConditional { gtsam::DefaultKeyFormatter) const; double operator()(size_t value) const; std::vector pmf() const; - size_t argmax() const; }; #include From 89f7f7f72198b385c18fa6de73b9fcf70d0fc46b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Jul 2024 23:43:29 -0400 Subject: [PATCH 20/38] improve DiscreteConditional::argmax method to accept parent values --- gtsam/discrete/DiscreteConditional.cpp | 10 ++++---- gtsam/discrete/DiscreteConditional.h | 2 +- .../tests/testDiscreteConditional.cpp | 23 +++++++++++++++++++ 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 5abc094fb..a7f472f26 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -235,16 +235,16 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( } /* ************************************************************************** */ -size_t DiscreteConditional::argmax() const { +size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { size_t maxValue = 0; double maxP = 0; + DiscreteValues values = parentsValues; + assert(nrFrontals() == 1); - assert(nrParents() == 0); - DiscreteValues frontals; Key j = firstFrontalKey(); for (size_t value = 0; value < cardinality(j); value++) { - frontals[j] = value; - double pValueS = (*this)(frontals); + values[j] = value; + double pValueS = (*this)(values); // Update MPE solution if better if (pValueS > maxP) { maxP = pValueS; diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 50fa6e161..8f38a83be 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -217,7 +217,7 @@ class GTSAM_EXPORT DiscreteConditional * @brief Return assignment that maximizes distribution. * @return Optimal assignment (1 frontal variable). */ - size_t argmax() const; + size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const; /// @} /// @name Advanced Interface diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index f2c6d7b9f..a11c87975 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -289,6 +289,29 @@ TEST(DiscreteConditional, choose) { EXPECT(assert_equal(expected3, *actual3, 1e-9)); } +/* ************************************************************************* */ +// Check argmax on P(C|D) and P(D) +TEST(DiscreteConditional, Argmax) { + DiscreteKey C(2, 2), D(4, 2); + DiscreteConditional D_cond(D, "1/3"); + DiscreteConditional C_given_DE((C | D) = "1/4 1/1"); + + // Case 1: No parents + size_t actual1 = D_cond.argmax(); + EXPECT_LONGS_EQUAL(1, actual1); + + // Case 2: Given parent values + DiscreteValues given; + given[D.first] = 1; + size_t actual2 = C_given_DE.argmax(given); + // Should be 0 since D=1 gives 0.5/0.5 + EXPECT_LONGS_EQUAL(0, actual2); + + given[D.first] = 0; + size_t actual3 = C_given_DE.argmax(given); + EXPECT_LONGS_EQUAL(1, actual3); +} + /* ************************************************************************* */ // Check markdown representation looks as expected, no parents. TEST(DiscreteConditional, markdown_prior) { From 1657262c8747a3913fa3ad6083b816893c10d536 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Jul 2024 23:44:43 -0400 Subject: [PATCH 21/38] DiscreteBayesNet::mode method to get maximizing assignment --- gtsam/discrete/DiscreteBayesNet.cpp | 8 ++++++ gtsam/discrete/DiscreteBayesNet.h | 8 ++++++ gtsam/discrete/tests/testDiscreteBayesNet.cpp | 26 +++++++++++++++++++ 3 files changed, 42 insertions(+) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index f754250ed..bce14ad46 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -62,6 +62,14 @@ DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const { return result; } +DiscreteValues DiscreteBayesNet::mode() const { + DiscreteValues result; + for (auto it = begin(); it != end(); ++it) { + result[(*it)->firstFrontalKey()] = (*it)->argmax(result); + } + return result; +} + /* *********************************************************************** */ std::string DiscreteBayesNet::markdown( const KeyFormatter& keyFormatter, diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index a5a4621aa..3bcdcfe84 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -124,6 +124,14 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { */ DiscreteValues sample(DiscreteValues given) const; + /** + * @brief Compute the discrete assignment which gives the highest + * probability for the DiscreteBayesNet. + * + * @return DiscreteValues + */ + DiscreteValues mode() const; + ///@} /// @name Wrapper support /// @{ diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 95f407cae..7cd445c5b 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -122,6 +122,32 @@ TEST(DiscreteBayesNet, Asia) { EXPECT(assert_equal(expectedSample, actualSample)); } +/* ************************************************************************* */ +TEST(DiscreteBayesNet, Mode) { + DiscreteBayesNet asia; + + asia.add(Asia, "99/1"); + asia.add(Smoking % "50/50"); // Signature version + + asia.add(Tuberculosis | Asia = "99/1 95/5"); + asia.add(LungCancer | Smoking = "99/1 90/10"); + asia.add(Bronchitis | Smoking = "70/30 40/60"); + + asia.add((Either | Tuberculosis, LungCancer) = "F T T T"); + + asia.add(XRay | Either = "95/5 2/98"); + asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9"); + + DiscreteValues actual = asia.mode(); + // NOTE: Examined the DBN and found the optimal assignment. + DiscreteValues expected{ + {Asia.first, 0}, {Smoking.first, 0}, {Tuberculosis.first, 0}, + {LungCancer.first, 0}, {Bronchitis.first, 0}, {Either.first, 0}, + {XRay.first, 0}, {Dyspnea.first, 0}, + }; + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST(DiscreteBayesNet, Sugar) { DiscreteKey T(0, 2), L(1, 2), E(2, 2), C(8, 3), S(7, 2); From d5be6d9bcea1e9a67a588bb0b1aab658889d2342 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 Jul 2024 00:19:17 -0400 Subject: [PATCH 22/38] wrap argmax and mode methods --- gtsam/discrete/discrete.i | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 5eacb3634..9f55da28e 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -137,6 +137,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor { size_t sample(size_t value) const; size_t sample() const; void sampleInPlace(gtsam::DiscreteValues @parentsValues) const; + size_t argmax(const gtsam::DiscreteValues& parents) const; // Markdown and HTML string markdown(const gtsam::KeyFormatter& keyFormatter = @@ -192,6 +193,7 @@ class DiscreteBayesNet { gtsam::DiscreteValues sample() const; gtsam::DiscreteValues sample(gtsam::DiscreteValues given) const; + gtsam::DiscreteValues mode() const; string dot( const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, From 96a24445a4d314285384250a8332590ddb3d00b6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 10:12:49 -0400 Subject: [PATCH 23/38] address review comments --- gtsam/discrete/tests/testDiscreteConditional.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index a11c87975..2abb67fca 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -293,22 +293,22 @@ TEST(DiscreteConditional, choose) { // Check argmax on P(C|D) and P(D) TEST(DiscreteConditional, Argmax) { DiscreteKey C(2, 2), D(4, 2); - DiscreteConditional D_cond(D, "1/3"); - DiscreteConditional C_given_DE((C | D) = "1/4 1/1"); + DiscreteConditional D_prior(D, "1/3"); + DiscreteConditional C_given_D((C | D) = "1/4 1/1"); // Case 1: No parents - size_t actual1 = D_cond.argmax(); + size_t actual1 = D_prior.argmax(); EXPECT_LONGS_EQUAL(1, actual1); // Case 2: Given parent values DiscreteValues given; given[D.first] = 1; - size_t actual2 = C_given_DE.argmax(given); + size_t actual2 = C_given_D.argmax(given); // Should be 0 since D=1 gives 0.5/0.5 EXPECT_LONGS_EQUAL(0, actual2); given[D.first] = 0; - size_t actual3 = C_given_DE.argmax(given); + size_t actual3 = C_given_D.argmax(given); EXPECT_LONGS_EQUAL(1, actual3); } From f6449c0ad8563cb576268dcb3b8692dd06ba59da Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 10:30:23 -0400 Subject: [PATCH 24/38] turns out we can merge DiscreteConditional and DiscreteLookupTable --- gtsam/discrete/DiscreteConditional.cpp | 32 +++++++++- gtsam/discrete/DiscreteConditional.h | 20 ++++--- gtsam/discrete/DiscreteLookupDAG.cpp | 83 +------------------------- gtsam/discrete/DiscreteLookupDAG.h | 38 +----------- 4 files changed, 49 insertions(+), 124 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index a7f472f26..ec17e22f6 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -236,6 +236,9 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( /* ************************************************************************** */ size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { + ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) + + // Initialize size_t maxValue = 0; double maxP = 0; DiscreteValues values = parentsValues; @@ -254,6 +257,33 @@ size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { return maxValue; } +/* ************************************************************************** */ +void DiscreteConditional::argmaxInPlace(DiscreteValues* values) const { + ADT pFS = choose(*values, true); // P(F|S=parentsValues) + + // Initialize + DiscreteValues mpe; + double maxP = 0; + + // Get all Possible Configurations + const auto allPosbValues = frontalAssignments(); + + // Find the maximum + for (const auto& frontalVals : allPosbValues) { + double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) + // Update maximum solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = frontalVals; + } + } + + // set values (inPlace) to maximum + for (Key j : frontals()) { + (*values)[j] = mpe[j]; + } +} + /* ************************************************************************** */ void DiscreteConditional::sampleInPlace(DiscreteValues* values) const { assert(nrFrontals() == 1); @@ -459,7 +489,7 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter, } /* ************************************************************************* */ -double DiscreteConditional::evaluate(const HybridValues& x) const{ +double DiscreteConditional::evaluate(const HybridValues& x) const { return this->evaluate(x.discrete()); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 8f38a83be..eda838e91 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -18,9 +18,9 @@ #pragma once -#include #include #include +#include #include #include @@ -39,7 +39,7 @@ class GTSAM_EXPORT DiscreteConditional public Conditional { public: // typedefs needed to play nice with gtsam - typedef DiscreteConditional This; ///< Typedef to this class + typedef DiscreteConditional This; ///< Typedef to this class typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef DecisionTreeFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional @@ -159,9 +159,7 @@ class GTSAM_EXPORT DiscreteConditional /// @{ /// Log-probability is just -error(x). - double logProbability(const DiscreteValues& x) const { - return -error(x); - } + double logProbability(const DiscreteValues& x) const { return -error(x); } /// print index signature only void printSignature( @@ -214,11 +212,18 @@ class GTSAM_EXPORT DiscreteConditional size_t sample() const; /** - * @brief Return assignment that maximizes distribution. - * @return Optimal assignment (1 frontal variable). + * @brief Return assignment for single frontal variable that maximizes value. + * @param parentsValues Known assignments for the parents. + * @return maximizing assignment for the frontal variable. */ size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const; + /** + * @brief Calculate assignment for frontal variables that maximizes value. + * @param (in/out) parentsValues Known assignments for the parents. + */ + void argmaxInPlace(DiscreteValues* parentsValues) const; + /// @} /// @name Advanced Interface /// @{ @@ -244,7 +249,6 @@ class GTSAM_EXPORT DiscreteConditional std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const Names& names = {}) const override; - /// @} /// @name HybridValues methods. /// @{ diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index ab62055ed..11900b502 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -29,97 +29,20 @@ using std::vector; namespace gtsam { -/* ************************************************************************** */ -// TODO(dellaert): copy/paste from DiscreteConditional.cpp :-( -void DiscreteLookupTable::print(const std::string& s, - const KeyFormatter& formatter) const { - using std::cout; - using std::endl; - - cout << s << " g( "; - for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { - cout << formatter(*it) << " "; - } - if (nrParents()) { - cout << "; "; - for (const_iterator it = beginParents(); it != endParents(); ++it) { - cout << formatter(*it) << " "; - } - } - cout << "):\n"; - ADT::print("", formatter); - cout << endl; -} - -/* ************************************************************************** */ -void DiscreteLookupTable::argmaxInPlace(DiscreteValues* values) const { - ADT pFS = choose(*values, true); // P(F|S=parentsValues) - - // Initialize - DiscreteValues mpe; - double maxP = 0; - - // Get all Possible Configurations - const auto allPosbValues = frontalAssignments(); - - // Find the maximum - for (const auto& frontalVals : allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update maximum solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } - - // set values (inPlace) to maximum - for (Key j : frontals()) { - (*values)[j] = mpe[j]; - } -} - -/* ************************************************************************** */ -size_t DiscreteLookupTable::argmax(const DiscreteValues& parentsValues) const { - ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) - - // Then, find the max over all remaining - // TODO(Duy): only works for one key now, seems horribly slow this way - size_t mpe = 0; - double maxP = 0; - DiscreteValues frontals; - assert(nrFrontals() == 1); - Key j = (firstFrontalKey()); - for (size_t value = 0; value < cardinality(j); value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = value; - } - } - return mpe; -} - /* ************************************************************************** */ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet( const DiscreteBayesNet& bayesNet) { DiscreteLookupDAG dag; for (auto&& conditional : bayesNet) { - if (auto lookupTable = - std::dynamic_pointer_cast(conditional)) { - dag.push_back(lookupTable); - } else { - throw std::runtime_error( - "DiscreteFactorGraph::maxProduct: Expected look up table."); - } + dag.push_back(conditional); } return dag; } DiscreteValues DiscreteLookupDAG::argmax(DiscreteValues result) const { // Argmax each node in turn in topological sort order (parents first). - for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) { + for (auto it = std::make_reverse_iterator(end()); + it != std::make_reverse_iterator(begin()); ++it) { // dereference to get the sharedFactor to the lookup table (*it)->argmaxInPlace(&result); } diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index f077a13d9..3b0a5770d 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -37,41 +37,9 @@ class DiscreteBayesNet; * Inherits from discrete conditional for convenience, but is not normalized. * Is used in the max-product algorithm. */ -class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { - public: - using This = DiscreteLookupTable; - using shared_ptr = std::shared_ptr; - using BaseConditional = Conditional; - - /** - * @brief Construct a new Discrete Lookup Table object - * - * @param nFrontals number of frontal variables - * @param keys a sorted list of gtsam::Keys - * @param potentials the algebraic decision tree with lookup values - */ - DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys, - const ADT& potentials) - : DiscreteConditional(nFrontals, keys, potentials) {} - - /// GTSAM-style print - void print( - const std::string& s = "Discrete Lookup Table: ", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /** - * @brief return assignment for single frontal variable that maximizes value. - * @param parentsValues Known assignments for the parents. - * @return maximizing assignment for the frontal variable. - */ - size_t argmax(const DiscreteValues& parentsValues) const; - - /** - * @brief Calculate assignment for frontal variables that maximizes value. - * @param (in/out) parentsValues Known assignments for the parents. - */ - void argmaxInPlace(DiscreteValues* parentsValues) const; -}; +// Typedef for backwards compatibility +// TODO(Varun): Remove +using DiscreteLookupTable = DiscreteConditional; /** A DAG made from lookup tables, as defined above. */ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet { From a43dad2e3439f1877944143114639d17c6a7712e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 10:31:27 -0400 Subject: [PATCH 25/38] use DiscreteLookupDAG for DiscreteBayesNet mode --- gtsam/discrete/DiscreteBayesNet.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index bce14ad46..bef0413c8 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace gtsam { @@ -56,18 +57,15 @@ DiscreteValues DiscreteBayesNet::sample() const { DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const { // sample each node in turn in topological sort order (parents first) - for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) { + for (auto it = std::make_reverse_iterator(end()); + it != std::make_reverse_iterator(begin()); ++it) { (*it)->sampleInPlace(&result); } return result; } DiscreteValues DiscreteBayesNet::mode() const { - DiscreteValues result; - for (auto it = begin(); it != end(); ++it) { - result[(*it)->firstFrontalKey()] = (*it)->argmax(result); - } - return result; + return DiscreteLookupDAG::FromBayesNet(*this).argmax(); } /* *********************************************************************** */ From 19ea2712c0281523a0b70a7e24789d43e2ae7683 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 10:31:50 -0400 Subject: [PATCH 26/38] setup discrete bayes net in mode test with proper ordering --- gtsam/discrete/tests/testDiscreteBayesNet.cpp | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 7cd445c5b..64c823203 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -16,14 +16,13 @@ * @author Frank Dellaert */ +#include +#include +#include +#include #include #include #include -#include -#include -#include - -#include #include #include @@ -43,8 +42,7 @@ TEST(DiscreteBayesNet, bayesNet) { DiscreteKey Parent(0, 2), Child(1, 2); auto prior = std::make_shared(Parent % "6/4"); - CHECK(assert_equal(ADT({Parent}, "0.6 0.4"), - (ADT)*prior)); + CHECK(assert_equal(ADT({Parent}, "0.6 0.4"), (ADT)*prior)); bayesNet.push_back(prior); auto conditional = @@ -126,17 +124,18 @@ TEST(DiscreteBayesNet, Asia) { TEST(DiscreteBayesNet, Mode) { DiscreteBayesNet asia; - asia.add(Asia, "99/1"); - asia.add(Smoking % "50/50"); // Signature version + // We need to order the Bayes net in bottom-up fashion + asia.add(XRay | Either = "95/5 2/98"); + asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9"); + + asia.add((Either | Tuberculosis, LungCancer) = "F T T T"); asia.add(Tuberculosis | Asia = "99/1 95/5"); asia.add(LungCancer | Smoking = "99/1 90/10"); asia.add(Bronchitis | Smoking = "70/30 40/60"); - asia.add((Either | Tuberculosis, LungCancer) = "F T T T"); - - asia.add(XRay | Either = "95/5 2/98"); - asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9"); + asia.add(Asia, "99/1"); + asia.add(Smoking % "50/50"); // Signature version DiscreteValues actual = asia.mode(); // NOTE: Examined the DBN and found the optimal assignment. From ffa72e7fadfff48864c990027f9b60a9f23506e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 15:53:14 -0400 Subject: [PATCH 27/38] remove DiscreteLookupTable from wrapper --- gtsam/discrete/discrete.i | 9 --------- 1 file changed, 9 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 9f55da28e..43d2559d9 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -263,15 +263,6 @@ class DiscreteBayesTree { #include -class DiscreteLookupTable : gtsam::DiscreteConditional{ - DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys, - const gtsam::DecisionTreeFactor::ADT& potentials); - void print(string s = "Discrete Lookup Table: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - size_t argmax(const gtsam::DiscreteValues& parentsValues) const; -}; - class DiscreteLookupDAG { DiscreteLookupDAG(); void push_back(const gtsam::DiscreteLookupTable* table); From 4e66fff1537ebf7ab5f5c104ac01284a82d8235f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 17:57:37 -0400 Subject: [PATCH 28/38] use MaxProduct to compute Discrete Bayes Net mode --- gtsam/discrete/DiscreteBayesNet.cpp | 3 ++- gtsam/discrete/DiscreteConditional.cpp | 5 +++-- gtsam/discrete/tests/testDiscreteBayesNet.cpp | 22 +++++++++++++++++++ 3 files changed, 27 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index bef0413c8..f00eca60c 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -65,7 +66,7 @@ DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const { } DiscreteValues DiscreteBayesNet::mode() const { - return DiscreteLookupDAG::FromBayesNet(*this).argmax(); + return DiscreteFactorGraph(*this).optimize(); } /* *********************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index ec17e22f6..90b3cfa39 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -238,7 +238,8 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) - // Initialize + // Then, find the max over all remaining + // TODO(Duy): only works for one key now, seems horribly slow this way size_t maxValue = 0; double maxP = 0; DiscreteValues values = parentsValues; @@ -247,7 +248,7 @@ size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { Key j = firstFrontalKey(); for (size_t value = 0; value < cardinality(j); value++) { values[j] = value; - double pValueS = (*this)(values); + double pValueS = pFS(values); // P(F=value|S=parentsValues) // Update MPE solution if better if (pValueS > maxP) { maxP = pValueS; diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 64c823203..b87e1c67a 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -147,6 +147,28 @@ TEST(DiscreteBayesNet, Mode) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(DiscreteBayesNet, ModeEdgeCase) { + // Declare 2 keys + DiscreteKey A(0, 2), B(1, 2); + + // Create Bayes net such that marginal on A is bigger for 0 than 1, but the + // MPE does not have A=0. + DiscreteBayesNet bayesNet; + bayesNet.add(B | A = "1/1 1/2"); + bayesNet.add(A % "10/9"); + + // Which we verify using max-product: + DiscreteFactorGraph graph(bayesNet); + // The expected MPE is A=1, B=1 + DiscreteValues expectedMPE = graph.optimize(); + + auto actualMPE = bayesNet.mode(); + + EXPECT(assert_equal(expectedMPE, actualMPE)); + EXPECT_DOUBLES_EQUAL(0.315789, bayesNet(expectedMPE), 1e-5); // regression +} + /* ************************************************************************* */ TEST(DiscreteBayesNet, Sugar) { DiscreteKey T(0, 2), L(1, 2), E(2, 2), C(8, 3), S(7, 2); From 4a04963197799e098e37cea414eb991342476279 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 15 Jul 2024 12:26:49 -0400 Subject: [PATCH 29/38] kill DiscreteBayesNet::mode --- gtsam/discrete/DiscreteBayesNet.cpp | 4 -- gtsam/discrete/DiscreteBayesNet.h | 8 --- gtsam/discrete/discrete.i | 1 - gtsam/discrete/tests/testDiscreteBayesNet.cpp | 49 ------------------- 4 files changed, 62 deletions(-) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index f00eca60c..c1aa18828 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -65,10 +65,6 @@ DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const { return result; } -DiscreteValues DiscreteBayesNet::mode() const { - return DiscreteFactorGraph(*this).optimize(); -} - /* *********************************************************************** */ std::string DiscreteBayesNet::markdown( const KeyFormatter& keyFormatter, diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index 3bcdcfe84..a5a4621aa 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -124,14 +124,6 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { */ DiscreteValues sample(DiscreteValues given) const; - /** - * @brief Compute the discrete assignment which gives the highest - * probability for the DiscreteBayesNet. - * - * @return DiscreteValues - */ - DiscreteValues mode() const; - ///@} /// @name Wrapper support /// @{ diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 43d2559d9..0f34840bf 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -193,7 +193,6 @@ class DiscreteBayesNet { gtsam::DiscreteValues sample() const; gtsam::DiscreteValues sample(gtsam::DiscreteValues given) const; - gtsam::DiscreteValues mode() const; string dot( const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index b87e1c67a..49a360cbb 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -120,55 +120,6 @@ TEST(DiscreteBayesNet, Asia) { EXPECT(assert_equal(expectedSample, actualSample)); } -/* ************************************************************************* */ -TEST(DiscreteBayesNet, Mode) { - DiscreteBayesNet asia; - - // We need to order the Bayes net in bottom-up fashion - asia.add(XRay | Either = "95/5 2/98"); - asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9"); - - asia.add((Either | Tuberculosis, LungCancer) = "F T T T"); - - asia.add(Tuberculosis | Asia = "99/1 95/5"); - asia.add(LungCancer | Smoking = "99/1 90/10"); - asia.add(Bronchitis | Smoking = "70/30 40/60"); - - asia.add(Asia, "99/1"); - asia.add(Smoking % "50/50"); // Signature version - - DiscreteValues actual = asia.mode(); - // NOTE: Examined the DBN and found the optimal assignment. - DiscreteValues expected{ - {Asia.first, 0}, {Smoking.first, 0}, {Tuberculosis.first, 0}, - {LungCancer.first, 0}, {Bronchitis.first, 0}, {Either.first, 0}, - {XRay.first, 0}, {Dyspnea.first, 0}, - }; - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(DiscreteBayesNet, ModeEdgeCase) { - // Declare 2 keys - DiscreteKey A(0, 2), B(1, 2); - - // Create Bayes net such that marginal on A is bigger for 0 than 1, but the - // MPE does not have A=0. - DiscreteBayesNet bayesNet; - bayesNet.add(B | A = "1/1 1/2"); - bayesNet.add(A % "10/9"); - - // Which we verify using max-product: - DiscreteFactorGraph graph(bayesNet); - // The expected MPE is A=1, B=1 - DiscreteValues expectedMPE = graph.optimize(); - - auto actualMPE = bayesNet.mode(); - - EXPECT(assert_equal(expectedMPE, actualMPE)); - EXPECT_DOUBLES_EQUAL(0.315789, bayesNet(expectedMPE), 1e-5); // regression -} - /* ************************************************************************* */ TEST(DiscreteBayesNet, Sugar) { DiscreteKey T(0, 2), L(1, 2), E(2, 2), C(8, 3), S(7, 2); From 83eff969c5d2e3c1cc34848bdb19d3aa25f9cf07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 15 Jul 2024 17:46:26 -0400 Subject: [PATCH 30/38] add tie-breaking test --- .../tests/testDiscreteConditional.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 2abb67fca..172dd0fa1 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -290,26 +290,32 @@ TEST(DiscreteConditional, choose) { } /* ************************************************************************* */ -// Check argmax on P(C|D) and P(D) +// Check argmax on P(C|D) and P(D), plus tie-breaking for P(B) TEST(DiscreteConditional, Argmax) { - DiscreteKey C(2, 2), D(4, 2); + DiscreteKey B(2, 2), C(2, 2), D(4, 2); + DiscreteConditional B_prior(D, "1/1"); DiscreteConditional D_prior(D, "1/3"); DiscreteConditional C_given_D((C | D) = "1/4 1/1"); - // Case 1: No parents - size_t actual1 = D_prior.argmax(); - EXPECT_LONGS_EQUAL(1, actual1); + // Case 1: Tie breaking + size_t actual1 = B_prior.argmax(); + // In the case of ties, the first value is chosen. + EXPECT_LONGS_EQUAL(0, actual1); + // Case 2: No parents + size_t actual2 = D_prior.argmax(); + // Selects 1 since it has 0.75 probability + EXPECT_LONGS_EQUAL(1, actual2); - // Case 2: Given parent values + // Case 3: Given parent values DiscreteValues given; given[D.first] = 1; - size_t actual2 = C_given_D.argmax(given); + size_t actual3 = C_given_D.argmax(given); // Should be 0 since D=1 gives 0.5/0.5 - EXPECT_LONGS_EQUAL(0, actual2); + EXPECT_LONGS_EQUAL(0, actual3); given[D.first] = 0; - size_t actual3 = C_given_D.argmax(given); - EXPECT_LONGS_EQUAL(1, actual3); + size_t actual4 = C_given_D.argmax(given); + EXPECT_LONGS_EQUAL(1, actual4); } /* ************************************************************************* */ From 52f1aba10cbfce27c4aa9d3e8854ab9f164a9f13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 10:30:23 -0400 Subject: [PATCH 31/38] turns out we can merge DiscreteConditional and DiscreteLookupTable --- gtsam/discrete/DiscreteConditional.cpp | 34 ++++++++++- gtsam/discrete/DiscreteConditional.h | 20 ++++--- gtsam/discrete/DiscreteLookupDAG.cpp | 83 +------------------------- gtsam/discrete/DiscreteLookupDAG.h | 38 +----------- 4 files changed, 50 insertions(+), 125 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 5abc094fb..1bca5078c 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -235,7 +235,10 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( } /* ************************************************************************** */ -size_t DiscreteConditional::argmax() const { +size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { + ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) + + // Initialize size_t maxValue = 0; double maxP = 0; assert(nrFrontals() == 1); @@ -254,6 +257,33 @@ size_t DiscreteConditional::argmax() const { return maxValue; } +/* ************************************************************************** */ +void DiscreteConditional::argmaxInPlace(DiscreteValues* values) const { + ADT pFS = choose(*values, true); // P(F|S=parentsValues) + + // Initialize + DiscreteValues mpe; + double maxP = 0; + + // Get all Possible Configurations + const auto allPosbValues = frontalAssignments(); + + // Find the maximum + for (const auto& frontalVals : allPosbValues) { + double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) + // Update maximum solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = frontalVals; + } + } + + // set values (inPlace) to maximum + for (Key j : frontals()) { + (*values)[j] = mpe[j]; + } +} + /* ************************************************************************** */ void DiscreteConditional::sampleInPlace(DiscreteValues* values) const { assert(nrFrontals() == 1); @@ -459,7 +489,7 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter, } /* ************************************************************************* */ -double DiscreteConditional::evaluate(const HybridValues& x) const{ +double DiscreteConditional::evaluate(const HybridValues& x) const { return this->evaluate(x.discrete()); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 50fa6e161..bc50e1301 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -18,9 +18,9 @@ #pragma once -#include #include #include +#include #include #include @@ -39,7 +39,7 @@ class GTSAM_EXPORT DiscreteConditional public Conditional { public: // typedefs needed to play nice with gtsam - typedef DiscreteConditional This; ///< Typedef to this class + typedef DiscreteConditional This; ///< Typedef to this class typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef DecisionTreeFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional @@ -159,9 +159,7 @@ class GTSAM_EXPORT DiscreteConditional /// @{ /// Log-probability is just -error(x). - double logProbability(const DiscreteValues& x) const { - return -error(x); - } + double logProbability(const DiscreteValues& x) const { return -error(x); } /// print index signature only void printSignature( @@ -214,11 +212,18 @@ class GTSAM_EXPORT DiscreteConditional size_t sample() const; /** - * @brief Return assignment that maximizes distribution. - * @return Optimal assignment (1 frontal variable). + * @brief Return assignment for single frontal variable that maximizes value. + * @param parentsValues Known assignments for the parents. + * @return maximizing assignment for the frontal variable. */ size_t argmax() const; + /** + * @brief Calculate assignment for frontal variables that maximizes value. + * @param (in/out) parentsValues Known assignments for the parents. + */ + void argmaxInPlace(DiscreteValues* parentsValues) const; + /// @} /// @name Advanced Interface /// @{ @@ -244,7 +249,6 @@ class GTSAM_EXPORT DiscreteConditional std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const Names& names = {}) const override; - /// @} /// @name HybridValues methods. /// @{ diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index ab62055ed..11900b502 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -29,97 +29,20 @@ using std::vector; namespace gtsam { -/* ************************************************************************** */ -// TODO(dellaert): copy/paste from DiscreteConditional.cpp :-( -void DiscreteLookupTable::print(const std::string& s, - const KeyFormatter& formatter) const { - using std::cout; - using std::endl; - - cout << s << " g( "; - for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { - cout << formatter(*it) << " "; - } - if (nrParents()) { - cout << "; "; - for (const_iterator it = beginParents(); it != endParents(); ++it) { - cout << formatter(*it) << " "; - } - } - cout << "):\n"; - ADT::print("", formatter); - cout << endl; -} - -/* ************************************************************************** */ -void DiscreteLookupTable::argmaxInPlace(DiscreteValues* values) const { - ADT pFS = choose(*values, true); // P(F|S=parentsValues) - - // Initialize - DiscreteValues mpe; - double maxP = 0; - - // Get all Possible Configurations - const auto allPosbValues = frontalAssignments(); - - // Find the maximum - for (const auto& frontalVals : allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update maximum solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } - - // set values (inPlace) to maximum - for (Key j : frontals()) { - (*values)[j] = mpe[j]; - } -} - -/* ************************************************************************** */ -size_t DiscreteLookupTable::argmax(const DiscreteValues& parentsValues) const { - ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) - - // Then, find the max over all remaining - // TODO(Duy): only works for one key now, seems horribly slow this way - size_t mpe = 0; - double maxP = 0; - DiscreteValues frontals; - assert(nrFrontals() == 1); - Key j = (firstFrontalKey()); - for (size_t value = 0; value < cardinality(j); value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = value; - } - } - return mpe; -} - /* ************************************************************************** */ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet( const DiscreteBayesNet& bayesNet) { DiscreteLookupDAG dag; for (auto&& conditional : bayesNet) { - if (auto lookupTable = - std::dynamic_pointer_cast(conditional)) { - dag.push_back(lookupTable); - } else { - throw std::runtime_error( - "DiscreteFactorGraph::maxProduct: Expected look up table."); - } + dag.push_back(conditional); } return dag; } DiscreteValues DiscreteLookupDAG::argmax(DiscreteValues result) const { // Argmax each node in turn in topological sort order (parents first). - for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) { + for (auto it = std::make_reverse_iterator(end()); + it != std::make_reverse_iterator(begin()); ++it) { // dereference to get the sharedFactor to the lookup table (*it)->argmaxInPlace(&result); } diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index f077a13d9..3b0a5770d 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -37,41 +37,9 @@ class DiscreteBayesNet; * Inherits from discrete conditional for convenience, but is not normalized. * Is used in the max-product algorithm. */ -class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { - public: - using This = DiscreteLookupTable; - using shared_ptr = std::shared_ptr; - using BaseConditional = Conditional; - - /** - * @brief Construct a new Discrete Lookup Table object - * - * @param nFrontals number of frontal variables - * @param keys a sorted list of gtsam::Keys - * @param potentials the algebraic decision tree with lookup values - */ - DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys, - const ADT& potentials) - : DiscreteConditional(nFrontals, keys, potentials) {} - - /// GTSAM-style print - void print( - const std::string& s = "Discrete Lookup Table: ", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /** - * @brief return assignment for single frontal variable that maximizes value. - * @param parentsValues Known assignments for the parents. - * @return maximizing assignment for the frontal variable. - */ - size_t argmax(const DiscreteValues& parentsValues) const; - - /** - * @brief Calculate assignment for frontal variables that maximizes value. - * @param (in/out) parentsValues Known assignments for the parents. - */ - void argmaxInPlace(DiscreteValues* parentsValues) const; -}; +// Typedef for backwards compatibility +// TODO(Varun): Remove +using DiscreteLookupTable = DiscreteConditional; /** A DAG made from lookup tables, as defined above. */ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet { From 5c055ec4b4ed4101fa82637b104592cf71ed60f5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 14 Jul 2024 15:53:14 -0400 Subject: [PATCH 32/38] remove DiscreteLookupTable from wrapper --- gtsam/discrete/discrete.i | 9 --------- 1 file changed, 9 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index a1731f8e5..5782f66c0 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -256,15 +256,6 @@ class DiscreteBayesTree { #include -class DiscreteLookupTable : gtsam::DiscreteConditional{ - DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys, - const gtsam::DecisionTreeFactor::ADT& potentials); - void print(string s = "Discrete Lookup Table: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - size_t argmax(const gtsam::DiscreteValues& parentsValues) const; -}; - class DiscreteLookupDAG { DiscreteLookupDAG(); void push_back(const gtsam::DiscreteLookupTable* table); From 016f6f28d1eba7f0780795557a064de80f16bd07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 15 Jul 2024 18:39:37 -0400 Subject: [PATCH 33/38] Revert "turns out we can merge DiscreteConditional and DiscreteLookupTable" This reverts commit f6449c0ad8563cb576268dcb3b8692dd06ba59da. --- gtsam/discrete/DiscreteConditional.cpp | 33 +--------- gtsam/discrete/DiscreteConditional.h | 20 +++---- gtsam/discrete/DiscreteLookupDAG.cpp | 83 +++++++++++++++++++++++++- gtsam/discrete/DiscreteLookupDAG.h | 38 +++++++++++- 4 files changed, 124 insertions(+), 50 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 90b3cfa39..6df26d291 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -236,10 +236,6 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( /* ************************************************************************** */ size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { - ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) - - // Then, find the max over all remaining - // TODO(Duy): only works for one key now, seems horribly slow this way size_t maxValue = 0; double maxP = 0; DiscreteValues values = parentsValues; @@ -258,33 +254,6 @@ size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { return maxValue; } -/* ************************************************************************** */ -void DiscreteConditional::argmaxInPlace(DiscreteValues* values) const { - ADT pFS = choose(*values, true); // P(F|S=parentsValues) - - // Initialize - DiscreteValues mpe; - double maxP = 0; - - // Get all Possible Configurations - const auto allPosbValues = frontalAssignments(); - - // Find the maximum - for (const auto& frontalVals : allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update maximum solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } - - // set values (inPlace) to maximum - for (Key j : frontals()) { - (*values)[j] = mpe[j]; - } -} - /* ************************************************************************** */ void DiscreteConditional::sampleInPlace(DiscreteValues* values) const { assert(nrFrontals() == 1); @@ -490,7 +459,7 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter, } /* ************************************************************************* */ -double DiscreteConditional::evaluate(const HybridValues& x) const { +double DiscreteConditional::evaluate(const HybridValues& x) const{ return this->evaluate(x.discrete()); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index eda838e91..8f38a83be 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -18,9 +18,9 @@ #pragma once +#include #include #include -#include #include #include @@ -39,7 +39,7 @@ class GTSAM_EXPORT DiscreteConditional public Conditional { public: // typedefs needed to play nice with gtsam - typedef DiscreteConditional This; ///< Typedef to this class + typedef DiscreteConditional This; ///< Typedef to this class typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef DecisionTreeFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional @@ -159,7 +159,9 @@ class GTSAM_EXPORT DiscreteConditional /// @{ /// Log-probability is just -error(x). - double logProbability(const DiscreteValues& x) const { return -error(x); } + double logProbability(const DiscreteValues& x) const { + return -error(x); + } /// print index signature only void printSignature( @@ -212,18 +214,11 @@ class GTSAM_EXPORT DiscreteConditional size_t sample() const; /** - * @brief Return assignment for single frontal variable that maximizes value. - * @param parentsValues Known assignments for the parents. - * @return maximizing assignment for the frontal variable. + * @brief Return assignment that maximizes distribution. + * @return Optimal assignment (1 frontal variable). */ size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const; - /** - * @brief Calculate assignment for frontal variables that maximizes value. - * @param (in/out) parentsValues Known assignments for the parents. - */ - void argmaxInPlace(DiscreteValues* parentsValues) const; - /// @} /// @name Advanced Interface /// @{ @@ -249,6 +244,7 @@ class GTSAM_EXPORT DiscreteConditional std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const Names& names = {}) const override; + /// @} /// @name HybridValues methods. /// @{ diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index 11900b502..ab62055ed 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -29,20 +29,97 @@ using std::vector; namespace gtsam { +/* ************************************************************************** */ +// TODO(dellaert): copy/paste from DiscreteConditional.cpp :-( +void DiscreteLookupTable::print(const std::string& s, + const KeyFormatter& formatter) const { + using std::cout; + using std::endl; + + cout << s << " g( "; + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + cout << formatter(*it) << " "; + } + if (nrParents()) { + cout << "; "; + for (const_iterator it = beginParents(); it != endParents(); ++it) { + cout << formatter(*it) << " "; + } + } + cout << "):\n"; + ADT::print("", formatter); + cout << endl; +} + +/* ************************************************************************** */ +void DiscreteLookupTable::argmaxInPlace(DiscreteValues* values) const { + ADT pFS = choose(*values, true); // P(F|S=parentsValues) + + // Initialize + DiscreteValues mpe; + double maxP = 0; + + // Get all Possible Configurations + const auto allPosbValues = frontalAssignments(); + + // Find the maximum + for (const auto& frontalVals : allPosbValues) { + double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) + // Update maximum solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = frontalVals; + } + } + + // set values (inPlace) to maximum + for (Key j : frontals()) { + (*values)[j] = mpe[j]; + } +} + +/* ************************************************************************** */ +size_t DiscreteLookupTable::argmax(const DiscreteValues& parentsValues) const { + ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) + + // Then, find the max over all remaining + // TODO(Duy): only works for one key now, seems horribly slow this way + size_t mpe = 0; + double maxP = 0; + DiscreteValues frontals; + assert(nrFrontals() == 1); + Key j = (firstFrontalKey()); + for (size_t value = 0; value < cardinality(j); value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = value; + } + } + return mpe; +} + /* ************************************************************************** */ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet( const DiscreteBayesNet& bayesNet) { DiscreteLookupDAG dag; for (auto&& conditional : bayesNet) { - dag.push_back(conditional); + if (auto lookupTable = + std::dynamic_pointer_cast(conditional)) { + dag.push_back(lookupTable); + } else { + throw std::runtime_error( + "DiscreteFactorGraph::maxProduct: Expected look up table."); + } } return dag; } DiscreteValues DiscreteLookupDAG::argmax(DiscreteValues result) const { // Argmax each node in turn in topological sort order (parents first). - for (auto it = std::make_reverse_iterator(end()); - it != std::make_reverse_iterator(begin()); ++it) { + for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) { // dereference to get the sharedFactor to the lookup table (*it)->argmaxInPlace(&result); } diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index 3b0a5770d..f077a13d9 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -37,9 +37,41 @@ class DiscreteBayesNet; * Inherits from discrete conditional for convenience, but is not normalized. * Is used in the max-product algorithm. */ -// Typedef for backwards compatibility -// TODO(Varun): Remove -using DiscreteLookupTable = DiscreteConditional; +class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { + public: + using This = DiscreteLookupTable; + using shared_ptr = std::shared_ptr; + using BaseConditional = Conditional; + + /** + * @brief Construct a new Discrete Lookup Table object + * + * @param nFrontals number of frontal variables + * @param keys a sorted list of gtsam::Keys + * @param potentials the algebraic decision tree with lookup values + */ + DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys, + const ADT& potentials) + : DiscreteConditional(nFrontals, keys, potentials) {} + + /// GTSAM-style print + void print( + const std::string& s = "Discrete Lookup Table: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; + + /** + * @brief return assignment for single frontal variable that maximizes value. + * @param parentsValues Known assignments for the parents. + * @return maximizing assignment for the frontal variable. + */ + size_t argmax(const DiscreteValues& parentsValues) const; + + /** + * @brief Calculate assignment for frontal variables that maximizes value. + * @param (in/out) parentsValues Known assignments for the parents. + */ + void argmaxInPlace(DiscreteValues* parentsValues) const; +}; /** A DAG made from lookup tables, as defined above. */ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet { From e0444ac722d8cb0cc692b17e07d19fc028f713d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 15 Jul 2024 18:40:07 -0400 Subject: [PATCH 34/38] Revert "remove DiscreteLookupTable from wrapper" This reverts commit ffa72e7fadfff48864c990027f9b60a9f23506e0. --- gtsam/discrete/discrete.i | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 0f34840bf..0bdebd0e1 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -262,6 +262,15 @@ class DiscreteBayesTree { #include +class DiscreteLookupTable : gtsam::DiscreteConditional{ + DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys, + const gtsam::DecisionTreeFactor::ADT& potentials); + void print(string s = "Discrete Lookup Table: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + size_t argmax(const gtsam::DiscreteValues& parentsValues) const; +}; + class DiscreteLookupDAG { DiscreteLookupDAG(); void push_back(const gtsam::DiscreteLookupTable* table); From 3d58ce56b2beab47561dc8e8981172cc182a8387 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 15 Jul 2024 18:45:15 -0400 Subject: [PATCH 35/38] small fix --- gtsam/discrete/DiscreteConditional.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 6df26d291..a7f472f26 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -244,7 +244,7 @@ size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { Key j = firstFrontalKey(); for (size_t value = 0; value < cardinality(j); value++) { values[j] = value; - double pValueS = pFS(values); // P(F=value|S=parentsValues) + double pValueS = (*this)(values); // Update MPE solution if better if (pValueS > maxP) { maxP = pValueS; From 8c693459378f5a01f0dc886cbf84bb38d8ca23f1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 21 Jul 2024 11:36:29 -0400 Subject: [PATCH 36/38] undo merge --- gtsam/discrete/DiscreteLookupDAG.cpp | 80 +++++++++++++++++++++++++++- gtsam/discrete/DiscreteLookupDAG.h | 38 +++++++++++-- gtsam/discrete/discrete.i | 9 ++++ 3 files changed, 123 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index 11900b502..4d02e007b 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -29,12 +29,90 @@ using std::vector; namespace gtsam { +/* ************************************************************************** */ +// TODO(dellaert): copy/paste from DiscreteConditional.cpp :-( +void DiscreteLookupTable::print(const std::string& s, + const KeyFormatter& formatter) const { + using std::cout; + using std::endl; + + cout << s << " g( "; + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + cout << formatter(*it) << " "; + } + if (nrParents()) { + cout << "; "; + for (const_iterator it = beginParents(); it != endParents(); ++it) { + cout << formatter(*it) << " "; + } + } + cout << "):\n"; + ADT::print("", formatter); + cout << endl; +} + +/* ************************************************************************** */ +void DiscreteLookupTable::argmaxInPlace(DiscreteValues* values) const { + ADT pFS = choose(*values, true); // P(F|S=parentsValues) + + // Initialize + DiscreteValues mpe; + double maxP = 0; + + // Get all Possible Configurations + const auto allPosbValues = frontalAssignments(); + + // Find the maximum + for (const auto& frontalVals : allPosbValues) { + double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) + // Update maximum solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = frontalVals; + } + } + + // set values (inPlace) to maximum + for (Key j : frontals()) { + (*values)[j] = mpe[j]; + } +} + +/* ************************************************************************** */ +size_t DiscreteLookupTable::argmax(const DiscreteValues& parentsValues) const { + ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) + + // Then, find the max over all remaining + // TODO(Duy): only works for one key now, seems horribly slow this way + size_t mpe = 0; + double maxP = 0; + DiscreteValues frontals; + assert(nrFrontals() == 1); + Key j = (firstFrontalKey()); + for (size_t value = 0; value < cardinality(j); value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = value; + } + } + return mpe; +} + /* ************************************************************************** */ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet( const DiscreteBayesNet& bayesNet) { DiscreteLookupDAG dag; for (auto&& conditional : bayesNet) { - dag.push_back(conditional); + if (auto lookupTable = + std::dynamic_pointer_cast(conditional)) { + dag.push_back(lookupTable); + } else { + throw std::runtime_error( + "DiscreteFactorGraph::maxProduct: Expected look up table."); + } } return dag; } diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index 3b0a5770d..f077a13d9 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -37,9 +37,41 @@ class DiscreteBayesNet; * Inherits from discrete conditional for convenience, but is not normalized. * Is used in the max-product algorithm. */ -// Typedef for backwards compatibility -// TODO(Varun): Remove -using DiscreteLookupTable = DiscreteConditional; +class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { + public: + using This = DiscreteLookupTable; + using shared_ptr = std::shared_ptr; + using BaseConditional = Conditional; + + /** + * @brief Construct a new Discrete Lookup Table object + * + * @param nFrontals number of frontal variables + * @param keys a sorted list of gtsam::Keys + * @param potentials the algebraic decision tree with lookup values + */ + DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys, + const ADT& potentials) + : DiscreteConditional(nFrontals, keys, potentials) {} + + /// GTSAM-style print + void print( + const std::string& s = "Discrete Lookup Table: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; + + /** + * @brief return assignment for single frontal variable that maximizes value. + * @param parentsValues Known assignments for the parents. + * @return maximizing assignment for the frontal variable. + */ + size_t argmax(const DiscreteValues& parentsValues) const; + + /** + * @brief Calculate assignment for frontal variables that maximizes value. + * @param (in/out) parentsValues Known assignments for the parents. + */ + void argmaxInPlace(DiscreteValues* parentsValues) const; +}; /** A DAG made from lookup tables, as defined above. */ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet { diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 0f34840bf..0bdebd0e1 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -262,6 +262,15 @@ class DiscreteBayesTree { #include +class DiscreteLookupTable : gtsam::DiscreteConditional{ + DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys, + const gtsam::DecisionTreeFactor::ADT& potentials); + void print(string s = "Discrete Lookup Table: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + size_t argmax(const gtsam::DiscreteValues& parentsValues) const; +}; + class DiscreteLookupDAG { DiscreteLookupDAG(); void push_back(const gtsam::DiscreteLookupTable* table); From 4d62b87e3536b97fb86a5994ed793ba20a07e18b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 21 Jul 2024 11:44:21 -0400 Subject: [PATCH 37/38] kill DiscreteConditional::argmaxInPlace --- gtsam/discrete/DiscreteConditional.cpp | 27 -------------------------- gtsam/discrete/DiscreteConditional.h | 6 ------ 2 files changed, 33 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index ec17e22f6..9cf19638c 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -257,33 +257,6 @@ size_t DiscreteConditional::argmax(const DiscreteValues& parentsValues) const { return maxValue; } -/* ************************************************************************** */ -void DiscreteConditional::argmaxInPlace(DiscreteValues* values) const { - ADT pFS = choose(*values, true); // P(F|S=parentsValues) - - // Initialize - DiscreteValues mpe; - double maxP = 0; - - // Get all Possible Configurations - const auto allPosbValues = frontalAssignments(); - - // Find the maximum - for (const auto& frontalVals : allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update maximum solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } - - // set values (inPlace) to maximum - for (Key j : frontals()) { - (*values)[j] = mpe[j]; - } -} - /* ************************************************************************** */ void DiscreteConditional::sampleInPlace(DiscreteValues* values) const { assert(nrFrontals() == 1); diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index eda838e91..4c7af1489 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -218,12 +218,6 @@ class GTSAM_EXPORT DiscreteConditional */ size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const; - /** - * @brief Calculate assignment for frontal variables that maximizes value. - * @param (in/out) parentsValues Known assignments for the parents. - */ - void argmaxInPlace(DiscreteValues* parentsValues) const; - /// @} /// @name Advanced Interface /// @{ From a90dbc7fc5031e519dd27aab4ebaa1cd6f4177e2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 25 Jul 2024 09:58:00 -0500 Subject: [PATCH 38/38] 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,