From bc8c77c54d36e55732dc60e0349e1815b607aa15 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 10:12:02 -0400 Subject: [PATCH 1/4] rename test file to correct form --- ...ianHybridFactorGraph.cpp => testHybridGaussianFactorGraph.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/hybrid/tests/{testGaussianHybridFactorGraph.cpp => testHybridGaussianFactorGraph.cpp} (100%) diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp similarity index 100% rename from gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp rename to gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp From 8820bf272c268ae298d99d8e13fe81052af80fc2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 12:33:28 -0400 Subject: [PATCH 2/4] Add test to expose bug in elimination with gaussian conditionals --- .../tests/testHybridGaussianFactorGraph.cpp | 41 +++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 40da42412..d199d7611 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -500,6 +500,7 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { } } +/* ************************************************************************* */ TEST(HybridGaussianFactorGraph, optimize) { HybridGaussianFactorGraph hfg; @@ -521,6 +522,46 @@ TEST(HybridGaussianFactorGraph, optimize) { EXPECT(assert_equal(hv.atDiscrete(C(1)), int(0))); } + +/* ************************************************************************* */ +// Test adding of gaussian conditional and re-elimination. +TEST(HybridGaussianFactorGraph, Conditionals) { + Switching switching(4); + HybridGaussianFactorGraph hfg; + + hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) + Ordering ordering; + ordering.push_back(X(1)); + HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering); + + hfg.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1) + hfg.push_back(*bayes_net); + hfg.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2) + hfg.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + ordering.push_back(X(2)); + ordering.push_back(X(3)); + ordering.push_back(M(1)); + ordering.push_back(M(2)); + + bayes_net = hfg.eliminateSequential(ordering); + + HybridValues result = bayes_net->optimize(); + + Values expected_continuous; + expected_continuous.insert(X(1), 0); + expected_continuous.insert(X(2), 1); + expected_continuous.insert(X(3), 2); + expected_continuous.insert(X(4), 4); + Values result_continuous = + switching.linearizationPoint.retract(result.continuous()); + EXPECT(assert_equal(expected_continuous, result_continuous)); + + DiscreteValues expected_discrete; + expected_discrete[M(1)] = 1; + expected_discrete[M(2)] = 1; + EXPECT(assert_equal(expected_discrete, result.discrete())); +} + /* ************************************************************************* */ int main() { TestResult tr; From 9002b6829183fdebb1ce65eb61bb261956c64307 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 12:33:37 -0400 Subject: [PATCH 3/4] fix the bug --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ddb776ff4..0faf0e86e 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -96,8 +96,12 @@ GaussianMixtureFactor::Sum sumFrontals( } } else if (f->isContinuous()) { - deferredFactors.push_back( - boost::dynamic_pointer_cast(f)->inner()); + if (auto gf = boost::dynamic_pointer_cast(f)) { + deferredFactors.push_back(gf->inner()); + } + if (auto cg = boost::dynamic_pointer_cast(f)) { + deferredFactors.push_back(cg->asGaussian()); + } } else if (f->isDiscrete()) { // Don't do anything for discrete-only factors From 6238a1f9017017c7d21ca2708b4dab46d3d3af2b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 12:34:53 -0400 Subject: [PATCH 4/4] more docs for Switching example --- gtsam/hybrid/tests/Switching.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 8bcb26c92..3ae8f0bb1 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -115,7 +115,6 @@ inline std::pair> makeBinaryOrdering( /* *************************************************************************** */ using MotionModel = BetweenFactor; -// using MotionMixture = MixtureFactor; // Test fixture with switching network. struct Switching { @@ -125,7 +124,13 @@ struct Switching { HybridGaussianFactorGraph linearizedFactorGraph; Values linearizationPoint; - /// Create with given number of time steps. + /** + * @brief Create with given number of time steps. + * + * @param K The total number of timesteps. + * @param between_sigma The stddev between poses. + * @param prior_sigma The stddev on priors (also used for measurements). + */ Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1) : K(K) { // Create DiscreteKeys for binary K modes, modes[0] will not be used. @@ -166,6 +171,8 @@ struct Switching { linearizationPoint.insert(X(k), static_cast(k)); } + // The ground truth is robot moving forward + // and one less than the linearization point linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint); }