diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 07c80f58c..46481e305 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -52,10 +52,6 @@ HybridGaussianConditional::ConstructorHelper::ConstructorHelper( return {std::dynamic_pointer_cast(c), value}; }; pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func); - - // Build continuousKeys - continuousKeys = frontals; - continuousKeys.insert(continuousKeys.end(), parents.begin(), parents.end()); } /* *******************************************************************************/ @@ -222,8 +218,8 @@ std::shared_ptr HybridGaussianConditional::likelihood( return {likelihood_m, Cgm_Kgcm}; } }); - return std::make_shared( - continuousParentKeys, discreteParentKeys, likelihoods); + return std::make_shared(discreteParentKeys, + likelihoods); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index ff9391d67..15ef43173 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -186,7 +186,7 @@ class GTSAM_EXPORT HybridGaussianConditional private: /// Helper struct for private constructor. struct ConstructorHelper { - KeyVector frontals, parents, continuousKeys; + KeyVector frontals, parents; HybridGaussianFactor::FactorValuePairs pairs; double negLogConstant; /// Compute all variables needed for the private constructor below. @@ -198,7 +198,7 @@ class GTSAM_EXPORT HybridGaussianConditional const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals, const ConstructorHelper &helper) - : BaseFactor(helper.continuousKeys, discreteParents, helper.pairs), + : BaseFactor(discreteParents, helper.pairs), BaseConditional(helper.frontals.size()), conditionals_(conditionals), negLogConstant_(helper.negLogConstant) {} diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 952bb7017..12ec1d2a8 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -85,11 +85,6 @@ HybridGaussianFactor::HybridGaussianFactor( } } - // Check that this worked. - if (continuousKeys.empty()) { - throw std::invalid_argument("Need at least one non-null factor."); - } - // Initialize the base class Factor::keys_ = continuousKeys; Factor::keys_.push_back(discreteKey.first); @@ -116,11 +111,6 @@ HybridGaussianFactor::HybridGaussianFactor( } } - // Check that this worked. - if (continuousKeys.empty()) { - throw std::invalid_argument("Need at least one non-null factor."); - } - // Initialize the base class Factor::keys_ = continuousKeys; Factor::keys_.push_back(discreteKey.first); @@ -150,11 +140,6 @@ HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys, } }); - // Check that this worked. - if (continuousKeys.empty()) { - throw std::invalid_argument("Need at least one non-null factor."); - } - // Initialize the base class Factor::keys_ = continuousKeys; for (const auto &discreteKey : discreteKeys) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 540ee446b..b519b3a0a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -346,7 +346,6 @@ static std::shared_ptr createDiscreteFactor( // for conditional constants. static std::shared_ptr createHybridGaussianFactor( const DecisionTree &eliminationResults, - const KeyVector &continuousSeparator, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional auto correct = [&](const Result &pair) -> GaussianFactorValuePair { @@ -364,8 +363,7 @@ static std::shared_ptr createHybridGaussianFactor( DecisionTree newFactors(eliminationResults, correct); - return std::make_shared(continuousSeparator, - discreteSeparator, newFactors); + return std::make_shared(discreteSeparator, newFactors); } static std::pair> @@ -407,8 +405,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, auto newFactor = continuousSeparator.empty() ? createDiscreteFactor(eliminationResults, discreteSeparator) - : createHybridGaussianFactor(eliminationResults, continuousSeparator, - discreteSeparator); + : createHybridGaussianFactor(eliminationResults, discreteSeparator); // Create the HybridGaussianConditional from the conditionals HybridGaussianConditional::Conditionals conditionals( diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index f5be6ded8..301d79593 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -169,7 +169,7 @@ std::shared_ptr HybridNonlinearFactor::linearize( DecisionTree> linearized_factors(factors_, linearizeDT); - return std::make_shared(continuousKeys_, discreteKeys_, + return std::make_shared(discreteKeys_, linearized_factors); } diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 8836d04c8..9144e3b6d 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -65,7 +65,7 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Z_3x1)); components.emplace_back( new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Vector3::Ones())); - hfg.add(HybridGaussianFactor({x(t), x(t + 1)}, {m(t), 2}, components)); + hfg.add(HybridGaussianFactor({m(t), 2}, components)); if (t > 1) { hfg.add(DecisionTreeFactor({{m(t - 1), 2}, {m(t), 2}}, "0 1 1 3")); diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index cde7e4063..743b98c66 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -101,6 +101,12 @@ TEST(GaussianMixture, GaussianMixtureModel) { auto hbn = GaussianMixtureModel(mu0, mu1, sigma, sigma); + // Check the number of keys matches what we expect + auto hgc = hbn.at(0)->asHybrid(); + EXPECT_LONGS_EQUAL(2, hgc->keys().size()); + EXPECT_LONGS_EQUAL(1, hgc->continuousKeys().size()); + EXPECT_LONGS_EQUAL(1, hgc->discreteKeys().size()); + // At the halfway point between the means, we should get P(m|z)=0.5 double midway = mu1 - mu0; auto pMid = SolveHBN(hbn, midway); @@ -135,6 +141,12 @@ TEST(GaussianMixture, GaussianMixtureModel2) { auto hbn = GaussianMixtureModel(mu0, mu1, sigma0, sigma1); + // Check the number of keys matches what we expect + auto hgc = hbn.at(0)->asHybrid(); + EXPECT_LONGS_EQUAL(2, hgc->keys().size()); + EXPECT_LONGS_EQUAL(1, hgc->continuousKeys().size()); + EXPECT_LONGS_EQUAL(1, hgc->discreteKeys().size()); + // We get zMax=3.1333 by finding the maximum value of the function, at which // point the mode m==1 is about twice as probable as m==0. double zMax = 3.133; diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 6c69607a4..2a20f5f5e 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -566,8 +566,8 @@ std::shared_ptr mixedVarianceFactor( [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair { return {gf, 0.0}; }); - return std::make_shared( - gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs); + return std::make_shared(gmf->discreteKeys(), + updated_pairs); } /****************************************************************************/ diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index e4d207af3..c6020de85 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -55,7 +55,7 @@ TEST(HybridFactorGraph, Keys) { std::vector components{ std::make_shared(X(1), I_3x3, Z_3x1), std::make_shared(X(1), I_3x3, Vector3::Ones())}; - hfg.add(HybridGaussianFactor({X(1)}, m1, components)); + hfg.add(HybridGaussianFactor(m1, components)); KeySet expected_continuous{X(0), X(1)}; EXPECT( diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 5df9f4811..080185d88 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -70,14 +70,14 @@ auto f11 = std::make_shared(X(1), A1, X(2), A2, b); // Test simple to complex constructors... TEST(HybridGaussianFactor, ConstructorVariants) { using namespace test_constructor; - HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11}); + HybridGaussianFactor fromFactors(m1, {f10, f11}); std::vector pairs{{f10, 0.0}, {f11, 0.0}}; - HybridGaussianFactor fromPairs({X(1), X(2)}, m1, pairs); + HybridGaussianFactor fromPairs(m1, pairs); assert_equal(fromFactors, fromPairs); HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs); - HybridGaussianFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree); + HybridGaussianFactor fromDecisionTree({m1}, decisionTree); assert_equal(fromDecisionTree, fromPairs); } @@ -95,13 +95,12 @@ TEST(HybridGaussianFactor, Sum) { // TODO(Frank): why specify keys at all? And: keys in factor should be *all* // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? // Design review! - HybridGaussianFactor hybridFactorA({X(1), X(2)}, m1, {f10, f11}); - HybridGaussianFactor hybridFactorB({X(1), X(3)}, m2, {f20, f21, f22}); + HybridGaussianFactor hybridFactorA(m1, {f10, f11}); + HybridGaussianFactor hybridFactorB(m2, {f20, f21, f22}); - // Check that number of keys is 3 + // Check the number of keys matches what we expect EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size()); - - // Check that number of discrete keys is 1 + EXPECT_LONGS_EQUAL(2, hybridFactorA.continuousKeys().size()); EXPECT_LONGS_EQUAL(1, hybridFactorA.discreteKeys().size()); // Create sum of two hybrid factors: it will be a decision tree now on both @@ -122,7 +121,7 @@ TEST(HybridGaussianFactor, Sum) { /* ************************************************************************* */ TEST(HybridGaussianFactor, Printing) { using namespace test_constructor; - HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f10, f11}); + HybridGaussianFactor hybridFactor(m1, {f10, f11}); std::string expected = R"(HybridGaussianFactor @@ -159,10 +158,6 @@ Hybrid [x1 x2; 1]{ /* ************************************************************************* */ TEST(HybridGaussianFactor, HybridGaussianConditional) { - KeyVector keys; - keys.push_back(X(0)); - keys.push_back(X(1)); - DiscreteKeys dKeys; dKeys.emplace_back(M(0), 2); dKeys.emplace_back(M(1), 2); @@ -189,7 +184,7 @@ TEST(HybridGaussianFactor, Error) { auto f0 = std::make_shared(X(1), A01, X(2), A02, b); auto f1 = std::make_shared(X(1), A11, X(2), A12, b); - HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f0, f1}); + HybridGaussianFactor hybridFactor(m1, {f0, f1}); VectorValues continuousValues; continuousValues.insert(X(1), Vector2(0, 0)); @@ -594,7 +589,7 @@ static HybridGaussianFactorGraph CreateFactorGraph( // the underlying scalar to be log(\sqrt(|2πΣ|)) std::vector factors{{f0, model0->negLogConstant()}, {f1, model1->negLogConstant()}}; - HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors); + HybridGaussianFactor motionFactor(m1, factors); HybridGaussianFactorGraph hfg; hfg.push_back(motionFactor); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 6e817d312..e4acda387 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -132,7 +132,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a hybrid gaussian factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); auto result = hfg.eliminateSequential(); @@ -155,7 +155,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { // Add factor between x0 and x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); // Discrete probability table for c1 hfg.add(DecisionTreeFactor(m1, {2, 8})); @@ -177,7 +177,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, two::components(X(1)))); + hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(1)))); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -204,7 +204,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Hybrid factor P(x1|c1) - hfg.add(HybridGaussianFactor({X(1)}, m, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m, two::components(X(1)))); // Prior factor on c1 hfg.add(DecisionTreeFactor(m, {2, 8})); @@ -229,8 +229,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); { - hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, two::components(X(0)))); - hfg.add(HybridGaussianFactor({X(2)}, {M(1), 2}, two::components(X(2)))); + hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0)))); + hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2)))); } hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); @@ -239,8 +239,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); { - hfg.add(HybridGaussianFactor({X(3)}, {M(3), 2}, two::components(X(3)))); - hfg.add(HybridGaussianFactor({X(5)}, {M(2), 2}, two::components(X(5)))); + hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3)))); + hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5)))); } auto ordering_full = @@ -525,7 +525,7 @@ TEST(HybridGaussianFactorGraph, optimize) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor({X(1)}, c1, two::components(X(1)))); + hfg.add(HybridGaussianFactor(c1, two::components(X(1)))); auto result = hfg.eliminateSequential(); diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 6a672a3d2..e09669117 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -75,7 +75,6 @@ BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet"); /* ****************************************************************************/ // Test HybridGaussianFactor serialization. TEST(HybridSerialization, HybridGaussianFactor) { - KeyVector continuousKeys{X(0)}; DiscreteKey discreteKey{M(0), 2}; auto A = Matrix::Zero(2, 1); @@ -85,7 +84,7 @@ TEST(HybridSerialization, HybridGaussianFactor) { auto f1 = std::make_shared(X(0), A, b1); std::vector factors{f0, f1}; - const HybridGaussianFactor factor(continuousKeys, discreteKey, factors); + const HybridGaussianFactor factor(discreteKey, factors); EXPECT(equalsObj(factor)); EXPECT(equalsXML(factor));