From 1f11b5472f957de00e614242fbc3e00c578125c3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 10:41:04 -0700
Subject: [PATCH 01/22] Initial attempt at removing extra arguments

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 79 ++++++++++++----------
 gtsam/hybrid/HybridGaussianConditional.h   |  9 +--
 gtsam/hybrid/HybridGaussianFactor.h        |  2 +-
 3 files changed, 46 insertions(+), 44 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index ef38237f2b..00ead068a2 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -28,56 +28,65 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
-HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(
-    const HybridGaussianConditional::Conditionals &conditionals) {
-  auto func = [](const GaussianConditional::shared_ptr &conditional)
-      -> GaussianFactorValuePair {
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKeys &discreteParents,
+    const HybridGaussianConditional::Conditionals &conditionals)
+    : BaseConditional(0) {  // Initialize with zero; we'll set it properly later
+
+  // Check if conditionals are empty
+  if (conditionals.empty()) {
+    throw std::invalid_argument("Conditionals cannot be empty");
+  }
+
+  KeyVector frontals, parents;
+  negLogConstant_ = std::numeric_limits<double>::infinity();
+  auto func =
+      [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair {
     double value = 0.0;
     // Check if conditional is pruned
-    if (conditional) {
+    if (c) {
+      KeyVector cf(c->frontals().begin(), c->frontals().end());
+      KeyVector cp(c->parents().begin(), c->parents().end());
+      if (frontals.empty()) {
+        // Get frontal/parent keys from first conditional.
+        frontals = cf;
+        parents = cp;
+      } else if (cf != frontals || cp != parents) {
+        throw std::invalid_argument(
+            "All conditionals must have the same frontals and parents");
+      }
       // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
-      value = conditional->negLogConstant();
+      value = c->negLogConstant();
+      this->negLogConstant_ = std::min(this->negLogConstant_, value);
     }
-    return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
+    return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
   };
-  return HybridGaussianFactor::FactorValuePairs(conditionals, func);
-}
+  BaseFactor::factors_ = HybridGaussianFactor::Factors(conditionals, func);
 
-HybridGaussianConditional::HybridGaussianConditional(
-    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-    const DiscreteKeys &discreteParents,
-    const HybridGaussianConditional::Conditionals &conditionals)
-    : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
-                 discreteParents, GetFactorValuePairs(conditionals)),
-      BaseConditional(continuousFrontals.size()),
-      conditionals_(conditionals) {
-  // Calculate negLogConstant_ as the minimum of the negative-log normalizers of
-  // the conditionals, by visiting the decision tree:
-  negLogConstant_ = std::numeric_limits<double>::infinity();
-  conditionals_.visit(
-      [this](const GaussianConditional::shared_ptr &conditional) {
-        if (conditional) {
-          this->negLogConstant_ =
-              std::min(this->negLogConstant_, conditional->negLogConstant());
-        }
-      });
-}
+  // Initialize base classes
+  KeyVector continuousKeys = frontals;
+  continuousKeys.insert(continuousKeys.end(), parents.begin(), parents.end());
+  BaseFactor::keys_ = continuousKeys;
+  BaseFactor::discreteKeys_ = discreteParents;
+  BaseConditional::nrFrontals_ = frontals.size();
 
-/* *******************************************************************************/
-const HybridGaussianConditional::Conditionals &
-HybridGaussianConditional::conditionals() const {
-  return conditionals_;
+  // Assign conditionals
+  conditionals_ = conditionals;  // TODO(frank): a duplicate of factors_ !!!
 }
 
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKey &discreteParent,
     const std::vector<GaussianConditional::shared_ptr> &conditionals)
-    : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                DiscreteKeys{discreteParent},
+    : HybridGaussianConditional(DiscreteKeys{discreteParent},
                                 Conditionals({discreteParent}, conditionals)) {}
 
+/* *******************************************************************************/
+const HybridGaussianConditional::Conditionals &
+HybridGaussianConditional::conditionals() const {
+  return conditionals_;
+}
+
 /* *******************************************************************************/
 GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
     const {
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index 9c70cf6cb9..6198f65fbe 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -95,17 +95,13 @@ class GTSAM_EXPORT HybridGaussianConditional
   /**
    * @brief Construct a new HybridGaussianConditional object.
    *
-   * @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,
-                            const KeyVector &continuousParents,
-                            const DiscreteKeys &discreteParents,
+  HybridGaussianConditional(const DiscreteKeys &discreteParents,
                             const Conditionals &conditionals);
 
   /**
@@ -113,14 +109,11 @@ class GTSAM_EXPORT HybridGaussianConditional
    * 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);
 
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 817e54e562..1aa6a02636 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -69,7 +69,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// typedef for Decision Tree of Gaussian factors.
   using Factors = DecisionTree<Key, sharedFactor>;
 
- private:
+ protected:
   /// Decision tree of Gaussian factors indexed by discrete keys.
   Factors factors_;
 

From 977112d004dc0a5bf34de17475373ddb46fd1791 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 11:28:41 -0700
Subject: [PATCH 02/22] Avoid double traversal

---
 gtsam/hybrid/HybridFactor.h                |  4 +---
 gtsam/hybrid/HybridGaussianConditional.cpp | 21 +++++++++++++--------
 gtsam/hybrid/HybridGaussianFactor.cpp      |  8 ++++----
 gtsam/hybrid/HybridGaussianFactor.h        |  3 +++
 4 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index fc91e08389..1e99c13655 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -56,11 +56,9 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// Enum to help with categorizing hybrid factors.
   enum class Category { None, Discrete, Continuous, Hybrid };
 
- private:
+ protected:
   /// Record what category of HybridFactor this is.
   Category category_ = Category::None;
-
- protected:
   // Set of DiscreteKeys for this factor.
   DiscreteKeys discreteKeys_;
   /// Record continuous keys for book-keeping
diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 00ead068a2..903542c24f 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -61,17 +61,22 @@ HybridGaussianConditional::HybridGaussianConditional(
     }
     return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
   };
-  BaseFactor::factors_ = HybridGaussianFactor::Factors(conditionals, func);
+  HybridGaussianFactor::FactorValuePairs pairs(conditionals, func);
 
-  // Initialize base classes
-  KeyVector continuousKeys = frontals;
-  continuousKeys.insert(continuousKeys.end(), parents.begin(), parents.end());
-  BaseFactor::keys_ = continuousKeys;
-  BaseFactor::discreteKeys_ = discreteParents;
+  // Adjust frontals size
   BaseConditional::nrFrontals_ = frontals.size();
 
-  // Assign conditionals
-  conditionals_ = conditionals;  // TODO(frank): a duplicate of factors_ !!!
+  // Initialize HybridFactor
+  HybridFactor::category_ = HybridFactor::Category::Hybrid;
+  HybridFactor::discreteKeys_ = discreteParents;
+  HybridFactor::keys_ = frontals;
+  keys_.insert(keys_.end(), parents.begin(), parents.end());
+
+  // Initialize BaseFactor
+  BaseFactor::factors_ = BaseFactor::augment(pairs);  // TODO(frank): expensive
+
+  // Assign local conditionals. TODO(frank): a duplicate of factors_ !!!
+  conditionals_ = conditionals;
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index d5773590bb..af2d0fde5a 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -38,11 +38,11 @@ namespace gtsam {
  * Gaussian factor in factors.
  * @return HybridGaussianFactor::Factors
  */
-static HybridGaussianFactor::Factors augment(
-    const HybridGaussianFactor::FactorValuePairs &factors) {
+HybridGaussianFactor::Factors HybridGaussianFactor::augment(
+    const 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,7 +73,7 @@ static HybridGaussianFactor::Factors augment(
     return std::dynamic_pointer_cast<GaussianFactor>(
         std::make_shared<JacobianFactor>(gfg));
   };
-  return HybridGaussianFactor::Factors(factors, update);
+  return Factors(factors, update);
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 1aa6a02636..9aa505092f 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -73,6 +73,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// Decision tree of Gaussian factors indexed by discrete keys.
   Factors factors_;
 
+  /// Helper function to "hide" the constants in the Jacobian factors.
+  static Factors augment(const FactorValuePairs &factors);
+
   /**
    * @brief Helper function to return factors and functional to create a
    * DecisionTree of Gaussian Factor Graphs.

From b45ba003cad605bd157f98b8fa88a9033aac44e7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 11:29:03 -0700
Subject: [PATCH 03/22] Simplify all call sites

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp    |  2 +-
 gtsam/hybrid/tests/TinyHybridExample.h        |  3 +-
 gtsam/hybrid/tests/testGaussianMixture.cpp    |  6 +--
 gtsam/hybrid/tests/testHybridBayesNet.cpp     |  6 +--
 gtsam/hybrid/tests/testHybridConditional.cpp  |  2 +
 gtsam/hybrid/tests/testHybridEstimation.cpp   | 10 +---
 .../tests/testHybridGaussianConditional.cpp   |  6 +--
 .../hybrid/tests/testHybridGaussianFactor.cpp |  7 ++-
 .../tests/testHybridGaussianFactorGraph.cpp   | 46 +++++--------------
 .../hybrid/tests/testSerializationHybrid.cpp  |  4 +-
 10 files changed, 27 insertions(+), 65 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 82828bb41e..540ee446b0 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -414,7 +414,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   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};
 }
diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h
index 09905bf797..f2ff7dde2c 100644
--- a/gtsam/hybrid/tests/TinyHybridExample.h
+++ b/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>(
-        KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals);
+    bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, conditionals);
   }
 
   // Create prior on X(0).
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index be4ba2ff4d..cde7e4063b 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/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,
-                                                std::vector{c0, c1});
+  hbn.emplace_shared<HybridGaussianConditional>(m, std::vector{c0, c1});
   hbn.push_back(mixing);
   return hbn;
 }
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 6e23db91e3..b049b03fd2 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/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,
-      std::vector{conditional0, conditional1});
+      Asia, 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,
-      std::vector{conditional0, conditional1});
+      Asia, std::vector{conditional0, conditional1});
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
   bayesNet.push_back(continuousConditional);
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 365dde3bc1..106ae28e5c 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -39,6 +39,8 @@ TEST(HybridConditional, Invariants) {
   const DiscreteValues d{{M(0), 1}};
   const HybridValues values{c, d};
 
+  GTSAM_PRINT(bn);
+
   // Check invariants for p(z|x,m)
   auto hc0 = bn.at(0);
   CHECK(hc0->isHybrid());
diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 4899205dfc..6c69607a44 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -623,10 +623,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>(
-      KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
-      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
-                                              conditionals));
+  bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
 
   VectorValues vv;
   vv.insert(Z(0), Z_1x1);
@@ -658,10 +655,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>(
-      KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
-      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
-                                              conditionals));
+  bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
 
   VectorValues vv;
   vv.insert(Z(0), Z_3x1);
diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
index adcf6c70fd..02163df9e4 100644
--- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
+++ b/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,
-                                                   conditionals);
+const HybridGaussianConditional hybrid_conditional(mode, 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,
-                                                   conditionals);
+const HybridGaussianConditional hybrid_conditional(mode, conditionals);
 }  // namespace mode_dependent_constants
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp
index 55c3bccfb0..5df9f4811a 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp
@@ -169,7 +169,7 @@ TEST(HybridGaussianFactor, HybridGaussianConditional) {
 
   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());
 }
@@ -234,9 +234,8 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
                                              -I_1x1, model1);
   DiscreteKeys discreteParents{m1};
   return std::make_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{X(0)}, discreteParents,
-      HybridGaussianConditional::Conditionals(discreteParents,
-                                              std::vector{c0, c1}));
+      discreteParents, HybridGaussianConditional::Conditionals(
+                           discreteParents, std::vector{c0, c1}));
 }
 
 /// Create two state Bayes network with 1 or two measurement models
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index b1c68adf3e..6e817d312b 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/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}),
-      HybridGaussianConditional::Conditionals(
-          M(0),
-          std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
-                                                I_3x3),
-          std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
-                                                X(1), I_3x3)));
+      {M(0), 2},
+      {std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), 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());
@@ -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>(
-      KeyVector{f01}, KeyVector{x0, x1}, discreteParents,
-      HybridGaussianConditional::Conditionals(discreteParents,
-                                              std::vector{c0, c1}));
+  hbn.emplace_shared<HybridGaussianConditional>(m1, 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,
-      HybridGaussianConditional::Conditionals(
-          discreteParents, std::vector{conditional0, conditional1}));
+      mode, 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>(
-      KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode},
-      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
-                                              conditionals));
+  auto gm = std::make_shared<HybridGaussianConditional>(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},
-      HybridGaussianConditional::Conditionals(
-          DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
+      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},
-      HybridGaussianConditional::Conditionals(
-          DiscreteKeys{mode}, std::vector{conditional0, conditional1}));
+      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>(
-        KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t},
-        HybridGaussianConditional::Conditionals(DiscreteKeys{noise_mode_t},
-                                                conditionals));
+    bn.emplace_shared<HybridGaussianConditional>(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>(
-        KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t},
-        HybridGaussianConditional::Conditionals(DiscreteKeys{motion_model_t},
-                                                conditionals));
+    auto gm = std::make_shared<HybridGaussianConditional>(motion_model_t,
+                                                          conditionals);
     bn.push_back(gm);
 
     // Create prior on motion model M(t):
diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp
index 4bf90019bd..6a672a3d24 100644
--- a/gtsam/hybrid/tests/testSerializationHybrid.cpp
+++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp
@@ -115,9 +115,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},
-                                     HybridGaussianConditional::Conditionals(
-                                         {mode}, {conditional0, conditional1}));
+  const HybridGaussianConditional gm(mode, {conditional0, conditional1});
 
   EXPECT(equalsObj<HybridGaussianConditional>(gm));
   EXPECT(equalsXML<HybridGaussianConditional>(gm));

From ebebf7ddd547ef968fec90e42de09efa3c6de634 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 11:43:05 -0700
Subject: [PATCH 04/22] Fix initialization issue with helper class

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 46 ++++++++--------------
 gtsam/hybrid/HybridGaussianConditional.h   | 20 ++++++++++
 2 files changed, 36 insertions(+), 30 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 903542c24f..07c80f58c8 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -28,57 +28,43 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
-HybridGaussianConditional::HybridGaussianConditional(
-    const DiscreteKeys &discreteParents,
-    const HybridGaussianConditional::Conditionals &conditionals)
-    : BaseConditional(0) {  // Initialize with zero; we'll set it properly later
-
-  // Check if conditionals are empty
-  if (conditionals.empty()) {
-    throw std::invalid_argument("Conditionals cannot be empty");
-  }
+/* *******************************************************************************/
+HybridGaussianConditional::ConstructorHelper::ConstructorHelper(
+    const HybridGaussianConditional::Conditionals &conditionals) {
+  negLogConstant = std::numeric_limits<double>::infinity();
 
-  KeyVector frontals, parents;
-  negLogConstant_ = std::numeric_limits<double>::infinity();
   auto func =
       [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair {
     double value = 0.0;
-    // Check if conditional is pruned
     if (c) {
       KeyVector cf(c->frontals().begin(), c->frontals().end());
       KeyVector cp(c->parents().begin(), c->parents().end());
       if (frontals.empty()) {
-        // Get frontal/parent keys from first conditional.
         frontals = cf;
         parents = cp;
       } else if (cf != frontals || cp != parents) {
         throw std::invalid_argument(
             "All conditionals must have the same frontals and parents");
       }
-      // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
       value = c->negLogConstant();
-      this->negLogConstant_ = std::min(this->negLogConstant_, value);
+      negLogConstant = std::min(negLogConstant, value);
     }
     return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
   };
-  HybridGaussianFactor::FactorValuePairs pairs(conditionals, func);
-
-  // Adjust frontals size
-  BaseConditional::nrFrontals_ = frontals.size();
+  pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
 
-  // Initialize HybridFactor
-  HybridFactor::category_ = HybridFactor::Category::Hybrid;
-  HybridFactor::discreteKeys_ = discreteParents;
-  HybridFactor::keys_ = frontals;
-  keys_.insert(keys_.end(), parents.begin(), parents.end());
-
-  // Initialize BaseFactor
-  BaseFactor::factors_ = BaseFactor::augment(pairs);  // TODO(frank): expensive
-
-  // Assign local conditionals. TODO(frank): a duplicate of factors_ !!!
-  conditionals_ = conditionals;
+  // Build continuousKeys
+  continuousKeys = frontals;
+  continuousKeys.insert(continuousKeys.end(), parents.begin(), parents.end());
 }
 
+/* *******************************************************************************/
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKeys &discreteParents,
+    const HybridGaussianConditional::Conditionals &conditionals)
+    : HybridGaussianConditional(discreteParents, conditionals,
+                                ConstructorHelper(conditionals)) {}
+
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKey &discreteParent,
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index 6198f65fbe..c857ec1620 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -68,6 +68,26 @@ class GTSAM_EXPORT HybridGaussianConditional
   ///< Take advantage of the neg-log space so everything is a minimization
   double negLogConstant_;
 
+  /// Helper struct for private constructor.
+  struct ConstructorHelper {
+    KeyVector frontals;
+    KeyVector parents;
+    KeyVector continuousKeys;
+    HybridGaussianFactor::FactorValuePairs pairs;
+    double negLogConstant;
+    ConstructorHelper(const Conditionals &conditionals);
+  };
+
+  /// Private constructor
+  HybridGaussianConditional(
+      const DiscreteKeys &discreteParents,
+      const HybridGaussianConditional::Conditionals &conditionals,
+      const ConstructorHelper &helper)
+      : BaseFactor(helper.continuousKeys, discreteParents, helper.pairs),
+        BaseConditional(helper.frontals.size()),
+        conditionals_(conditionals),
+        negLogConstant_(helper.negLogConstant) {}
+
   /**
    * @brief Convert a HybridGaussianConditional of conditionals into
    * a DecisionTree of Gaussian factor graphs.

From e04f0afd0e76a4ccb335a09ae9ea9ec2b2f7a7b4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 11:45:09 -0700
Subject: [PATCH 05/22] De-clutter header

---
 gtsam/hybrid/HybridGaussianConditional.h     | 65 +++++++++-----------
 gtsam/hybrid/tests/testHybridConditional.cpp |  2 -
 2 files changed, 28 insertions(+), 39 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index c857ec1620..ff9391d678 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -64,47 +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_;
 
-  /// Helper struct for private constructor.
-  struct ConstructorHelper {
-    KeyVector frontals;
-    KeyVector parents;
-    KeyVector continuousKeys;
-    HybridGaussianFactor::FactorValuePairs pairs;
-    double negLogConstant;
-    ConstructorHelper(const Conditionals &conditionals);
-  };
-
-  /// Private constructor
-  HybridGaussianConditional(
-      const DiscreteKeys &discreteParents,
-      const HybridGaussianConditional::Conditionals &conditionals,
-      const ConstructorHelper &helper)
-      : BaseFactor(helper.continuousKeys, discreteParents, helper.pairs),
-        BaseConditional(helper.frontals.size()),
-        conditionals_(conditionals),
-        negLogConstant_(helper.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
   /// @{
@@ -220,6 +184,33 @@ class GTSAM_EXPORT HybridGaussianConditional
   /// @}
 
  private:
+  /// Helper struct for private constructor.
+  struct ConstructorHelper {
+    KeyVector frontals, parents, continuousKeys;
+    HybridGaussianFactor::FactorValuePairs pairs;
+    double negLogConstant;
+    /// Compute all variables needed for the private constructor below.
+    ConstructorHelper(const Conditionals &conditionals);
+  };
+
+  /// Private constructor that uses helper struct above.
+  HybridGaussianConditional(
+      const DiscreteKeys &discreteParents,
+      const HybridGaussianConditional::Conditionals &conditionals,
+      const ConstructorHelper &helper)
+      : BaseFactor(helper.continuousKeys, discreteParents, helper.pairs),
+        BaseConditional(helper.frontals.size()),
+        conditionals_(conditionals),
+        negLogConstant_(helper.negLogConstant) {}
+
+  /// 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;
 
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 106ae28e5c..365dde3bc1 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -39,8 +39,6 @@ TEST(HybridConditional, Invariants) {
   const DiscreteValues d{{M(0), 1}};
   const HybridValues values{c, d};
 
-  GTSAM_PRINT(bn);
-
   // Check invariants for p(z|x,m)
   auto hc0 = bn.at(0);
   CHECK(hc0->isHybrid());

From e8089dc7cb7320c0ef9dc92829581bf7b9aeca51 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 11:56:47 -0700
Subject: [PATCH 06/22] Made augment static

---
 gtsam/hybrid/HybridGaussianFactor.cpp | 6 ------
 gtsam/hybrid/HybridGaussianFactor.h   | 3 ++-
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index af2d0fde5a..3db6e7965b 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -76,12 +76,6 @@ HybridGaussianFactor::Factors HybridGaussianFactor::augment(
   return Factors(factors, update);
 }
 
-/* *******************************************************************************/
-HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
-                                           const DiscreteKeys &discreteKeys,
-                                           const FactorValuePairs &factors)
-    : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {}
-
 /* *******************************************************************************/
 bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 9aa505092f..8e36e6615e 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -133,7 +133,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    */
   HybridGaussianFactor(const KeyVector &continuousKeys,
                        const DiscreteKeys &discreteKeys,
-                       const FactorValuePairs &factors);
+                       const FactorValuePairs &factors)
+      : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {}
 
   /// @}
   /// @name Testable

From 35dd42ed2329c264f77f6b9553ca2c9ecc1446e0 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 12:53:50 -0700
Subject: [PATCH 07/22] Three constructor variants extract keys

---
 gtsam/hybrid/HybridFactor.h           |   4 +
 gtsam/hybrid/HybridGaussianFactor.cpp | 115 +++++++++++++++++++++++---
 gtsam/hybrid/HybridGaussianFactor.h   |  55 ++++++------
 3 files changed, 134 insertions(+), 40 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 1e99c13655..39a72eb261 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -140,6 +140,10 @@ class GTSAM_EXPORT HybridFactor : public Factor {
 
   /// @}
 
+ protected:
+  /// protected constructor to initialize the category
+  HybridFactor(Category category) : category_(category) {}
+
  private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 3db6e7965b..952bb7017e 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -26,18 +26,11 @@
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include "gtsam/hybrid/HybridFactor.h"
+
 namespace gtsam {
 
-/**
- * @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
- */
+/* *******************************************************************************/
 HybridGaussianFactor::Factors HybridGaussianFactor::augment(
     const FactorValuePairs &factors) {
   // Find the minimum value so we can "proselytize" to positive values.
@@ -76,13 +69,111 @@ HybridGaussianFactor::Factors HybridGaussianFactor::augment(
   return Factors(factors, update);
 }
 
+/* *******************************************************************************/
+HybridGaussianFactor::HybridGaussianFactor(
+    const DiscreteKey &discreteKey,
+    const std::vector<GaussianFactor::shared_ptr> &factors)
+    : Base(HybridFactor::Category::Hybrid) {
+  // Extract continuous keys from first-null factor and verify all others
+  KeyVector continuousKeys;
+  for (const auto &factor : factors) {
+    if (!factor) continue;
+    if (continuousKeys.empty()) {
+      continuousKeys = factor->keys();
+    } else if (factor->keys() != continuousKeys) {
+      throw std::invalid_argument("All factors must have the same keys");
+    }
+  }
+
+  // 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);
+  Base::discreteKeys_ = {discreteKey};
+  Base::continuousKeys_ = continuousKeys;
+
+  // Build the DecisionTree from factor vector
+  factors_ = Factors({discreteKey}, factors);
+}
+
+/* *******************************************************************************/
+HybridGaussianFactor::HybridGaussianFactor(
+    const DiscreteKey &discreteKey,
+    const std::vector<GaussianFactorValuePair> &factorPairs)
+    : Base(HybridFactor::Category::Hybrid) {
+  // Extract continuous keys from first-null factor and verify all others
+  KeyVector continuousKeys;
+  for (const auto &pair : factorPairs) {
+    if (!pair.first) continue;
+    if (continuousKeys.empty()) {
+      continuousKeys = pair.first->keys();
+    } else if (pair.first->keys() != continuousKeys) {
+      throw std::invalid_argument("All factors must have the same keys");
+    }
+  }
+
+  // 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);
+  Base::discreteKeys_ = {discreteKey};
+  Base::continuousKeys_ = continuousKeys;
+
+  // Build the FactorValuePairs DecisionTree
+  FactorValuePairs pairTree({discreteKey}, factorPairs);
+
+  // Assign factors_ after calling augment
+  factors_ = augment(pairTree);
+}
+
+/* *******************************************************************************/
+HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
+                                           const FactorValuePairs &factorPairs)
+    : Base(HybridFactor::Category::Hybrid) {
+  // Verify that all factors have the same keys
+  KeyVector continuousKeys;
+  factorPairs.visit([&](const GaussianFactorValuePair &pair) {
+    if (pair.first) {
+      if (continuousKeys.empty()) {
+        continuousKeys = pair.first->keys();
+      } else if (pair.first->keys() != continuousKeys) {
+        throw std::invalid_argument("All factors must have the same keys");
+      }
+    }
+  });
+
+  // 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) {
+    Factor::keys_.push_back(discreteKey.first);
+  }
+  Base::discreteKeys_ = discreteKeys;
+  Base::continuousKeys_ = continuousKeys;
+
+  // Assign factors_ after calling augment
+  factors_ = augment(factorPairs);
+}
+
 /* *******************************************************************************/
 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,
-  // but not if both are empty or both are not empty:
+  // This will return false if either factors_ is empty or e->factors_ is
+  // 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:
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 8e36e6615e..0dc80ed36f 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -73,17 +73,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// Decision tree of Gaussian factors indexed by discrete keys.
   Factors factors_;
 
-  /// Helper function to "hide" the constants in the Jacobian factors.
-  static Factors augment(const FactorValuePairs &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
   /// @{
@@ -96,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,
-                       const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactor::shared_ptr> &factors)
-      : Base(continuousKeys, {discreteKey}), factors_({discreteKey}, factors) {}
+  HybridGaussianFactor(const DiscreteKey &discreteKey,
+                       const std::vector<GaussianFactor::shared_ptr> &factors);
 
   /**
    * @brief Construct a new HybridGaussianFactor on a single discrete key,
@@ -111,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,
-                       const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactorValuePair> &factors)
-      : HybridGaussianFactor(continuousKeys, {discreteKey},
-                             FactorValuePairs({discreteKey}, factors)) {}
+  HybridGaussianFactor(const DiscreteKey &discreteKey,
+                       const std::vector<GaussianFactorValuePair> &factorPairs);
 
   /**
    * @brief Construct a new HybridGaussianFactor on a several discrete keys M,
@@ -127,14 +109,11 @@ 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,
-                       const DiscreteKeys &discreteKeys,
-                       const FactorValuePairs &factors)
-      : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {}
+  HybridGaussianFactor(const DiscreteKeys &discreteKeys,
+                       const FactorValuePairs &factors);
 
   /// @}
   /// @name Testable
@@ -189,7 +168,27 @@ 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;

From 08cf399bbf3e656ba8caf1c5f482d84318c9ee24 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 13:07:10 -0700
Subject: [PATCH 08/22] Fix all call sites

---
 gtsam/hybrid/HybridGaussianConditional.cpp    |  8 ++----
 gtsam/hybrid/HybridGaussianConditional.h      |  4 +--
 gtsam/hybrid/HybridGaussianFactor.cpp         | 15 -----------
 gtsam/hybrid/HybridGaussianFactorGraph.cpp    |  7 ++----
 gtsam/hybrid/HybridNonlinearFactor.cpp        |  2 +-
 gtsam/hybrid/tests/Switching.h                |  2 +-
 gtsam/hybrid/tests/testGaussianMixture.cpp    | 12 +++++++++
 gtsam/hybrid/tests/testHybridEstimation.cpp   |  4 +--
 gtsam/hybrid/tests/testHybridFactorGraph.cpp  |  2 +-
 .../hybrid/tests/testHybridGaussianFactor.cpp | 25 ++++++++-----------
 .../tests/testHybridGaussianFactorGraph.cpp   | 18 ++++++-------
 .../hybrid/tests/testSerializationHybrid.cpp  |  3 +--
 12 files changed, 43 insertions(+), 59 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 07c80f58c8..46481e3058 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/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>(
-      continuousParentKeys, discreteParentKeys, likelihoods);
+  return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
+                                                likelihoods);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index ff9391d678..15ef431730 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 952bb7017e..12ec1d2a8f 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 540ee446b0..b519b3a0ae 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/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,
-                                                discreteSeparator, newFactors);
+  return std::make_shared<HybridGaussianFactor>(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,
-                                       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 f5be6ded87..301d79593f 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/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);
 }
 
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 8836d04c88..9144e3b6dd 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 cde7e4063b..743b98c66a 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 6c69607a44..2a20f5f5ea 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/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>(
-      gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs);
+  return std::make_shared<HybridGaussianFactor>(gmf->discreteKeys(),
+                                                updated_pairs);
 }
 
 /****************************************************************************/
diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
index e4d207af39..c6020de850 100644
--- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp
+++ b/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(
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp
index 5df9f4811a..080185d88d 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp
+++ b/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 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<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);
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 6e817d312b..e4acda3871 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 6a672a3d24..e09669117e 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<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));

From 69b1313eed61e5e185b84d5cc745c0e31ef43ee7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 13:22:32 -0700
Subject: [PATCH 09/22] Working version with helper

---
 gtsam/hybrid/HybridGaussianFactor.cpp      | 71 +++++++---------------
 gtsam/hybrid/HybridGaussianFactor.h        | 33 +++++++++-
 gtsam/hybrid/tests/testGaussianMixture.cpp | 12 ----
 3 files changed, 52 insertions(+), 64 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 12ec1d2a8f..279ac4069d 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -70,14 +70,13 @@ HybridGaussianFactor::Factors HybridGaussianFactor::augment(
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::HybridGaussianFactor(
+HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
     const DiscreteKey &discreteKey,
     const std::vector<GaussianFactor::shared_ptr> &factors)
-    : Base(HybridFactor::Category::Hybrid) {
-  // Extract continuous keys from first-null factor and verify all others
-  KeyVector continuousKeys;
+    : discreteKeys({discreteKey}) {
+  // Extract continuous keys from the first non-null factor
   for (const auto &factor : factors) {
-    if (!factor) continue;
+    if (!factor) continue;  // Skip null factors
     if (continuousKeys.empty()) {
       continuousKeys = factor->keys();
     } else if (factor->keys() != continuousKeys) {
@@ -85,25 +84,18 @@ HybridGaussianFactor::HybridGaussianFactor(
     }
   }
 
-  // Initialize the base class
-  Factor::keys_ = continuousKeys;
-  Factor::keys_.push_back(discreteKey.first);
-  Base::discreteKeys_ = {discreteKey};
-  Base::continuousKeys_ = continuousKeys;
-
-  // Build the DecisionTree from factor vector
-  factors_ = Factors({discreteKey}, factors);
+  // Build the DecisionTree from the factor vector
+  factorsTree = Factors(discreteKeys, factors);
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::HybridGaussianFactor(
+HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
     const DiscreteKey &discreteKey,
     const std::vector<GaussianFactorValuePair> &factorPairs)
-    : Base(HybridFactor::Category::Hybrid) {
-  // Extract continuous keys from first-null factor and verify all others
-  KeyVector continuousKeys;
+    : discreteKeys({discreteKey}) {
+  // Extract continuous keys from the first non-null factor
   for (const auto &pair : factorPairs) {
-    if (!pair.first) continue;
+    if (!pair.first) continue;  // Skip null factors
     if (continuousKeys.empty()) {
       continuousKeys = pair.first->keys();
     } else if (pair.first->keys() != continuousKeys) {
@@ -111,45 +103,26 @@ HybridGaussianFactor::HybridGaussianFactor(
     }
   }
 
-  // Initialize the base class
-  Factor::keys_ = continuousKeys;
-  Factor::keys_.push_back(discreteKey.first);
-  Base::discreteKeys_ = {discreteKey};
-  Base::continuousKeys_ = continuousKeys;
-
   // Build the FactorValuePairs DecisionTree
-  FactorValuePairs pairTree({discreteKey}, factorPairs);
-
-  // Assign factors_ after calling augment
-  factors_ = augment(pairTree);
+  pairs = FactorValuePairs(discreteKeys, factorPairs);
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
-                                           const FactorValuePairs &factorPairs)
-    : Base(HybridFactor::Category::Hybrid) {
-  // Verify that all factors have the same keys
-  KeyVector continuousKeys;
+HybridGaussianFactor::ConstructorHelper::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) {
-      if (continuousKeys.empty()) {
-        continuousKeys = pair.first->keys();
-      } else if (pair.first->keys() != continuousKeys) {
-        throw std::invalid_argument("All factors must have the same keys");
-      }
+    if (!pair.first) return;  // Skip null factors
+    if (continuousKeys.empty()) {
+      continuousKeys = pair.first->keys();
+    } else if (pair.first->keys() != continuousKeys) {
+      throw std::invalid_argument("All factors must have the same keys");
     }
   });
 
-  // Initialize the base class
-  Factor::keys_ = continuousKeys;
-  for (const auto &discreteKey : discreteKeys) {
-    Factor::keys_.push_back(discreteKey.first);
-  }
-  Base::discreteKeys_ = discreteKeys;
-  Base::continuousKeys_ = continuousKeys;
-
-  // Assign factors_ after calling augment
-  factors_ = augment(factorPairs);
+  // Build the FactorValuePairs DecisionTree
+  pairs = factorPairs;
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 0dc80ed36f..b20dc130c8 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -89,7 +89,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factors Vector of gaussian factors, one for each mode.
    */
   HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactor::shared_ptr> &factors);
+                       const std::vector<GaussianFactor::shared_ptr> &factors)
+      : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
 
   /**
    * @brief Construct a new HybridGaussianFactor on a single discrete key,
@@ -101,7 +102,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factorPairs Vector of gaussian factor-scalar pairs, one per mode.
    */
   HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactorValuePair> &factorPairs);
+                       const std::vector<GaussianFactorValuePair> &factorPairs)
+      : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
 
   /**
    * @brief Construct a new HybridGaussianFactor on a several discrete keys M,
@@ -113,7 +115,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factors The decision tree of Gaussian factor/scalar pairs.
    */
   HybridGaussianFactor(const DiscreteKeys &discreteKeys,
-                       const FactorValuePairs &factors);
+                       const FactorValuePairs &factors)
+      : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
 
   /// @}
   /// @name Testable
@@ -193,6 +196,30 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   double potentiallyPrunedComponentError(
       const sharedFactor &gf, const VectorValues &continuousValues) const;
 
+  /// Helper struct to assist in constructing the HybridGaussianFactor
+  struct ConstructorHelper {
+    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> &factorsVec);
+
+    ConstructorHelper(const DiscreteKey &discreteKey,
+                      const std::vector<GaussianFactorValuePair> &factorPairs);
+
+    ConstructorHelper(const DiscreteKeys &discreteKeys,
+                      const FactorValuePairs &factorPairs);
+  };
+
+  // Private constructor using ConstructorHelper
+  HybridGaussianFactor(const ConstructorHelper &helper)
+      : Base(helper.continuousKeys, helper.discreteKeys),
+        factors_(helper.factorsTree.empty() ? augment(helper.pairs)
+                                            : helper.factorsTree) {}
+
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 743b98c66a..cde7e4063b 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -101,12 +101,6 @@ 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);
@@ -141,12 +135,6 @@ 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;

From 83fae8ecf8157a708a4404d8dfd7ce70f4a81cc8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 13:34:03 -0700
Subject: [PATCH 10/22] Remove error checking

---
 gtsam/hybrid/HybridGaussianConditional.cpp |  9 ++-------
 gtsam/hybrid/HybridGaussianFactor.cpp      | 17 +++++------------
 2 files changed, 7 insertions(+), 19 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 46481e3058..f3a699094b 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -37,14 +37,9 @@ HybridGaussianConditional::ConstructorHelper::ConstructorHelper(
       [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair {
     double value = 0.0;
     if (c) {
-      KeyVector cf(c->frontals().begin(), c->frontals().end());
-      KeyVector cp(c->parents().begin(), c->parents().end());
       if (frontals.empty()) {
-        frontals = cf;
-        parents = cp;
-      } else if (cf != frontals || cp != parents) {
-        throw std::invalid_argument(
-            "All conditionals must have the same frontals and parents");
+        frontals = KeyVector(c->frontals().begin(), c->frontals().end());
+        parents = KeyVector(c->parents().begin(), c->parents().end());
       }
       value = c->negLogConstant();
       negLogConstant = std::min(negLogConstant, value);
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 279ac4069d..938a79b19d 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -76,11 +76,9 @@ HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
     : discreteKeys({discreteKey}) {
   // Extract continuous keys from the first non-null factor
   for (const auto &factor : factors) {
-    if (!factor) continue;  // Skip null factors
-    if (continuousKeys.empty()) {
+    if (factor && continuousKeys.empty()) {
       continuousKeys = factor->keys();
-    } else if (factor->keys() != continuousKeys) {
-      throw std::invalid_argument("All factors must have the same keys");
+      break;
     }
   }
 
@@ -95,11 +93,9 @@ HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
     : discreteKeys({discreteKey}) {
   // Extract continuous keys from the first non-null factor
   for (const auto &pair : factorPairs) {
-    if (!pair.first) continue;  // Skip null factors
-    if (continuousKeys.empty()) {
+    if (pair.first && continuousKeys.empty()) {
       continuousKeys = pair.first->keys();
-    } else if (pair.first->keys() != continuousKeys) {
-      throw std::invalid_argument("All factors must have the same keys");
+      break;
     }
   }
 
@@ -113,11 +109,8 @@ HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
     : discreteKeys(discreteKeys) {
   // Extract continuous keys from the first non-null factor
   factorPairs.visit([&](const GaussianFactorValuePair &pair) {
-    if (!pair.first) return;  // Skip null factors
-    if (continuousKeys.empty()) {
+    if (pair.first && continuousKeys.empty()) {
       continuousKeys = pair.first->keys();
-    } else if (pair.first->keys() != continuousKeys) {
-      throw std::invalid_argument("All factors must have the same keys");
     }
   });
 

From e18dd3e90500a4d40aaa3b746e2acf484f545da6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 13:38:54 -0700
Subject: [PATCH 11/22] Removed unneeded changes

---
 gtsam/hybrid/HybridFactor.h | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 39a72eb261..fc91e08389 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -56,9 +56,11 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// Enum to help with categorizing hybrid factors.
   enum class Category { None, Discrete, Continuous, Hybrid };
 
- protected:
+ private:
   /// Record what category of HybridFactor this is.
   Category category_ = Category::None;
+
+ protected:
   // Set of DiscreteKeys for this factor.
   DiscreteKeys discreteKeys_;
   /// Record continuous keys for book-keeping
@@ -140,10 +142,6 @@ class GTSAM_EXPORT HybridFactor : public Factor {
 
   /// @}
 
- protected:
-  /// protected constructor to initialize the category
-  HybridFactor(Category category) : category_(category) {}
-
  private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */

From 2c12e685eae4e128808d9b968c0f8fa83d686b25 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 14:50:50 -0700
Subject: [PATCH 12/22] Fix HybridNonlinearFactor

---
 gtsam/hybrid/HybridGaussianFactor.h    |  9 ++--
 gtsam/hybrid/HybridNonlinearFactor.cpp | 66 ++++++++++++++------------
 gtsam/hybrid/HybridNonlinearFactor.h   | 43 ++++++++++++-----
 3 files changed, 70 insertions(+), 48 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index b20dc130c8..46b21b8aa9 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -196,16 +196,15 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   double potentiallyPrunedComponentError(
       const sharedFactor &gf, const VectorValues &continuousValues) const;
 
-  /// Helper struct to assist in constructing the HybridGaussianFactor
+  /// Helper struct to assist private constructor below.
   struct ConstructorHelper {
     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> &factorsVec);
+    ConstructorHelper(const DiscreteKey &discreteKey,
+                      const std::vector<GaussianFactor::shared_ptr> &factors);
 
     ConstructorHelper(const DiscreteKey &discreteKey,
                       const std::vector<GaussianFactorValuePair> &factorPairs);
@@ -214,7 +213,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
                       const FactorValuePairs &factorPairs);
   };
 
-  // Private constructor using ConstructorHelper
+  // Private constructor using ConstructorHelper above.
   HybridGaussianFactor(const ConstructorHelper &helper)
       : Base(helper.continuousKeys, helper.discreteKeys),
         factors_(helper.factorsTree.empty() ? augment(helper.pairs)
diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp
index 301d79593f..351a7fea4f 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@@ -18,55 +18,59 @@
 
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 
+#include <memory>
+
+#include "gtsam/nonlinear/NonlinearFactor.h"
+
 namespace gtsam {
 
 /* *******************************************************************************/
-static void checkKeys(const KeyVector& continuousKeys,
-                      const std::vector<NonlinearFactorValuePair>& pairs) {
-  KeySet factor_keys_set;
-  for (const auto& pair : pairs) {
-    auto f = pair.first;
-    // Insert all factor continuous keys in the continuous keys set.
-    std::copy(f->keys().begin(), f->keys().end(),
-              std::inserter(factor_keys_set, factor_keys_set.end()));
-  }
-
-  KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
-  if (continuous_keys_set != factor_keys_set) {
+static void CopyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor,
+                                      KeyVector* continuousKeys) {
+  if (!factor) return;
+  if (continuousKeys->empty()) {
+    *continuousKeys = factor->keys();
+  } else if (factor->keys() != *continuousKeys) {
     throw std::runtime_error(
-        "HybridNonlinearFactor: The specified continuous keys and the keys in "
-        "the factors do not match!");
+        "HybridNonlinearFactor: all factors should have the same keys!");
   }
 }
 
 /* *******************************************************************************/
-HybridNonlinearFactor::HybridNonlinearFactor(
-    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
+    const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactor::shared_ptr>& factors)
-    : Base(continuousKeys, {discreteKey}) {
+    : discreteKeys({discreteKey}) {
   std::vector<NonlinearFactorValuePair> pairs;
-  for (auto&& f : factors) {
-    pairs.emplace_back(f, 0.0);
+  // Extract continuous keys from the first non-null factor
+  for (const auto& factor : factors) {
+    pairs.emplace_back(factor, 0.0);
+    CopyOrCheckContinuousKeys(factor, &continuousKeys);
   }
-  checkKeys(continuousKeys, pairs);
-  factors_ = FactorValuePairs({discreteKey}, pairs);
+  factorTree = FactorValuePairs({discreteKey}, pairs);
 }
 
 /* *******************************************************************************/
-HybridNonlinearFactor::HybridNonlinearFactor(
-    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
+    const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactorValuePair>& pairs)
-    : Base(continuousKeys, {discreteKey}) {
-  KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
-  checkKeys(continuousKeys, pairs);
-  factors_ = FactorValuePairs({discreteKey}, pairs);
+    : discreteKeys({discreteKey}) {
+  // Extract continuous keys from the first non-null factor
+  for (const auto& pair : pairs) {
+    CopyOrCheckContinuousKeys(pair.first, &continuousKeys);
+  }
+  factorTree = FactorValuePairs({discreteKey}, pairs);
 }
 
 /* *******************************************************************************/
-HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys,
-                                             const DiscreteKeys& discreteKeys,
-                                             const FactorValuePairs& factors)
-    : Base(continuousKeys, discreteKeys), factors_(factors) {}
+HybridNonlinearFactor::ConstructorHelper::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, &continuousKeys);
+  });
+}
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(
diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h
index 7843afc836..161a7b3570 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.h
+++ b/gtsam/hybrid/HybridNonlinearFactor.h
@@ -90,13 +90,12 @@ 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 std::vector<NonlinearFactor::shared_ptr>& factors);
+  HybridNonlinearFactor(const DiscreteKey& discreteKey,
+                        const std::vector<NonlinearFactor::shared_ptr>& factors)
+      : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a single discrete key,
@@ -104,13 +103,12 @@ 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,
-                        const DiscreteKey& discreteKey,
-                        const std::vector<NonlinearFactorValuePair>& pairs);
+  HybridNonlinearFactor(const DiscreteKey& discreteKey,
+                        const std::vector<NonlinearFactorValuePair>& pairs)
+      : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {}
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a several discrete keys M,
@@ -118,13 +116,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,
-                        const DiscreteKeys& discreteKeys,
-                        const FactorValuePairs& factors);
+  HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
+                        const FactorValuePairs& factors)
+      : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
   /**
    * @brief Compute error of the HybridNonlinearFactor as a tree.
    *
@@ -181,6 +178,28 @@ 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 {
+    KeyVector continuousKeys;   // Continuous keys extracted from factors
+    DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
+    FactorValuePairs factorTree;
+
+    ConstructorHelper(const DiscreteKey& discreteKey,
+                      const std::vector<NonlinearFactor::shared_ptr>& factors);
+
+    ConstructorHelper(const DiscreteKey& discreteKey,
+                      const std::vector<NonlinearFactorValuePair>& factorPairs);
+
+    ConstructorHelper(const DiscreteKeys& discreteKeys,
+                      const FactorValuePairs& factorPairs);
+  };
+
+  // Private constructor using ConstructorHelper above.
+  HybridNonlinearFactor(const ConstructorHelper& helper)
+      : Base(helper.continuousKeys, helper.discreteKeys),
+        factors_(helper.factorTree) {}
 };
 
 // traits

From bb4c3c95abb93ea6ab31d925cfc17d573179593a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 14:51:10 -0700
Subject: [PATCH 13/22] Fix call sites for HybridNonlinearFactor

---
 gtsam/hybrid/tests/Switching.h                |  3 +-
 gtsam/hybrid/tests/testHybridBayesNet.cpp     |  2 +-
 gtsam/hybrid/tests/testHybridEstimation.cpp   | 11 ++-----
 gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 12 ++------
 .../tests/testHybridNonlinearFactor.cpp       | 10 +++----
 .../tests/testHybridNonlinearFactorGraph.cpp  | 30 ++-----------------
 .../hybrid/tests/testHybridNonlinearISAM.cpp  | 12 ++------
 7 files changed, 19 insertions(+), 61 deletions(-)

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 9144e3b6dd..23a102fd5b 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -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);
     }
 
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index b049b03fd2..8659100acb 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -388,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);
diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 2a20f5f5ea..1b0286084d 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/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,
-                                            components);
+  nfg.emplace_shared<HybridNonlinearFactor>(m, components);
 
   return nfg;
 }
@@ -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)};
-  DiscreteKey modes(M(0), 2);
-  HybridNonlinearFactor mf(keys, modes, components);
+  HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert(X(0), 0.0);
   initial.insert(X(1), 0.0);
@@ -681,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)};
-  DiscreteKey modes(M(0), 2);
-  HybridNonlinearFactor mf(keys, modes, components);
+  HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert<Vector3>(X(0), Z_3x1);
   initial.insert<Vector3>(X(1), Z_3x1);
diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
index 9ce109647e..280c6cb531 100644
--- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
+++ b/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>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(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>(
-      contKeys, gtsam::DiscreteKey(M(2), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(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>(
-      contKeys, gtsam::DiscreteKey(M(3), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
index d949a1e061..2b441ab131 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
+++ b/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});
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 63a41a63a1..f9a546c814 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/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 =
-      std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
+  auto dcFactor = std::make_shared<HybridNonlinearFactor>(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>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), motion_models);
+  fg.emplace_shared<HybridNonlinearFactor>(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);
diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index fe81fc13e5..9e93116fc0 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/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>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(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>(
-      contKeys, gtsam::DiscreteKey(M(2), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(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>(
-      contKeys, gtsam::DiscreteKey(M(3), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

From 1a566ea2bb8076d3da680855995777191b7e5877 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 14:51:39 -0700
Subject: [PATCH 14/22] Fix wrapper

---
 gtsam/hybrid/hybrid.i                        | 16 +++++++---------
 python/gtsam/tests/test_HybridFactorGraph.py |  5 ++---
 2 files changed, 9 insertions(+), 12 deletions(-)

diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index d1b8fbf6d1..9ed96d1154 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/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);
 
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 4ec1aec1ed..3c63b91541 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -17,7 +17,7 @@
 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)
@@ -102,8 +102,7 @@ def tiny(num_measurements: int = 1,
                                                                  X(0), [0],
                                                                  sigma=3)
             bayesNet.push_back(
-                HybridGaussianConditional([Z(i)], [X(0)], mode,
-                                          [conditional0, conditional1]))
+                HybridGaussianConditional(mode, [conditional0, conditional1]))
 
         # Create prior on X(0).
         prior_on_x0 = GaussianConditional.FromMeanAndStddev(

From 71d5a6c1f14e2a053ac6fd48099577459efb079c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 16:28:56 -0700
Subject: [PATCH 15/22] Fix more wrapper tests

---
 python/gtsam/tests/test_HybridBayesNet.py    | 5 ++---
 python/gtsam/tests/test_HybridFactorGraph.py | 4 ++--
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py
index bf2b6a0330..57346d4d4b 100644
--- a/python/gtsam/tests/test_HybridBayesNet.py
+++ b/python/gtsam/tests/test_HybridBayesNet.py
@@ -17,7 +17,7 @@
 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 @@ def test_evaluate(self):
         bayesNet = HybridBayesNet()
         bayesNet.push_back(conditional)
         bayesNet.push_back(
-            HybridGaussianConditional([X(1)], [], Asia,
-                                      [conditional0, conditional1]))
+            HybridGaussianConditional(Asia, [conditional0, conditional1]))
         bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
 
         # Create values at which to evaluate.
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 3c63b91541..6d609deb03 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -35,7 +35,7 @@ def test_create(self):
         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 @@ def test_optimize(self):
         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)

From 3e6227f2b520452d60b26854ac0f633cf4e4a06b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 16:47:07 -0700
Subject: [PATCH 16/22] Move code to cpp file.

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 51 ++++++-----
 gtsam/hybrid/HybridGaussianConditional.h   | 14 +--
 gtsam/hybrid/HybridGaussianFactor.cpp      | 99 +++++++++++++---------
 gtsam/hybrid/HybridGaussianFactor.h        | 30 ++-----
 gtsam/hybrid/HybridNonlinearFactor.cpp     | 93 ++++++++++++--------
 gtsam/hybrid/HybridNonlinearFactor.h       | 32 ++-----
 6 files changed, 164 insertions(+), 155 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index f3a699094b..87dff7e59b 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -29,34 +29,47 @@
 
 namespace gtsam {
 /* *******************************************************************************/
-HybridGaussianConditional::ConstructorHelper::ConstructorHelper(
-    const HybridGaussianConditional::Conditionals &conditionals) {
-  negLogConstant = std::numeric_limits<double>::infinity();
-
-  auto func =
-      [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair {
-    double value = 0.0;
-    if (c) {
-      if (frontals.empty()) {
-        frontals = KeyVector(c->frontals().begin(), c->frontals().end());
-        parents = KeyVector(c->parents().begin(), c->parents().end());
+struct HybridGaussianConditional::ConstructorHelper {
+  KeyVector frontals, parents;
+  HybridGaussianFactor::FactorValuePairs pairs;
+  double negLogConstant;
+  /// Compute all variables needed for the private constructor below.
+  ConstructorHelper(const Conditionals &conditionals) {
+    negLogConstant = std::numeric_limits<double>::infinity();
+
+    auto func = [&](const GaussianConditional::shared_ptr &c)
+        -> GaussianFactorValuePair {
+      double value = 0.0;
+      if (c) {
+        if (frontals.empty()) {
+          frontals = KeyVector(c->frontals().begin(), c->frontals().end());
+          parents = KeyVector(c->parents().begin(), c->parents().end());
+        }
+        value = c->negLogConstant();
+        negLogConstant = std::min(negLogConstant, value);
       }
-      value = c->negLogConstant();
-      negLogConstant = std::min(negLogConstant, value);
-    }
-    return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
-  };
-  pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
-}
+      return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
+    };
+    pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
+  }
+};
 
 /* *******************************************************************************/
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKeys &discreteParents,
+    const HybridGaussianConditional::Conditionals &conditionals,
+    const ConstructorHelper &helper)
+    : BaseFactor(discreteParents, helper.pairs),
+      BaseConditional(helper.frontals.size()),
+      conditionals_(conditionals),
+      negLogConstant_(helper.negLogConstant) {}
+
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKeys &discreteParents,
     const HybridGaussianConditional::Conditionals &conditionals)
     : HybridGaussianConditional(discreteParents, conditionals,
                                 ConstructorHelper(conditionals)) {}
 
-/* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKey &discreteParent,
     const std::vector<GaussianConditional::shared_ptr> &conditionals)
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index 15ef431730..b7e2ff0245 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -185,23 +185,13 @@ class GTSAM_EXPORT HybridGaussianConditional
 
  private:
   /// Helper struct for private constructor.
-  struct ConstructorHelper {
-    KeyVector frontals, parents;
-    HybridGaussianFactor::FactorValuePairs pairs;
-    double negLogConstant;
-    /// Compute all variables needed for the private constructor below.
-    ConstructorHelper(const Conditionals &conditionals);
-  };
+  struct ConstructorHelper;
 
   /// Private constructor that uses helper struct above.
   HybridGaussianConditional(
       const DiscreteKeys &discreteParents,
       const HybridGaussianConditional::Conditionals &conditionals,
-      const ConstructorHelper &helper)
-      : BaseFactor(discreteParents, helper.pairs),
-        BaseConditional(helper.frontals.size()),
-        conditionals_(conditionals),
-        negLogConstant_(helper.negLogConstant) {}
+      const ConstructorHelper &helper);
 
   /// Convert to a DecisionTree of Gaussian factor graphs.
   GaussianFactorGraphTree asGaussianFactorGraphTree() const;
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 938a79b19d..3672eb2e24 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -70,53 +70,76 @@ HybridGaussianFactor::Factors HybridGaussianFactor::augment(
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::ConstructorHelper::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;
+struct HybridGaussianFactor::ConstructorHelper {
+  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);
-}
+    // Build the DecisionTree from the factor vector
+    factorsTree = Factors(discreteKeys, factors);
+  }
 
-/* *******************************************************************************/
-HybridGaussianFactor::ConstructorHelper::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;
+  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);
   }
 
-  // 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::ConstructorHelper::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();
-    }
-  });
+HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper)
+    : Base(helper.continuousKeys, helper.discreteKeys),
+      factors_(helper.factorsTree.empty() ? augment(helper.pairs)
+                                          : helper.factorsTree) {}
 
-  // Build the FactorValuePairs DecisionTree
-  pairs = factorPairs;
-}
+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)
+    : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
 
 /* *******************************************************************************/
 bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 46b21b8aa9..972ee47ba3 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -89,8 +89,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factors Vector of gaussian factors, one for each mode.
    */
   HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactor::shared_ptr> &factors)
-      : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
+                       const std::vector<GaussianFactor::shared_ptr> &factors);
 
   /**
    * @brief Construct a new HybridGaussianFactor on a single discrete key,
@@ -102,8 +101,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factorPairs Vector of gaussian factor-scalar pairs, one per mode.
    */
   HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactorValuePair> &factorPairs)
-      : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
+                       const std::vector<GaussianFactorValuePair> &factorPairs);
 
   /**
    * @brief Construct a new HybridGaussianFactor on a several discrete keys M,
@@ -115,8 +113,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factors The decision tree of Gaussian factor/scalar pairs.
    */
   HybridGaussianFactor(const DiscreteKeys &discreteKeys,
-                       const FactorValuePairs &factors)
-      : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
+                       const FactorValuePairs &factors);
 
   /// @}
   /// @name Testable
@@ -197,27 +194,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
       const sharedFactor &gf, const VectorValues &continuousValues) const;
 
   /// Helper struct to assist private constructor below.
-  struct ConstructorHelper {
-    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);
-
-    ConstructorHelper(const DiscreteKey &discreteKey,
-                      const std::vector<GaussianFactorValuePair> &factorPairs);
-
-    ConstructorHelper(const DiscreteKeys &discreteKeys,
-                      const FactorValuePairs &factorPairs);
-  };
+  struct ConstructorHelper;
 
   // Private constructor using ConstructorHelper above.
-  HybridGaussianFactor(const ConstructorHelper &helper)
-      : Base(helper.continuousKeys, helper.discreteKeys),
-        factors_(helper.factorsTree.empty() ? augment(helper.pairs)
-                                            : helper.factorsTree) {}
+  HybridGaussianFactor(const ConstructorHelper &helper);
 
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp
index 351a7fea4f..ac7e943b4a 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@@ -25,52 +25,71 @@
 namespace gtsam {
 
 /* *******************************************************************************/
-static void CopyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor,
-                                      KeyVector* continuousKeys) {
-  if (!factor) return;
-  if (continuousKeys->empty()) {
-    *continuousKeys = factor->keys();
-  } else if (factor->keys() != *continuousKeys) {
-    throw std::runtime_error(
-        "HybridNonlinearFactor: all factors should have the same keys!");
+struct HybridNonlinearFactor::ConstructorHelper {
+  KeyVector continuousKeys;   // Continuous keys extracted from factors
+  DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
+  FactorValuePairs factorTree;
+
+  void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) {
+    if (!factor) return;
+    if (continuousKeys.empty()) {
+      continuousKeys = factor->keys();
+    } else if (factor->keys() != continuousKeys) {
+      throw std::runtime_error(
+          "HybridNonlinearFactor: all factors should have the same keys!");
+    }
   }
-}
+
+  ConstructorHelper(const DiscreteKey& discreteKey,
+                    const std::vector<NonlinearFactor::shared_ptr>& factors)
+      : discreteKeys({discreteKey}) {
+    std::vector<NonlinearFactorValuePair> pairs;
+    // 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::ConstructorHelper::ConstructorHelper(
+HybridNonlinearFactor::HybridNonlinearFactor(const ConstructorHelper& helper)
+    : Base(helper.continuousKeys, helper.discreteKeys),
+      factors_(helper.factorTree) {}
+
+HybridNonlinearFactor::HybridNonlinearFactor(
     const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactor::shared_ptr>& factors)
-    : discreteKeys({discreteKey}) {
-  std::vector<NonlinearFactorValuePair> pairs;
-  // Extract continuous keys from the first non-null factor
-  for (const auto& factor : factors) {
-    pairs.emplace_back(factor, 0.0);
-    CopyOrCheckContinuousKeys(factor, &continuousKeys);
-  }
-  factorTree = FactorValuePairs({discreteKey}, pairs);
-}
+    : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
 
-/* *******************************************************************************/
-HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
+HybridNonlinearFactor::HybridNonlinearFactor(
     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, &continuousKeys);
-  }
-  factorTree = FactorValuePairs({discreteKey}, pairs);
-}
+    : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {}
 
-/* *******************************************************************************/
-HybridNonlinearFactor::ConstructorHelper::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, &continuousKeys);
-  });
-}
+HybridNonlinearFactor::HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
+                                             const FactorValuePairs& factors)
+    : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(
diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h
index 161a7b3570..a3b6ad4d94 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.h
+++ b/gtsam/hybrid/HybridNonlinearFactor.h
@@ -93,9 +93,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * @param discreteKey The discrete key for the "mode", indexing components.
    * @param factors Vector of gaussian factors, one for each mode.
    */
-  HybridNonlinearFactor(const DiscreteKey& discreteKey,
-                        const std::vector<NonlinearFactor::shared_ptr>& factors)
-      : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
+  HybridNonlinearFactor(
+      const DiscreteKey& discreteKey,
+      const std::vector<NonlinearFactor::shared_ptr>& factors);
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a single discrete key,
@@ -107,8 +107,7 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * @param pairs Vector of gaussian factor-scalar pairs, one per mode.
    */
   HybridNonlinearFactor(const DiscreteKey& discreteKey,
-                        const std::vector<NonlinearFactorValuePair>& pairs)
-      : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {}
+                        const std::vector<NonlinearFactorValuePair>& pairs);
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a several discrete keys M,
@@ -120,8 +119,8 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * @param factors The decision tree of nonlinear factor/scalar pairs.
    */
   HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
-                        const FactorValuePairs& factors)
-      : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
+                        const FactorValuePairs& factors);
+
   /**
    * @brief Compute error of the HybridNonlinearFactor as a tree.
    *
@@ -181,25 +180,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
 
  private:
   /// Helper struct to assist private constructor below.
-  struct ConstructorHelper {
-    KeyVector continuousKeys;   // Continuous keys extracted from factors
-    DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
-    FactorValuePairs factorTree;
-
-    ConstructorHelper(const DiscreteKey& discreteKey,
-                      const std::vector<NonlinearFactor::shared_ptr>& factors);
-
-    ConstructorHelper(const DiscreteKey& discreteKey,
-                      const std::vector<NonlinearFactorValuePair>& factorPairs);
-
-    ConstructorHelper(const DiscreteKeys& discreteKeys,
-                      const FactorValuePairs& factorPairs);
-  };
+  struct ConstructorHelper;
 
   // Private constructor using ConstructorHelper above.
-  HybridNonlinearFactor(const ConstructorHelper& helper)
-      : Base(helper.continuousKeys, helper.discreteKeys),
-        factors_(helper.factorTree) {}
+  HybridNonlinearFactor(const ConstructorHelper& helper);
 };
 
 // traits

From d93ebeafde1c7ce27c5093d63bdbce2180b3f4c6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 26 Sep 2024 17:23:44 -0700
Subject: [PATCH 17/22] Yet another python test

---
 python/gtsam/tests/test_HybridNonlinearFactorGraph.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py
index 4a1abdcf39..1880c18f27 100644
--- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py
+++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py
@@ -14,8 +14,6 @@
 
 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 @@ def test_nonlinear_hybrid(self):
                                       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))
-        nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3"))
+        mode = (10, 2)
+        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))

From 14ad127094e8b0638bec2baeaa6f4161050998d0 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 27 Sep 2024 06:48:23 -0700
Subject: [PATCH 18/22] Review comments

---
 gtsam/hybrid/HybridGaussianFactor.cpp  | 3 +--
 gtsam/hybrid/HybridGaussianFactor.h    | 2 +-
 gtsam/hybrid/HybridNonlinearFactor.cpp | 3 +--
 3 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 3672eb2e24..6f33c53284 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -21,13 +21,12 @@
 #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>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
-#include "gtsam/hybrid/HybridFactor.h"
-
 namespace gtsam {
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 972ee47ba3..f23d065b6e 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -69,7 +69,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// typedef for Decision Tree of Gaussian factors.
   using Factors = DecisionTree<Key, sharedFactor>;
 
- protected:
+ private:
   /// Decision tree of Gaussian factors indexed by discrete keys.
   Factors factors_;
 
diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp
index ac7e943b4a..7b63c0dff3 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@@ -17,11 +17,10 @@
  */
 
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <memory>
 
-#include "gtsam/nonlinear/NonlinearFactor.h"
-
 namespace gtsam {
 
 /* *******************************************************************************/

From bc555aef0a0a6e152c92a85a88635d8d97bb413c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 27 Sep 2024 06:49:07 -0700
Subject: [PATCH 19/22] Changed order

---
 gtsam/hybrid/HybridGaussianConditional.h | 26 +++++++++++-------------
 1 file changed, 12 insertions(+), 14 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index b7e2ff0245..e0bf66e529 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -77,7 +77,18 @@ 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 discreteParents the discrete parents. Will be placed last.
    * @param conditionals a decision tree of GaussianConditionals. The number of
@@ -88,19 +99,6 @@ class GTSAM_EXPORT HybridGaussianConditional
   HybridGaussianConditional(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 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);
-
   /// @}
   /// @name Testable
   /// @{

From 2b26b3c971b613226054e2f00668c3b3271b78c5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 27 Sep 2024 06:49:36 -0700
Subject: [PATCH 20/22] Major improvement -> no need to copy keys anymore

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 29 +++++++++++++---------
 1 file changed, 17 insertions(+), 12 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 87dff7e59b..6bd859c1da 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -27,30 +27,35 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include <cstddef>
+
 namespace gtsam {
 /* *******************************************************************************/
 struct HybridGaussianConditional::ConstructorHelper {
-  KeyVector frontals, parents;
+  std::optional<size_t> nrFrontals;
   HybridGaussianFactor::FactorValuePairs pairs;
-  double negLogConstant;
-  /// Compute all variables needed for the private constructor below.
-  ConstructorHelper(const Conditionals &conditionals) {
-    negLogConstant = std::numeric_limits<double>::infinity();
+  double minNegLogConstant;
 
-    auto func = [&](const GaussianConditional::shared_ptr &c)
+  /// Compute all variables needed for the private constructor below.
+  ConstructorHelper(const Conditionals &conditionals)
+      : minNegLogConstant(std::numeric_limits<double>::infinity()) {
+    auto func = [this](const GaussianConditional::shared_ptr &c)
         -> GaussianFactorValuePair {
       double value = 0.0;
       if (c) {
-        if (frontals.empty()) {
-          frontals = KeyVector(c->frontals().begin(), c->frontals().end());
-          parents = KeyVector(c->parents().begin(), c->parents().end());
+        if (!nrFrontals.has_value()) {
+          nrFrontals = c->nrFrontals();
         }
         value = c->negLogConstant();
-        negLogConstant = std::min(negLogConstant, value);
+        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.");
+    }
   }
 };
 
@@ -60,9 +65,9 @@ HybridGaussianConditional::HybridGaussianConditional(
     const HybridGaussianConditional::Conditionals &conditionals,
     const ConstructorHelper &helper)
     : BaseFactor(discreteParents, helper.pairs),
-      BaseConditional(helper.frontals.size()),
+      BaseConditional(*helper.nrFrontals),
       conditionals_(conditionals),
-      negLogConstant_(helper.negLogConstant) {}
+      negLogConstant_(helper.minNegLogConstant) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKeys &discreteParents,

From 7d51e1cdb4d1786757dd447e1d1dc8c3fb16327e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 27 Sep 2024 07:54:14 -0700
Subject: [PATCH 21/22] tiny improvement

---
 gtsam/hybrid/HybridFactorGraph.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index f5a7bcdfef..a395e17018 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -29,8 +29,7 @@ std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
       for (const DiscreteKey& key : p->discreteKeys()) {
         keys.insert(key);
       }
-    }
-    if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
+    } else if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
       for (const DiscreteKey& key : p->discreteKeys()) {
         keys.insert(key);
       }

From bc25fcea4db0746da18d061b397ae89f92b908c8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 27 Sep 2024 07:54:47 -0700
Subject: [PATCH 22/22] major improvement: continuousSeparator no longer needed

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 26 +++++++++-------------
 1 file changed, 10 insertions(+), 16 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index b519b3a0ae..7ab7893cb1 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -369,7 +369,6 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
 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.
@@ -386,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;
   };
 
@@ -403,9 +407,9 @@ 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()
-          ? createDiscreteFactor(eliminationResults, discreteSeparator)
-          : createHybridGaussianFactor(eliminationResults, discreteSeparator);
+      someContinuousLeft
+          ? createHybridGaussianFactor(eliminationResults, discreteSeparator)
+          : createDiscreteFactor(eliminationResults, discreteSeparator);
 
   // Create the HybridGaussianConditional from the conditionals
   HybridGaussianConditional::Conditionals conditionals(
@@ -514,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
-    // 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.
+    // Find all discrete keys.
     // 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,
-                             discreteSeparator);
+    return hybridElimination(factors, frontalKeys, discreteSeparator);
   }
 }