Fix all call sites

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -52,10 +52,6 @@ HybridGaussianConditional::ConstructorHelper::ConstructorHelper(
     return {std::dynamic_pointer_cast<GaussianFactor>(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<HybridGaussianFactor> HybridGaussianConditional::likelihood(
           return {likelihood_m, Cgm_Kgcm};
         }
       });
-  return std::make_shared<HybridGaussianFactor>(
+  return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
-      continuousParentKeys, discreteParentKeys, likelihoods);
+                                                likelihoods);
 }
 
 /* ************************************************************************* */

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) {}

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) {

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -346,7 +346,6 @@ static std::shared_ptr<Factor> createDiscreteFactor(
 // for conditional constants.
 static std::shared_ptr<Factor> createHybridGaussianFactor(
     const DecisionTree<Key, Result> &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<Factor> createHybridGaussianFactor(
   DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
                                                         correct);
 
-  return std::make_shared<HybridGaussianFactor>(continuousSeparator,
+  return std::make_shared<HybridGaussianFactor>(discreteSeparator, newFactors);
-                                                discreteSeparator, newFactors);
 }
 
 static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
@@ -407,8 +405,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   auto newFactor =
       continuousSeparator.empty()
           ? createDiscreteFactor(eliminationResults, discreteSeparator)
-          : createHybridGaussianFactor(eliminationResults, continuousSeparator,
+          : createHybridGaussianFactor(eliminationResults, discreteSeparator);
-                                       discreteSeparator);
 
   // Create the HybridGaussianConditional from the conditionals
   HybridGaussianConditional::Conditionals conditionals(

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -169,7 +169,7 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
   DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>
       linearized_factors(factors_, linearizeDT);
 
-  return std::make_shared<HybridGaussianFactor>(continuousKeys_, discreteKeys_,
+  return std::make_shared<HybridGaussianFactor>(discreteKeys_,
                                                 linearized_factors);
 }
 

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"));

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;

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -566,8 +566,8 @@ std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
       [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
         return {gf, 0.0};
       });
-  return std::make_shared<HybridGaussianFactor>(
+  return std::make_shared<HybridGaussianFactor>(gmf->discreteKeys(),
-      gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs);
+                                                updated_pairs);
 }
 
 /****************************************************************************/

gtsam/hybrid/tests/testHybridFactorGraph.cpp

@@ -55,7 +55,7 @@ TEST(HybridFactorGraph, Keys) {
   std::vector<GaussianFactor::shared_ptr> components{
       std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       std::make_shared<JacobianFactor>(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(

gtsam/hybrid/tests/testHybridGaussianFactor.cpp

@@ -70,14 +70,14 @@ auto f11 = std::make_shared<JacobianFactor>(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<GaussianFactorValuePair> 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 hybridFactorA(m1, {f10, f11});
-  HybridGaussianFactor hybridFactorB({X(1), X(3)}, m2, {f20, f21, f22});
+  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());
-
+  EXPECT_LONGS_EQUAL(2, hybridFactorA.continuousKeys().size());
-  // Check that number of discrete keys is 1
   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<JacobianFactor>(X(1), A01, X(2), A02, b);
   auto f1 = std::make_shared<JacobianFactor>(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<GaussianFactorValuePair> 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);

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({M(0), 2}, two::components(X(0))));
-    hfg.add(HybridGaussianFactor({X(2)}, {M(1), 2}, two::components(X(2))));
+    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({M(3), 2}, two::components(X(3))));
-    hfg.add(HybridGaussianFactor({X(5)}, {M(2), 2}, two::components(X(5))));
+    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();
 

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<JacobianFactor>(X(0), A, b1);
   std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
 
-  const HybridGaussianFactor factor(continuousKeys, discreteKey, factors);
+  const HybridGaussianFactor factor(discreteKey, factors);
 
   EXPECT(equalsObj<HybridGaussianFactor>(factor));
   EXPECT(equalsXML<HybridGaussianFactor>(factor));