Merge pull request #1848 from borglab/feature/simpler_constructors

Simpler HybridGaussianFoo constructors

gtsam/hybrid/HybridFactorGraph.cpp

@@ -29,8 +29,7 @@ std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
       for (const DiscreteKey& key : p->discreteKeys()) {
         keys.insert(key);
       }
-    }
+    } else if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
-    if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
       for (const DiscreteKey& key : p->discreteKeys()) {
         keys.insert(key);
       }

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -27,41 +27,59 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include <cstddef>
+
 namespace gtsam {
-HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(
+/* *******************************************************************************/
-    const HybridGaussianConditional::Conditionals &conditionals) {
+struct HybridGaussianConditional::ConstructorHelper {
-  auto func = [](const GaussianConditional::shared_ptr &conditional)
+  std::optional<size_t> nrFrontals;
-      -> GaussianFactorValuePair {
+  HybridGaussianFactor::FactorValuePairs pairs;
-    double value = 0.0;
+  double minNegLogConstant;
-    // Check if conditional is pruned
+
-    if (conditional) {
+  /// Compute all variables needed for the private constructor below.
-      // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
+  ConstructorHelper(const Conditionals &conditionals)
-      value = conditional->negLogConstant();
+      : minNegLogConstant(std::numeric_limits<double>::infinity()) {
+    auto func = [this](const GaussianConditional::shared_ptr &c)
+        -> GaussianFactorValuePair {
+      double value = 0.0;
+      if (c) {
+        if (!nrFrontals.has_value()) {
+          nrFrontals = c->nrFrontals();
+        }
+        value = c->negLogConstant();
+        minNegLogConstant = std::min(minNegLogConstant, value);
+      }
+      return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
+    };
+    pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
+    if (!nrFrontals.has_value()) {
+      throw std::runtime_error(
+          "HybridGaussianConditional: need at least one frontal variable.");
     }
-    return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
+  }
-  };
+};
-  return HybridGaussianFactor::FactorValuePairs(conditionals, func);
+
-}
+/* *******************************************************************************/
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKeys &discreteParents,
+    const HybridGaussianConditional::Conditionals &conditionals,
+    const ConstructorHelper &helper)
+    : BaseFactor(discreteParents, helper.pairs),
+      BaseConditional(*helper.nrFrontals),
+      conditionals_(conditionals),
+      negLogConstant_(helper.minNegLogConstant) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
-    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
     const HybridGaussianConditional::Conditionals &conditionals)
-    : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
+    : HybridGaussianConditional(discreteParents, conditionals,
-                 discreteParents, GetFactorValuePairs(conditionals)),
+                                ConstructorHelper(conditionals)) {}
-      BaseConditional(continuousFrontals.size()),
+
-      conditionals_(conditionals) {
+HybridGaussianConditional::HybridGaussianConditional(
-  // Calculate negLogConstant_ as the minimum of the negative-log normalizers of
+    const DiscreteKey &discreteParent,
-  // the conditionals, by visiting the decision tree:
+    const std::vector<GaussianConditional::shared_ptr> &conditionals)
-  negLogConstant_ = std::numeric_limits<double>::infinity();
+    : HybridGaussianConditional(DiscreteKeys{discreteParent},
-  conditionals_.visit(
+                                Conditionals({discreteParent}, conditionals)) {}
-      [this](const GaussianConditional::shared_ptr &conditional) {
-        if (conditional) {
-          this->negLogConstant_ =
-              std::min(this->negLogConstant_, conditional->negLogConstant());
-        }
-      });
-}
 
 /* *******************************************************************************/
 const HybridGaussianConditional::Conditionals &
@@ -69,15 +87,6 @@ HybridGaussianConditional::conditionals() const {
   return conditionals_;
 }
 
-/* *******************************************************************************/
-HybridGaussianConditional::HybridGaussianConditional(
-    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-    const DiscreteKey &discreteParent,
-    const std::vector<GaussianConditional::shared_ptr> &conditionals)
-    : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                DiscreteKeys{discreteParent},
-                                Conditionals({discreteParent}, conditionals)) {}
-
 /* *******************************************************************************/
 GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
     const {
@@ -222,8 +231,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

@@ -64,27 +64,11 @@ class GTSAM_EXPORT HybridGaussianConditional
 
  private:
   Conditionals conditionals_;  ///< a decision tree of Gaussian conditionals.
+
   ///< Negative-log of the normalization constant (log(\sqrt(|2πΣ|))).
   ///< Take advantage of the neg-log space so everything is a minimization
   double negLogConstant_;
 
-  /**
-   * @brief Convert a HybridGaussianConditional of conditionals into
-   * a DecisionTree of Gaussian factor graphs.
-   */
-  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
-
-  /**
-   * @brief Helper function to get the pruner functor.
-   *
-   * @param discreteProbs The pruned discrete probabilities.
-   * @return std::function<GaussianConditional::shared_ptr(
-   * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
-   */
-  std::function<GaussianConditional::shared_ptr(
-      const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
-  prunerFunc(const DecisionTreeFactor &discreteProbs);
-
  public:
   /// @name Constructors
   /// @{
@@ -93,37 +77,28 @@ class GTSAM_EXPORT HybridGaussianConditional
   HybridGaussianConditional() = default;
 
   /**
-   * @brief Construct a new HybridGaussianConditional object.
+   * @brief Construct from one discrete key and vector of conditionals.
+   *
+   * @param discreteParent Single discrete parent variable
+   * @param conditionals Vector of conditionals with the same size as the
+   * cardinality of the discrete parent.
+   */
+  HybridGaussianConditional(
+      const DiscreteKey &discreteParent,
+      const std::vector<GaussianConditional::shared_ptr> &conditionals);
+
+  /**
+   * @brief Construct from multiple discrete keys and conditional tree.
    *
-   * @param continuousFrontals the continuous frontals.
-   * @param continuousParents the continuous parents.
    * @param discreteParents the discrete parents. Will be placed last.
    * @param conditionals a decision tree of GaussianConditionals. The number of
    * conditionals should be C^(number of discrete parents), where C is the
    * cardinality of the DiscreteKeys in discreteParents, since the
    * discreteParents will be used as the labels in the decision tree.
    */
-  HybridGaussianConditional(const KeyVector &continuousFrontals,
+  HybridGaussianConditional(const DiscreteKeys &discreteParents,
-                            const KeyVector &continuousParents,
-                            const DiscreteKeys &discreteParents,
                             const Conditionals &conditionals);
 
-  /**
-   * @brief Make a Hybrid Gaussian Conditional from
-   * a vector of Gaussian conditionals.
-   * The DecisionTree-based constructor is preferred over this one.
-   *
-   * @param continuousFrontals The continuous frontal variables
-   * @param continuousParents The continuous parent variables
-   * @param discreteParent Single discrete parent variable
-   * @param conditionals Vector of conditionals with the same size as the
-   * cardinality of the discrete parent.
-   */
-  HybridGaussianConditional(
-      const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-      const DiscreteKey &discreteParent,
-      const std::vector<GaussianConditional::shared_ptr> &conditionals);
-
   /// @}
   /// @name Testable
   /// @{
@@ -207,6 +182,23 @@ class GTSAM_EXPORT HybridGaussianConditional
   /// @}
 
  private:
+  /// Helper struct for private constructor.
+  struct ConstructorHelper;
+
+  /// Private constructor that uses helper struct above.
+  HybridGaussianConditional(
+      const DiscreteKeys &discreteParents,
+      const HybridGaussianConditional::Conditionals &conditionals,
+      const ConstructorHelper &helper);
+
+  /// Convert to a DecisionTree of Gaussian factor graphs.
+  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
+
+  //// Get the pruner functor from pruned discrete probabilities.
+  std::function<GaussianConditional::shared_ptr(
+      const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+  prunerFunc(const DecisionTreeFactor &prunedProbabilities);
+
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;
 

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
 #include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/linear/GaussianFactor.h>
@@ -28,21 +29,12 @@
 
 namespace gtsam {
 
-/**
+/* *******************************************************************************/
- * @brief Helper function to augment the [A|b] matrices in the factor components
+HybridGaussianFactor::Factors HybridGaussianFactor::augment(
- * with the additional scalar values.
+    const FactorValuePairs &factors) {
- * This is done by storing the value in
- * the `b` vector as an additional row.
- *
- * @param factors DecisionTree of GaussianFactors and arbitrary scalars.
- * Gaussian factor in factors.
- * @return HybridGaussianFactor::Factors
- */
-static HybridGaussianFactor::Factors augment(
-    const HybridGaussianFactor::FactorValuePairs &factors) {
   // Find the minimum value so we can "proselytize" to positive values.
   // Done because we can't have sqrt of negative numbers.
-  HybridGaussianFactor::Factors gaussianFactors;
+  Factors gaussianFactors;
   AlgebraicDecisionTree<Key> valueTree;
   std::tie(gaussianFactors, valueTree) = unzip(factors);
 
@@ -73,22 +65,88 @@ static HybridGaussianFactor::Factors augment(
     return std::dynamic_pointer_cast<GaussianFactor>(
         std::make_shared<JacobianFactor>(gfg));
   };
-  return HybridGaussianFactor::Factors(factors, update);
+  return Factors(factors, update);
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
+struct HybridGaussianFactor::ConstructorHelper {
-                                           const DiscreteKeys &discreteKeys,
+  KeyVector continuousKeys;   // Continuous keys extracted from factors
+  DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
+  FactorValuePairs pairs;     // Used only if factorsTree is empty
+  Factors factorsTree;
+
+  ConstructorHelper(const DiscreteKey &discreteKey,
+                    const std::vector<GaussianFactor::shared_ptr> &factors)
+      : discreteKeys({discreteKey}) {
+    // Extract continuous keys from the first non-null factor
+    for (const auto &factor : factors) {
+      if (factor && continuousKeys.empty()) {
+        continuousKeys = factor->keys();
+        break;
+      }
+    }
+
+    // Build the DecisionTree from the factor vector
+    factorsTree = Factors(discreteKeys, factors);
+  }
+
+  ConstructorHelper(const DiscreteKey &discreteKey,
+                    const std::vector<GaussianFactorValuePair> &factorPairs)
+      : discreteKeys({discreteKey}) {
+    // Extract continuous keys from the first non-null factor
+    for (const auto &pair : factorPairs) {
+      if (pair.first && continuousKeys.empty()) {
+        continuousKeys = pair.first->keys();
+        break;
+      }
+    }
+
+    // Build the FactorValuePairs DecisionTree
+    pairs = FactorValuePairs(discreteKeys, factorPairs);
+  }
+
+  ConstructorHelper(const DiscreteKeys &discreteKeys,
+                    const FactorValuePairs &factorPairs)
+      : discreteKeys(discreteKeys) {
+    // Extract continuous keys from the first non-null factor
+    factorPairs.visit([&](const GaussianFactorValuePair &pair) {
+      if (pair.first && continuousKeys.empty()) {
+        continuousKeys = pair.first->keys();
+      }
+    });
+
+    // Build the FactorValuePairs DecisionTree
+    pairs = factorPairs;
+  }
+};
+
+/* *******************************************************************************/
+HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper)
+    : Base(helper.continuousKeys, helper.discreteKeys),
+      factors_(helper.factorsTree.empty() ? augment(helper.pairs)
+                                          : helper.factorsTree) {}
+
+HybridGaussianFactor::HybridGaussianFactor(
+    const DiscreteKey &discreteKey,
+    const std::vector<GaussianFactor::shared_ptr> &factors)
+    : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
+
+HybridGaussianFactor::HybridGaussianFactor(
+    const DiscreteKey &discreteKey,
+    const std::vector<GaussianFactorValuePair> &factorPairs)
+    : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
+
+HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
                                            const FactorValuePairs &factors)
-    : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {}
+    : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
 
 /* *******************************************************************************/
 bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
   if (e == nullptr) return false;
 
-  // This will return false if either factors_ is empty or e->factors_ is empty,
+  // This will return false if either factors_ is empty or e->factors_ is
-  // but not if both are empty or both are not empty:
+  // empty, but not if both are empty or both are not empty:
   if (factors_.empty() ^ e->factors_.empty()) return false;
 
   // Check the base and the factors:

gtsam/hybrid/HybridGaussianFactor.h

@@ -73,14 +73,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// Decision tree of Gaussian factors indexed by discrete keys.
   Factors factors_;
 
-  /**
-   * @brief Helper function to return factors and functional to create a
-   * DecisionTree of Gaussian Factor Graphs.
-   *
-   * @return GaussianFactorGraphTree
-   */
-  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
-
  public:
   /// @name Constructors
   /// @{
@@ -93,14 +85,11 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * providing the factors for each mode m as a vector of factors ϕ_m(x).
    * The value ϕ(x,m) for the factor is simply ϕ_m(x).
    *
-   * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKey The discrete key for the "mode", indexing components.
    * @param factors Vector of gaussian factors, one for each mode.
    */
-  HybridGaussianFactor(const KeyVector &continuousKeys,
+  HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const DiscreteKey &discreteKey,
+                       const std::vector<GaussianFactor::shared_ptr> &factors);
-                       const std::vector<GaussianFactor::shared_ptr> &factors)
-      : Base(continuousKeys, {discreteKey}), factors_({discreteKey}, factors) {}
 
   /**
    * @brief Construct a new HybridGaussianFactor on a single discrete key,
@@ -108,15 +97,11 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * provided as a vector of pairs (ϕ_m(x), E_m).
    * The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.
    *
-   * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKey The discrete key for the "mode", indexing components.
-   * @param factors Vector of gaussian factor-scalar pairs, one per mode.
+   * @param factorPairs Vector of gaussian factor-scalar pairs, one per mode.
    */
-  HybridGaussianFactor(const KeyVector &continuousKeys,
+  HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const DiscreteKey &discreteKey,
+                       const std::vector<GaussianFactorValuePair> &factorPairs);
-                       const std::vector<GaussianFactorValuePair> &factors)
-      : HybridGaussianFactor(continuousKeys, {discreteKey},
-                             FactorValuePairs({discreteKey}, factors)) {}
 
   /**
    * @brief Construct a new HybridGaussianFactor on a several discrete keys M,
@@ -124,12 +109,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M).
    * The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.
    *
-   * @param continuousKeys A vector of keys representing continuous variables.
    * @param discreteKeys Discrete variables and their cardinalities.
    * @param factors The decision tree of Gaussian factor/scalar pairs.
    */
-  HybridGaussianFactor(const KeyVector &continuousKeys,
+  HybridGaussianFactor(const DiscreteKeys &discreteKeys,
-                       const DiscreteKeys &discreteKeys,
                        const FactorValuePairs &factors);
 
   /// @}
@@ -185,11 +168,37 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   }
   /// @}
 
+ protected:
+  /**
+   * @brief Helper function to return factors and functional to create a
+   * DecisionTree of Gaussian Factor Graphs.
+   *
+   * @return GaussianFactorGraphTree
+   */
+  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
+
  private:
+  /**
+   * @brief Helper function to augment the [A|b] matrices in the factor
+   * components with the additional scalar values. This is done by storing the
+   * value in the `b` vector as an additional row.
+   *
+   * @param factors DecisionTree of GaussianFactors and arbitrary scalars.
+   * Gaussian factor in factors.
+   * @return HybridGaussianFactor::Factors
+   */
+  static Factors augment(const FactorValuePairs &factors);
+
   /// Helper method to compute the error of a component.
   double potentiallyPrunedComponentError(
       const sharedFactor &gf, const VectorValues &continuousValues) const;
 
+  /// Helper struct to assist private constructor below.
+  struct ConstructorHelper;
+
+  // Private constructor using ConstructorHelper above.
+  HybridGaussianFactor(const ConstructorHelper &helper);
+
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;

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,14 +363,12 @@ 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>>
 hybridElimination(const HybridGaussianFactorGraph &factors,
                   const Ordering &frontalKeys,
-                  const KeyVector &continuousSeparator,
                   const std::set<DiscreteKey> &discreteSeparatorSet) {
   // NOTE: since we use the special JunctionTree,
   // only possibility is continuous conditioned on discrete.
@@ -388,13 +385,18 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   factorGraphTree = removeEmpty(factorGraphTree);
 
   // This is the elimination method on the leaf nodes
+  bool someContinuousLeft = false;
   auto eliminate = [&](const GaussianFactorGraph &graph) -> Result {
     if (graph.empty()) {
       return {nullptr, nullptr};
     }
 
+    // Expensive elimination of product factor.
     auto result = EliminatePreferCholesky(graph, frontalKeys);
 
+    // Record whether there any continuous variables left
+    someContinuousLeft |= !result.second->empty();
+
     return result;
   };
 
@@ -405,16 +407,15 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   // error for each discrete choice. Otherwise, create a HybridGaussianFactor
   // on the separator, taking care to correct for conditional constants.
   auto newFactor =
-      continuousSeparator.empty()
+      someContinuousLeft
-          ? createDiscreteFactor(eliminationResults, discreteSeparator)
+          ? createHybridGaussianFactor(eliminationResults, discreteSeparator)
-          : createHybridGaussianFactor(eliminationResults, continuousSeparator,
+          : createDiscreteFactor(eliminationResults, discreteSeparator);
-                                       discreteSeparator);
 
   // Create the HybridGaussianConditional from the conditionals
   HybridGaussianConditional::Conditionals conditionals(
       eliminationResults, [](const Result &pair) { return pair.first; });
   auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
-      frontalKeys, continuousSeparator, discreteSeparator, conditionals);
+      discreteSeparator, conditionals);
 
   return {std::make_shared<HybridConditional>(hybridGaussian), newFactor};
 }
@@ -517,22 +518,12 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
     // Case 3: We are now in the hybrid land!
     KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end());
 
-    // Find all the keys in the set of continuous keys
+    // Find all discrete keys.
-    // which are not in the frontal keys. This is our continuous separator.
-    KeyVector continuousSeparator;
-    auto continuousKeySet = factors.continuousKeySet();
-    std::set_difference(
-        continuousKeySet.begin(), continuousKeySet.end(),
-        frontalKeysSet.begin(), frontalKeysSet.end(),
-        std::inserter(continuousSeparator, continuousSeparator.begin()));
-
-    // Similarly for the discrete separator.
     // Since we eliminate all continuous variables first,
     // the discrete separator will be *all* the discrete keys.
     std::set<DiscreteKey> discreteSeparator = factors.discreteKeys();
 
-    return hybridElimination(factors, frontalKeys, continuousSeparator,
+    return hybridElimination(factors, frontalKeys, discreteSeparator);
-                             discreteSeparator);
   }
 }
 

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -17,56 +17,78 @@
  */
 
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <memory>
 
 namespace gtsam {
 
 /* *******************************************************************************/
-static void checkKeys(const KeyVector& continuousKeys,
+struct HybridNonlinearFactor::ConstructorHelper {
-                      const std::vector<NonlinearFactorValuePair>& pairs) {
+  KeyVector continuousKeys;   // Continuous keys extracted from factors
-  KeySet factor_keys_set;
+  DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
-  for (const auto& pair : pairs) {
+  FactorValuePairs factorTree;
-    auto f = pair.first;
+
-    // Insert all factor continuous keys in the continuous keys set.
+  void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) {
-    std::copy(f->keys().begin(), f->keys().end(),
+    if (!factor) return;
-              std::inserter(factor_keys_set, factor_keys_set.end()));
+    if (continuousKeys.empty()) {
+      continuousKeys = factor->keys();
+    } else if (factor->keys() != continuousKeys) {
+      throw std::runtime_error(
+          "HybridNonlinearFactor: all factors should have the same keys!");
+    }
   }
 
-  KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
+  ConstructorHelper(const DiscreteKey& discreteKey,
-  if (continuous_keys_set != factor_keys_set) {
+                    const std::vector<NonlinearFactor::shared_ptr>& factors)
-    throw std::runtime_error(
+      : discreteKeys({discreteKey}) {
-        "HybridNonlinearFactor: The specified continuous keys and the keys in "
+    std::vector<NonlinearFactorValuePair> pairs;
-        "the factors do not match!");
+    // Extract continuous keys from the first non-null factor
+    for (const auto& factor : factors) {
+      pairs.emplace_back(factor, 0.0);
+      copyOrCheckContinuousKeys(factor);
+    }
+    factorTree = FactorValuePairs({discreteKey}, pairs);
   }
-}
+
+  ConstructorHelper(const DiscreteKey& discreteKey,
+                    const std::vector<NonlinearFactorValuePair>& pairs)
+      : discreteKeys({discreteKey}) {
+    // Extract continuous keys from the first non-null factor
+    for (const auto& pair : pairs) {
+      copyOrCheckContinuousKeys(pair.first);
+    }
+    factorTree = FactorValuePairs({discreteKey}, pairs);
+  }
+
+  ConstructorHelper(const DiscreteKeys& discreteKeys,
+                    const FactorValuePairs& factorPairs)
+      : discreteKeys(discreteKeys), factorTree(factorPairs) {
+    // Extract continuous keys from the first non-null factor
+    factorPairs.visit([&](const NonlinearFactorValuePair& pair) {
+      copyOrCheckContinuousKeys(pair.first);
+    });
+  }
+};
 
 /* *******************************************************************************/
+HybridNonlinearFactor::HybridNonlinearFactor(const ConstructorHelper& helper)
+    : Base(helper.continuousKeys, helper.discreteKeys),
+      factors_(helper.factorTree) {}
+
 HybridNonlinearFactor::HybridNonlinearFactor(
-    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+    const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactor::shared_ptr>& factors)
-    : Base(continuousKeys, {discreteKey}) {
+    : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
-  std::vector<NonlinearFactorValuePair> pairs;
-  for (auto&& f : factors) {
-    pairs.emplace_back(f, 0.0);
-  }
-  checkKeys(continuousKeys, pairs);
-  factors_ = FactorValuePairs({discreteKey}, pairs);
-}
 
-/* *******************************************************************************/
 HybridNonlinearFactor::HybridNonlinearFactor(
-    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+    const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactorValuePair>& pairs)
-    : Base(continuousKeys, {discreteKey}) {
+    : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {}
-  KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
-  checkKeys(continuousKeys, pairs);
-  factors_ = FactorValuePairs({discreteKey}, pairs);
-}
 
-/* *******************************************************************************/
+HybridNonlinearFactor::HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
-HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys,
-                                             const DiscreteKeys& discreteKeys,
                                              const FactorValuePairs& factors)
-    : Base(continuousKeys, discreteKeys), factors_(factors) {}
+    : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(
@@ -169,7 +191,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/HybridNonlinearFactor.h

@@ -90,12 +90,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * providing the factors for each mode m as a vector of factors ϕ_m(x).
    * The value ϕ(x,m) for the factor is simply ϕ_m(x).
    *
-   * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKey The discrete key for the "mode", indexing components.
    * @param factors Vector of gaussian factors, one for each mode.
    */
   HybridNonlinearFactor(
-      const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+      const DiscreteKey& discreteKey,
       const std::vector<NonlinearFactor::shared_ptr>& factors);
 
   /**
@@ -104,12 +103,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * provided as a vector of pairs (ϕ_m(x), E_m).
    * The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.
    *
-   * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKey The discrete key for the "mode", indexing components.
    * @param pairs Vector of gaussian factor-scalar pairs, one per mode.
    */
-  HybridNonlinearFactor(const KeyVector& continuousKeys,
+  HybridNonlinearFactor(const DiscreteKey& discreteKey,
-                        const DiscreteKey& discreteKey,
                         const std::vector<NonlinearFactorValuePair>& pairs);
 
   /**
@@ -118,13 +115,12 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M).
    * The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.
    *
-   * @param continuousKeys A vector of keys representing continuous variables.
    * @param discreteKeys Discrete variables and their cardinalities.
    * @param factors The decision tree of nonlinear factor/scalar pairs.
    */
-  HybridNonlinearFactor(const KeyVector& continuousKeys,
+  HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
-                        const DiscreteKeys& discreteKeys,
                         const FactorValuePairs& factors);
+
   /**
    * @brief Compute error of the HybridNonlinearFactor as a tree.
    *
@@ -181,6 +177,13 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
   /// Linearize all the continuous factors to get a HybridGaussianFactor.
   std::shared_ptr<HybridGaussianFactor> linearize(
       const Values& continuousValues) const;
+
+ private:
+  /// Helper struct to assist private constructor below.
+  struct ConstructorHelper;
+
+  // Private constructor using ConstructorHelper above.
+  HybridNonlinearFactor(const ConstructorHelper& helper);
 };
 
 // traits

gtsam/hybrid/hybrid.i

@@ -75,10 +75,12 @@ virtual class HybridConditional {
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 class HybridGaussianFactor : gtsam::HybridFactor {
   HybridGaussianFactor(
-      const gtsam::KeyVector& continuousKeys,
+      const gtsam::DiscreteKey& discreteKey,
+      const std::vector<gtsam::GaussianFactor::shared_ptr>& factors);
+  HybridGaussianFactor(
       const gtsam::DiscreteKey& discreteKey,
       const std::vector<std::pair<gtsam::GaussianFactor::shared_ptr, double>>&
-          factorsList);
+          factorPairs);
 
   void print(string s = "HybridGaussianFactor\n",
              const gtsam::KeyFormatter& keyFormatter =
@@ -88,13 +90,9 @@ class HybridGaussianFactor : gtsam::HybridFactor {
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 class HybridGaussianConditional : gtsam::HybridFactor {
   HybridGaussianConditional(
-      const gtsam::KeyVector& continuousFrontals,
-      const gtsam::KeyVector& continuousParents,
       const gtsam::DiscreteKeys& discreteParents,
       const gtsam::HybridGaussianConditional::Conditionals& conditionals);
   HybridGaussianConditional(
-      const gtsam::KeyVector& continuousFrontals,
-      const gtsam::KeyVector& continuousParents,
       const gtsam::DiscreteKey& discreteParent,
       const std::vector<gtsam::GaussianConditional::shared_ptr>& conditionals);
 
@@ -246,15 +244,15 @@ class HybridNonlinearFactorGraph {
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 class HybridNonlinearFactor : gtsam::HybridFactor {
   HybridNonlinearFactor(
-      const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
+      const gtsam::DiscreteKey& discreteKey,
       const std::vector<gtsam::NonlinearFactor*>& factors);
 
   HybridNonlinearFactor(
-      const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
+      const gtsam::DiscreteKey& discreteKey,
       const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors);
 
   HybridNonlinearFactor(
-      const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
+      const gtsam::DiscreteKeys& discreteKeys,
       const gtsam::DecisionTree<
           gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& 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"));
@@ -159,9 +159,8 @@ struct Switching {
 
     // Add "motion models".
     for (size_t k = 0; k < K - 1; k++) {
-      KeyVector keys = {X(k), X(k + 1)};
       auto motion_models = motionModels(k, between_sigma);
-      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
+      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(modes[k],
                                                                  motion_models);
     }
 

gtsam/hybrid/tests/TinyHybridExample.h

@@ -46,8 +46,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
     std::vector<GaussianConditional::shared_ptr> conditionals{
         GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
         GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
-    bayesNet.emplace_shared<HybridGaussianConditional>(
+    bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, conditionals);
-        KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals);
   }
 
   // Create prior on X(0).

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -43,9 +43,6 @@ const DiscreteValues m1Assignment{{M(0), 1}};
 DiscreteConditional::shared_ptr mixing =
     std::make_shared<DiscreteConditional>(m, "60/40");
 
-// define Continuous keys
-const KeyVector continuousKeys{Z(0)};
-
 /**
  * Create a simple Gaussian Mixture Model represented as p(z|m)P(m)
  * where m is a discrete variable and z is a continuous variable.
@@ -61,8 +58,7 @@ HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0,
                                                   model0),
        c1 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1,
                                                   model1);
-  hbn.emplace_shared<HybridGaussianConditional>(continuousKeys, KeyVector{}, m,
+  hbn.emplace_shared<HybridGaussianConditional>(m, std::vector{c0, c1});
-                                                std::vector{c0, c1});
   hbn.push_back(mixing);
   return hbn;
 }

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -108,8 +108,7 @@ TEST(HybridBayesNet, evaluateHybrid) {
   HybridBayesNet bayesNet;
   bayesNet.push_back(continuousConditional);
   bayesNet.emplace_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{}, Asia,
+      Asia, std::vector{conditional0, conditional1});
-      std::vector{conditional0, conditional1});
   bayesNet.emplace_shared<DiscreteConditional>(Asia, "99/1");
 
   // Create values at which to evaluate.
@@ -169,8 +168,7 @@ TEST(HybridBayesNet, Error) {
                  X(1), Vector1::Constant(2), I_1x1, model1);
 
   auto gm = std::make_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{}, Asia,
+      Asia, std::vector{conditional0, conditional1});
-      std::vector{conditional0, conditional1});
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
   bayesNet.push_back(continuousConditional);
@@ -390,7 +388,7 @@ TEST(HybridBayesNet, Sampling) {
   auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(
-      KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2),
+      DiscreteKey(M(0), 2),
       std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
 
   DiscreteKey mode(M(0), 2);

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -437,8 +437,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
                                                          one_motion};
-  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
+  nfg.emplace_shared<HybridNonlinearFactor>(m, components);
-                                            components);
 
   return nfg;
 }
@@ -566,8 +565,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);
 }
 
 /****************************************************************************/
@@ -591,9 +590,7 @@ TEST(HybridEstimation, ModeSelection) {
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
   std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
 
-  KeyVector keys = {X(0), X(1)};
+  HybridNonlinearFactor mf({M(0), 2}, components);
-  DiscreteKey modes(M(0), 2);
-  HybridNonlinearFactor mf(keys, modes, components);
 
   initial.insert(X(0), 0.0);
   initial.insert(X(1), 0.0);
@@ -623,10 +620,7 @@ TEST(HybridEstimation, ModeSelection) {
                                                Z_1x1, noise_loose),
       GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
                                                Z_1x1, noise_tight)};
-  bn.emplace_shared<HybridGaussianConditional>(
+  bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
-      KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
-      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
-                                              conditionals));
 
   VectorValues vv;
   vv.insert(Z(0), Z_1x1);
@@ -658,10 +652,7 @@ TEST(HybridEstimation, ModeSelection2) {
                                                Z_3x1, noise_loose),
       GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
                                                Z_3x1, noise_tight)};
-  bn.emplace_shared<HybridGaussianConditional>(
+  bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
-      KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
-      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
-                                              conditionals));
 
   VectorValues vv;
   vv.insert(Z(0), Z_3x1);
@@ -687,9 +678,7 @@ TEST(HybridEstimation, ModeSelection2) {
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
   std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
 
-  KeyVector keys = {X(0), X(1)};
+  HybridNonlinearFactor mf({M(0), 2}, components);
-  DiscreteKey modes(M(0), 2);
-  HybridNonlinearFactor mf(keys, modes, components);
 
   initial.insert<Vector3>(X(0), Z_3x1);
   initial.insert<Vector3>(X(1), Z_3x1);

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/testHybridGaussianConditional.cpp

@@ -52,8 +52,7 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
                                              commonSigma),
     GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
                                              commonSigma)};
-const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode,
+const HybridGaussianConditional hybrid_conditional(mode, conditionals);
-                                                   conditionals);
 }  // namespace equal_constants
 
 /* ************************************************************************* */
@@ -158,8 +157,7 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
                                              0.5),
     GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
                                              3.0)};
-const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode,
+const HybridGaussianConditional hybrid_conditional(mode, conditionals);
-                                                   conditionals);
 }  // namespace mode_dependent_constants
 
 /* ************************************************************************* */

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,17 +158,13 @@ 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);
 
   auto gaussians = std::make_shared<GaussianConditional>();
   HybridGaussianConditional::Conditionals conditionals(gaussians);
-  HybridGaussianConditional gm({}, keys, dKeys, conditionals);
+  HybridGaussianConditional gm(dKeys, conditionals);
 
   EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
 }
@@ -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));
@@ -234,9 +229,8 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
                                              -I_1x1, model1);
   DiscreteKeys discreteParents{m1};
   return std::make_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{X(0)}, discreteParents,
+      discreteParents, HybridGaussianConditional::Conditionals(
-      HybridGaussianConditional::Conditionals(discreteParents,
+                           discreteParents, std::vector{c0, c1}));
-                                              std::vector{c0, c1}));
 }
 
 /// Create two state Bayes network with 1 or two measurement models
@@ -595,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

@@ -72,13 +72,10 @@ TEST(HybridGaussianFactorGraph, Creation) {
   // Define a hybrid gaussian conditional P(x0|x1, c0)
   // and add it to the factor graph.
   HybridGaussianConditional gm(
-      {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}),
+      {M(0), 2},
-      HybridGaussianConditional::Conditionals(
+      {std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3),
-          M(0),
+       std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3, X(1),
-          std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
+                                             I_3x3)});
-                                                I_3x3),
-          std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
-                                                X(1), I_3x3)));
   hfg.add(gm);
 
   EXPECT_LONGS_EQUAL(2, hfg.size());
@@ -135,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();
 
@@ -158,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}));
@@ -180,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
@@ -207,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}));
 
@@ -232,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"));
@@ -242,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 =
@@ -528,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();
 
@@ -654,11 +651,7 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) {
                                              x0, -I_1x1, model0),
        c1 = make_shared<GaussianConditional>(f01, Vector1(mu), I_1x1, x1, I_1x1,
                                              x0, -I_1x1, model1);
-  DiscreteKeys discreteParents{m1};
+  hbn.emplace_shared<HybridGaussianConditional>(m1, std::vector{c0, c1});
-  hbn.emplace_shared<HybridGaussianConditional>(
-      KeyVector{f01}, KeyVector{x0, x1}, discreteParents,
-      HybridGaussianConditional::Conditionals(discreteParents,
-                                              std::vector{c0, c1}));
 
   // Discrete uniform prior.
   hbn.emplace_shared<DiscreteConditional>(m1, "0.5/0.5");
@@ -830,11 +823,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
                  X(0), Vector1(14.1421), I_1x1 * 2.82843),
              conditional1 = std::make_shared<GaussianConditional>(
                  X(0), Vector1(10.1379), I_1x1 * 2.02759);
-  DiscreteKeys discreteParents{mode};
   expectedBayesNet.emplace_shared<HybridGaussianConditional>(
-      KeyVector{X(0)}, KeyVector{}, discreteParents,
+      mode, std::vector{conditional0, conditional1});
-      HybridGaussianConditional::Conditionals(
-          discreteParents, std::vector{conditional0, conditional1}));
 
   // Add prior on mode.
   expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "74/26");
@@ -860,10 +850,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
   std::vector<GaussianConditional::shared_ptr> conditionals{
       GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3),
       GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)};
-  auto gm = std::make_shared<HybridGaussianConditional>(
+  auto gm = std::make_shared<HybridGaussianConditional>(mode, conditionals);
-      KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode},
-      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
-                                              conditionals));
   bn.push_back(gm);
 
   // Create prior on X(0).
@@ -891,9 +878,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
              conditional1 = std::make_shared<GaussianConditional>(
                  X(0), Vector1(14.1421), I_1x1 * 2.82843);
   expectedBayesNet.emplace_shared<HybridGaussianConditional>(
-      KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode},
+      mode, std::vector{conditional0, conditional1});
-      HybridGaussianConditional::Conditionals(
-          DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
 
   // Add prior on mode.
   expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "1/1");
@@ -929,9 +914,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
              conditional1 = std::make_shared<GaussianConditional>(
                  X(0), Vector1(10.274), I_1x1 * 2.0548);
   expectedBayesNet.emplace_shared<HybridGaussianConditional>(
-      KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode},
+      mode, std::vector{conditional0, conditional1});
-      HybridGaussianConditional::Conditionals(
-          DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
 
   // Add prior on mode.
   expectedBayesNet.emplace_shared<DiscreteConditional>(mode, "23/77");
@@ -980,10 +963,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
         GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5),
         GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1,
                                                  3.0)};
-    bn.emplace_shared<HybridGaussianConditional>(
+    bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, conditionals);
-        KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t},
-        HybridGaussianConditional::Conditionals(DiscreteKeys{noise_mode_t},
-                                                conditionals));
 
     // Create prior on discrete mode N(t):
     bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80");
@@ -999,10 +979,8 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
                                                  0.2),
         GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1,
                                                  0.2)};
-    auto gm = std::make_shared<HybridGaussianConditional>(
+    auto gm = std::make_shared<HybridGaussianConditional>(motion_model_t,
-        KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t},
+                                                          conditionals);
-        HybridGaussianConditional::Conditionals(DiscreteKeys{motion_model_t},
-                                                conditionals));
     bn.push_back(gm);
 
     // Create prior on motion model M(t):

gtsam/hybrid/tests/testHybridGaussianISAM.cpp

@@ -414,15 +414,13 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   // Add odometry factor with discrete modes.
   Pose2 odometry(1.0, 0.0, 0.0);
-  KeyVector contKeys = {W(0), W(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
   std::vector<NonlinearFactor::shared_ptr> components;
   components.emplace_back(
       new PlanarMotionModel(W(0), W(1), odometry, noise_model));  // moving
   components.emplace_back(
       new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model));  // still
-  fg.emplace_shared<HybridNonlinearFactor>(
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@@ -454,14 +452,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   /*************** Run Round 3 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(1), W(2)};
   components.clear();
   components.emplace_back(
       new PlanarMotionModel(W(1), W(2), odometry, noise_model));  // moving
   components.emplace_back(
       new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model));  // still
-  fg.emplace_shared<HybridNonlinearFactor>(
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
-      contKeys, gtsam::DiscreteKey(M(2), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@@ -496,14 +492,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   /*************** Run Round 4 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(2), W(3)};
   components.clear();
   components.emplace_back(
       new PlanarMotionModel(W(2), W(3), odometry, noise_model));  // moving
   components.emplace_back(
       new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model));  // still
-  fg.emplace_shared<HybridNonlinearFactor>(
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
-      contKeys, gtsam::DiscreteKey(M(3), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

gtsam/hybrid/tests/testHybridNonlinearFactor.cpp

@@ -60,14 +60,14 @@ auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
 // Test simple to complex constructors...
 TEST(HybridGaussianFactor, ConstructorVariants) {
   using namespace test_constructor;
-  HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1});
+  HybridNonlinearFactor fromFactors(m1, {f0, f1});
 
   std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
-  HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs);
+  HybridNonlinearFactor fromPairs(m1, pairs);
   assert_equal(fromFactors, fromPairs);
 
   HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
-  HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
+  HybridNonlinearFactor fromDecisionTree({m1}, decisionTree);
   assert_equal(fromDecisionTree, fromPairs);
 }
 
@@ -75,7 +75,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) {
 // Test .print() output.
 TEST(HybridNonlinearFactor, Printing) {
   using namespace test_constructor;
-  HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1});
+  HybridNonlinearFactor hybridFactor({m1}, {f0, f1});
 
   std::string expected =
       R"(Hybrid [x1 x2; 1]
@@ -101,7 +101,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
   auto f1 =
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
-  return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1});
+  return HybridNonlinearFactor(m1, {f0, f1});
 }
 
 /* ************************************************************************* */

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -117,7 +117,6 @@ TEST(HybridNonlinearFactorGraph, Resize) {
 
 /***************************************************************************/
 namespace test_motion {
-KeyVector contKeys = {X(0), X(1)};
 gtsam::DiscreteKey m1(M(1), 2);
 auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
 std::vector<NonlinearFactor::shared_ptr> components = {
@@ -139,8 +138,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
   auto discreteFactor = std::make_shared<DecisionTreeFactor>();
   hnfg.push_back(discreteFactor);
 
-  auto dcFactor =
+  auto dcFactor = std::make_shared<HybridNonlinearFactor>(m1, components);
-      std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
   hnfg.push_back(dcFactor);
 
   Values linearizationPoint;
@@ -156,26 +154,6 @@ TEST(HybridGaussianFactorGraph, Resize) {
   EXPECT_LONGS_EQUAL(gfg.size(), 0);
 }
 
-/***************************************************************************
- * Test that the HybridNonlinearFactor reports correctly if the number of
- * continuous keys provided do not match the keys in the factors.
- */
-TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
-  using namespace test_motion;
-
-  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
-      X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
-  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
-
-  // Check for exception when number of continuous keys are under-specified.
-  THROWS_EXCEPTION(
-      std::make_shared<HybridNonlinearFactor>(KeyVector{X(0)}, m1, components));
-
-  // Check for exception when number of continuous keys are too many.
-  THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
-      KeyVector{X(0), X(1), X(2)}, m1, components));
-}
-
 /*****************************************************************************
  * Test push_back on HFG makes the correct distinction.
  */
@@ -828,14 +806,12 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
 
   // Add odometry factor
   Pose2 odometry(2.0, 0.0, 0.0);
-  KeyVector contKeys = {X(0), X(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
   std::vector<NonlinearFactor::shared_ptr> motion_models = {
       std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
                                           noise_model),
       std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
-  fg.emplace_shared<HybridNonlinearFactor>(
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey{M(1), 2}, motion_models);
-      contKeys, gtsam::DiscreteKey(M(1), 2), motion_models);
 
   // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
   // create a noise model for the landmark measurements
@@ -901,7 +877,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
   std::vector<NonlinearFactorValuePair> factors{{f0, model0->negLogConstant()},
                                                 {f1, model1->negLogConstant()}};
 
-  HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);
+  HybridNonlinearFactor mixtureFactor(m1, factors);
 
   HybridNonlinearFactorGraph hfg;
   hfg.push_back(mixtureFactor);

gtsam/hybrid/tests/testHybridNonlinearISAM.cpp

@@ -433,15 +433,13 @@ TEST(HybridNonlinearISAM, NonTrivial) {
   /*************** Run Round 2 ***************/
   // Add odometry factor with discrete modes.
   Pose2 odometry(1.0, 0.0, 0.0);
-  KeyVector contKeys = {W(0), W(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
   auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
                                                     noise_model);
   std::vector<NonlinearFactor::shared_ptr> components{moving, still};
-  fg.emplace_shared<HybridNonlinearFactor>(
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@@ -473,14 +471,12 @@ TEST(HybridNonlinearISAM, NonTrivial) {
 
   /*************** Run Round 3 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(1), W(2)};
   still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
   components = {moving, still};
-  fg.emplace_shared<HybridNonlinearFactor>(
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
-      contKeys, gtsam::DiscreteKey(M(2), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@@ -515,14 +511,12 @@ TEST(HybridNonlinearISAM, NonTrivial) {
 
   /*************** Run Round 4 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(2), W(3)};
   still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
   components = {moving, still};
-  fg.emplace_shared<HybridNonlinearFactor>(
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
-      contKeys, gtsam::DiscreteKey(M(3), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

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));
@@ -115,9 +114,7 @@ TEST(HybridSerialization, HybridGaussianConditional) {
       GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5));
   const auto conditional1 = std::make_shared<GaussianConditional>(
       GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3));
-  const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode},
+  const HybridGaussianConditional gm(mode, {conditional0, conditional1});
-                                     HybridGaussianConditional::Conditionals(
-                                         {mode}, {conditional0, conditional1}));
 
   EXPECT(equalsObj<HybridGaussianConditional>(gm));
   EXPECT(equalsXML<HybridGaussianConditional>(gm));

python/gtsam/tests/test_HybridBayesNet.py

@@ -17,7 +17,7 @@ import numpy as np
 from gtsam.symbol_shorthand import A, X
 from gtsam.utils.test_case import GtsamTestCase
 
-from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues,
+from gtsam import (DiscreteConditional, DiscreteValues,
                    GaussianConditional, HybridBayesNet,
                    HybridGaussianConditional, HybridValues, VectorValues,
                    noiseModel)
@@ -48,8 +48,7 @@ class TestHybridBayesNet(GtsamTestCase):
         bayesNet = HybridBayesNet()
         bayesNet.push_back(conditional)
         bayesNet.push_back(
-            HybridGaussianConditional([X(1)], [], Asia,
+            HybridGaussianConditional(Asia, [conditional0, conditional1]))
-                                      [conditional0, conditional1]))
         bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
 
         # Create values at which to evaluate.

python/gtsam/tests/test_HybridFactorGraph.py

@@ -17,7 +17,7 @@ from gtsam.symbol_shorthand import C, M, X, Z
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
+from gtsam import (DiscreteConditional, GaussianConditional,
                    HybridBayesNet, HybridGaussianConditional,
                    HybridGaussianFactor, HybridGaussianFactorGraph,
                    HybridValues, JacobianFactor, noiseModel)
@@ -35,7 +35,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
         jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
 
-        gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
+        gmf = HybridGaussianFactor((C(0), 2), [(jf1, 0), (jf2, 0)])
 
         hfg = HybridGaussianFactorGraph()
         hfg.push_back(jf1)
@@ -60,7 +60,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
         jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
 
-        gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
+        gmf = HybridGaussianFactor((C(0), 2), [(jf1, 0), (jf2, 0)])
 
         hfg = HybridGaussianFactorGraph()
         hfg.push_back(jf1)
@@ -102,8 +102,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
                                                                  X(0), [0],
                                                                  sigma=3)
             bayesNet.push_back(
-                HybridGaussianConditional([Z(i)], [X(0)], mode,
+                HybridGaussianConditional(mode, [conditional0, conditional1]))
-                                          [conditional0, conditional1]))
 
         # Create prior on X(0).
         prior_on_x0 = GaussianConditional.FromMeanAndStddev(

python/gtsam/tests/test_HybridNonlinearFactorGraph.py

@@ -14,8 +14,6 @@ from __future__ import print_function
 
 import unittest
 
-import numpy as np
-from gtsam.symbol_shorthand import C, X
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
@@ -38,8 +36,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
                                       noiseModel.Unit.Create(3)), 0.0),
                    (PriorFactorPoint3(1, Point3(1, 2, 1),
                                       noiseModel.Unit.Create(3)), 0.0)]
-        nlfg.push_back(gtsam.HybridNonlinearFactor([1], (10, 2), factors))
+        mode = (10, 2)
-        nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3"))
+        nlfg.push_back(gtsam.HybridNonlinearFactor(mode, factors))
+        nlfg.push_back(gtsam.DecisionTreeFactor(mode, "1 3"))
         values = gtsam.Values()
         values.insert_point3(1, Point3(0, 0, 0))
         values.insert_point3(2, Point3(2, 3, 1))