Merge pull request #1863 from borglab/feature/no_hiding

.clang-format

@@ -1,8 +1 @@
 BasedOnStyle: Google
-
-BinPackArguments: false
-BinPackParameters: false
-ColumnLimit: 100
-DerivePointerAlignment: false
-IncludeBlocks: Preserve
-PointerAlignment: Left

gtsam/hybrid/HybridBayesTree.cpp

@@ -22,14 +22,13 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/inference/BayesTree-inst.h>
 #include <gtsam/inference/BayesTreeCliqueBase-inst.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
 
 #include <memory>
 
-#include "gtsam/hybrid/HybridConditional.h"
-
 namespace gtsam {
 
 // Instantiate base class

gtsam/hybrid/HybridConditional.cpp

@@ -13,6 +13,7 @@
  *  @file HybridConditional.cpp
  *  @date Mar 11, 2022
  *  @author Fan Jiang
+ *  @author Varun Agrawal
  */
 
 #include <gtsam/hybrid/HybridConditional.h>

gtsam/hybrid/HybridConditional.h

@@ -13,6 +13,7 @@
  *  @file HybridConditional.h
  *  @date Mar 11, 2022
  *  @author Fan Jiang
+ *  @author Varun Agrawal
  */
 
 #pragma once

gtsam/hybrid/HybridFactor.h

@@ -32,9 +32,6 @@ namespace gtsam {
 
 class HybridValues;
 
-/// Alias for DecisionTree of GaussianFactorGraphs
-using GaussianFactorGraphTree = DecisionTree<Key, GaussianFactorGraph>;
-
 KeyVector CollectKeys(const KeyVector &continuousKeys,
                       const DiscreteKeys &discreteKeys);
 KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -32,6 +32,16 @@
 
 namespace gtsam {
 /* *******************************************************************************/
+/**
+ * @brief Helper struct for constructing HybridGaussianConditional objects
+ *
+ * This struct contains the following fields:
+ * - nrFrontals: Optional size_t for number of frontal variables
+ * - pairs: FactorValuePairs for storing conditionals with their negLogConstant
+ * - conditionals: Conditionals for storing conditionals. TODO(frank): kill!
+ * - minNegLogConstant: minimum negLogConstant, computed here, subtracted in
+ * constructor
+ */
 struct HybridGaussianConditional::Helper {
   std::optional<size_t> nrFrontals;
   FactorValuePairs pairs;
@@ -68,16 +78,12 @@ struct HybridGaussianConditional::Helper {
   explicit Helper(const Conditionals &conditionals)
       : conditionals(conditionals),
         minNegLogConstant(std::numeric_limits<double>::infinity()) {
-    auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair {
+    auto func = [this](const GC::shared_ptr &gc) -> GaussianFactorValuePair {
-      double value = 0.0;
+      if (!gc) return {nullptr, std::numeric_limits<double>::infinity()};
-      if (c) {
+      if (!nrFrontals) nrFrontals = gc->nrFrontals();
-        if (!nrFrontals.has_value()) {
+      double value = gc->negLogConstant();
-          nrFrontals = c->nrFrontals();
-        }
-        value = c->negLogConstant();
       minNegLogConstant = std::min(minNegLogConstant, value);
-      }
+      return {gc, value};
-      return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
     };
     pairs = FactorValuePairs(conditionals, func);
     if (!nrFrontals.has_value()) {
@@ -91,7 +97,14 @@ struct HybridGaussianConditional::Helper {
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKeys &discreteParents, const Helper &helper)
-    : BaseFactor(discreteParents, helper.pairs),
+    : BaseFactor(discreteParents,
+                 FactorValuePairs(helper.pairs,
+                                  [&](const GaussianFactorValuePair &
+                                          pair) {  // subtract minNegLogConstant
+                                    return GaussianFactorValuePair{
+                                        pair.first,
+                                        pair.second - helper.minNegLogConstant};
+                                  })),
       BaseConditional(*helper.nrFrontals),
       conditionals_(helper.conditionals),
       negLogConstant_(helper.minNegLogConstant) {}
@@ -135,29 +148,6 @@ HybridGaussianConditional::conditionals() const {
   return conditionals_;
 }
 
-/* *******************************************************************************/
-GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
-    const {
-  auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
-    // First check if conditional has not been pruned
-    if (gc) {
-      const double Cgm_Kgcm = gc->negLogConstant() - this->negLogConstant_;
-      // If there is a difference in the covariances, we need to account for
-      // that since the error is dependent on the mode.
-      if (Cgm_Kgcm > 0.0) {
-        // We add a constant factor which will be used when computing
-        // the probability of the discrete variables.
-        Vector c(1);
-        c << std::sqrt(2.0 * Cgm_Kgcm);
-        auto constantFactor = std::make_shared<JacobianFactor>(c);
-        return GaussianFactorGraph{gc, constantFactor};
-      }
-    }
-    return GaussianFactorGraph{gc};
-  };
-  return {conditionals_, wrap};
-}
-
 /* *******************************************************************************/
 size_t HybridGaussianConditional::nrComponents() const {
   size_t total = 0;
@@ -192,9 +182,11 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf,
 
   // Check the base and the factors:
   return BaseFactor::equals(*e, tol) &&
-         conditionals_.equals(
+         conditionals_.equals(e->conditionals_,
-             e->conditionals_, [tol](const auto &f1, const auto &f2) {
+                              [tol](const GaussianConditional::shared_ptr &f1,
-               return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
+                                    const GaussianConditional::shared_ptr &f2) {
+                                return (!f1 && !f2) ||
+                                       (f1 && f2 && f1->equals(*f2, tol));
                               });
 }
 
@@ -202,9 +194,6 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf,
 void HybridGaussianConditional::print(const std::string &s,
                                       const KeyFormatter &formatter) const {
   std::cout << (s.empty() ? "" : s + "\n");
-  if (isContinuous()) std::cout << "Continuous ";
-  if (isDiscrete()) std::cout << "Discrete ";
-  if (isHybrid()) std::cout << "Hybrid ";
   BaseConditional::print("", formatter);
   std::cout << " Discrete Keys = ";
   for (auto &dk : discreteKeys()) {
@@ -270,13 +259,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
           -> GaussianFactorValuePair {
         const auto likelihood_m = conditional->likelihood(given);
         const double Cgm_Kgcm = conditional->negLogConstant() - negLogConstant_;
-        if (Cgm_Kgcm == 0.0) {
-          return {likelihood_m, 0.0};
-        } else {
-          // Add a constant to the likelihood in case the noise models
-          // are not all equal.
         return {likelihood_m, Cgm_Kgcm};
-        }
       });
   return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
                                                 likelihoods);

gtsam/hybrid/HybridGaussianConditional.h

@@ -231,9 +231,6 @@ class GTSAM_EXPORT HybridGaussianConditional
   HybridGaussianConditional(const DiscreteKeys &discreteParents,
                             const Helper &helper);
 
-  /// Convert to a DecisionTree of Gaussian factor graphs.
-  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
-
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;
 

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -18,63 +18,26 @@
  * @date   Mar 12, 2022
  */
 
+#include <gtsam/base/types.h>
 #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/HybridGaussianProductFactor.h>
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
 
-/* *******************************************************************************/
-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.
-  Factors gaussianFactors;
-  AlgebraicDecisionTree<Key> valueTree;
-  std::tie(gaussianFactors, valueTree) = unzip(factors);
-
-  // Compute minimum value for normalization.
-  double min_value = valueTree.min();
-
-  // Finally, update the [A|b] matrices.
-  auto update = [&min_value](const GaussianFactorValuePair &gfv) {
-    auto [gf, value] = gfv;
-
-    auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
-    if (!jf) return gf;
-
-    double normalized_value = value - min_value;
-
-    // If the value is 0, do nothing
-    if (normalized_value == 0.0) return gf;
-
-    GaussianFactorGraph gfg;
-    gfg.push_back(jf);
-
-    Vector c(1);
-    // When hiding c inside the `b` vector, value == 0.5*c^2
-    c << std::sqrt(2.0 * normalized_value);
-    auto constantFactor = std::make_shared<JacobianFactor>(c);
-
-    gfg.push_back(constantFactor);
-    return std::dynamic_pointer_cast<GaussianFactor>(
-        std::make_shared<JacobianFactor>(gfg));
-  };
-  return Factors(factors, update);
-}
-
 /* *******************************************************************************/
 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
+  FactorValuePairs pairs;     // The decision tree with factors and scalars
-  Factors factorsTree;
 
+  /// Constructor for a single discrete key and a vector of Gaussian factors
   ConstructorHelper(const DiscreteKey& discreteKey,
                     const std::vector<GaussianFactor::shared_ptr>& factors)
       : discreteKeys({discreteKey}) {
@@ -85,16 +48,22 @@ struct HybridGaussianFactor::ConstructorHelper {
         break;
       }
     }
-
+    // Build the FactorValuePairs DecisionTree
-    // Build the DecisionTree from the factor vector
+    pairs = FactorValuePairs(
-    factorsTree = Factors(discreteKeys, factors);
+        DecisionTree<Key, GaussianFactor::shared_ptr>(discreteKeys, factors),
+        [](const sharedFactor& f) {
+          return std::pair{f,
+                           f ? 0.0 : std::numeric_limits<double>::infinity()};
+        });
   }
 
+  /// Constructor for a single discrete key and a vector of
+  /// GaussianFactorValuePairs
   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) {
+    for (const GaussianFactorValuePair& pair : factorPairs) {
       if (pair.first && continuousKeys.empty()) {
         continuousKeys = pair.first->keys();
         break;
@@ -105,10 +74,13 @@ struct HybridGaussianFactor::ConstructorHelper {
     pairs = FactorValuePairs(discreteKeys, factorPairs);
   }
 
+  /// Constructor for a vector of discrete keys and a vector of
+  /// GaussianFactorValuePairs
   ConstructorHelper(const DiscreteKeys& discreteKeys,
                     const FactorValuePairs& factorPairs)
       : discreteKeys(discreteKeys) {
     // Extract continuous keys from the first non-null factor
+    // TODO: just stop after first non-null factor
     factorPairs.visit([&](const GaussianFactorValuePair& pair) {
       if (pair.first && continuousKeys.empty()) {
         continuousKeys = pair.first->keys();
@@ -123,22 +95,18 @@ struct HybridGaussianFactor::ConstructorHelper {
 /* *******************************************************************************/
 HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper& helper)
     : Base(helper.continuousKeys, helper.discreteKeys),
-      factors_(helper.factorsTree.empty() ? augment(helper.pairs)
+      factors_(helper.pairs) {}
-                                          : helper.factorsTree) {}
 
-/* *******************************************************************************/
 HybridGaussianFactor::HybridGaussianFactor(
     const DiscreteKey& discreteKey,
     const std::vector<GaussianFactor::shared_ptr>& factors)
     : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
 
-/* *******************************************************************************/
 HybridGaussianFactor::HybridGaussianFactor(
     const DiscreteKey& discreteKey,
     const std::vector<GaussianFactorValuePair>& factorPairs)
     : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
 
-/* *******************************************************************************/
 HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys& discreteKeys,
                                            const FactorValuePairs& factors)
     : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
@@ -153,17 +121,19 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
   if (factors_.empty() ^ e->factors_.empty()) return false;
 
   // Check the base and the factors:
-  return Base::equals(*e, tol) &&
+  auto compareFunc = [tol](const GaussianFactorValuePair& pair1,
-         factors_.equals(e->factors_, [tol](const auto &f1, const auto &f2) {
+                           const GaussianFactorValuePair& pair2) {
-           return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
+    auto f1 = pair1.first, f2 = pair2.first;
-         });
+    bool match = (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
+    return match && gtsam::equal(pair1.second, pair2.second, tol);
+  };
+  return Base::equals(*e, tol) && factors_.equals(e->factors_, compareFunc);
 }
 
 /* *******************************************************************************/
 void HybridGaussianFactor::print(const std::string& s,
                                  const KeyFormatter& formatter) const {
   std::cout << (s.empty() ? "" : s + "\n");
-  std::cout << "HybridGaussianFactor" << std::endl;
   HybridFactor::print("", formatter);
   std::cout << "{\n";
   if (factors_.empty()) {
@@ -171,11 +141,12 @@ void HybridGaussianFactor::print(const std::string &s,
   } else {
     factors_.print(
         "", [&](Key k) { return formatter(k); },
-        [&](const sharedFactor &gf) -> std::string {
+        [&](const GaussianFactorValuePair& pair) -> std::string {
           RedirectCout rd;
           std::cout << ":\n";
-          if (gf) {
+          if (pair.first) {
-            gf->print("", formatter);
+            pair.first->print("", formatter);
+            std::cout << "scalar: " << pair.second << "\n";
             return rd.str();
           } else {
             return "nullptr";
@@ -186,51 +157,37 @@ void HybridGaussianFactor::print(const std::string &s,
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()(
+GaussianFactorValuePair HybridGaussianFactor::operator()(
     const DiscreteValues& assignment) const {
   return factors_(assignment);
 }
 
 /* *******************************************************************************/
-GaussianFactorGraphTree HybridGaussianFactor::add(
+HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const {
-    const GaussianFactorGraphTree &sum) const {
+  // Implemented by creating a new DecisionTree where:
-  using Y = GaussianFactorGraph;
+  // - The structure (keys and assignments) is preserved from factors_
-  auto add = [](const Y &graph1, const Y &graph2) {
+  // - Each leaf converted to a GaussianFactorGraph with just the factor and its
-    auto result = graph1;
+  // scalar.
-    result.push_back(graph2);
+  return {{factors_,
-    return result;
+           [](const GaussianFactorValuePair& pair)
-  };
+               -> std::pair<GaussianFactorGraph, double> {
-  const auto tree = asGaussianFactorGraphTree();
+             return {GaussianFactorGraph{pair.first}, pair.second};
-  return sum.empty() ? tree : sum.apply(tree, add);
+           }}};
 }
 
 /* *******************************************************************************/
-GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
+inline static double PotentiallyPrunedComponentError(
-    const {
+    const GaussianFactorValuePair& pair, const VectorValues& continuousValues) {
-  auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; };
+  return pair.first ? pair.first->error(continuousValues) + pair.second
-  return {factors_, wrap};
+                    : std::numeric_limits<double>::infinity();
-}
-
-/* *******************************************************************************/
-/// Helper method to compute the error of a component.
-static double PotentiallyPrunedComponentError(
-    const GaussianFactor::shared_ptr &gf, const VectorValues &values) {
-  // Check if valid pointer
-  if (gf) {
-    return gf->error(values);
-  } else {
-    // If nullptr this component was pruned, so we return maximum error. This
-    // way the negative exponential will give a probability value close to 0.0.
-    return std::numeric_limits<double>::max();
-  }
 }
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
     const VectorValues& continuousValues) const {
   // functor to convert from sharedFactor to double error value.
-  auto errorFunc = [&continuousValues](const sharedFactor &gf) {
+  auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) {
-    return PotentiallyPrunedComponentError(gf, continuousValues);
+    return PotentiallyPrunedComponentError(pair, continuousValues);
   };
   DecisionTree<Key, double> error_tree(factors_, errorFunc);
   return error_tree;
@@ -239,8 +196,8 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
 /* *******************************************************************************/
 double HybridGaussianFactor::error(const HybridValues& values) const {
   // Directly index to get the component, no need to build the whole tree.
-  const sharedFactor gf = factors_(values.discrete());
+  const GaussianFactorValuePair pair = factors_(values.discrete());
-  return PotentiallyPrunedComponentError(gf, values.continuous());
+  return PotentiallyPrunedComponentError(pair, values.continuous());
 }
 
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianFactor.h

@@ -24,6 +24,7 @@
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridGaussianProductFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
@@ -66,12 +67,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 
   /// typedef for Decision Tree of Gaussian factors and arbitrary value.
   using FactorValuePairs = DecisionTree<Key, GaussianFactorValuePair>;
-  /// typedef for Decision Tree of Gaussian factors.
-  using Factors = DecisionTree<Key, sharedFactor>;
 
  private:
   /// Decision tree of Gaussian factors indexed by discrete keys.
-  Factors factors_;
+  FactorValuePairs factors_;
 
  public:
   /// @name Constructors
@@ -110,10 +109,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.
    *
    * @param discreteKeys Discrete variables and their cardinalities.
-   * @param factors The decision tree of Gaussian factor/scalar pairs.
+   * @param factorPairs The decision tree of Gaussian factor/scalar pairs.
    */
   HybridGaussianFactor(const DiscreteKeys &discreteKeys,
-                       const FactorValuePairs &factors);
+                       const FactorValuePairs &factorPairs);
 
   /// @}
   /// @name Testable
@@ -129,17 +128,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// @{
 
   /// Get factor at a given discrete assignment.
-  sharedFactor operator()(const DiscreteValues &assignment) const;
+  GaussianFactorValuePair operator()(const DiscreteValues &assignment) const;
-
-  /**
-   * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while
-   * maintaining the original tree structure.
-   *
-   * @param sum Decision Tree of Gaussian Factor Graphs indexed by the
-   * variables.
-   * @return Sum
-   */
-  GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
 
   /**
    * @brief Compute error of the HybridGaussianFactor as a tree.
@@ -158,24 +147,16 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   double error(const HybridValues &values) const override;
 
   /// Getter for GaussianFactor decision tree
-  const Factors &factors() const { return factors_; }
+  const FactorValuePairs &factors() const { return factors_; }
-
-  /// Add HybridNonlinearFactor to a Sum, syntactic sugar.
-  friend GaussianFactorGraphTree &operator+=(
-      GaussianFactorGraphTree &sum, const HybridGaussianFactor &factor) {
-    sum = factor.add(sum);
-    return sum;
-  }
-  /// @}
-
- protected:
   /**
    * @brief Helper function to return factors and functional to create a
    * DecisionTree of Gaussian Factor Graphs.
    *
-   * @return GaussianFactorGraphTree
+   * @return HybridGaussianProductFactor
    */
-  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
+  virtual HybridGaussianProductFactor asProductFactor() const;
+
+  /// @}
 
  private:
   /**
@@ -184,10 +165,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * value in the `b` vector as an additional row.
    *
    * @param factors DecisionTree of GaussianFactors and arbitrary scalars.
-   * Gaussian factor in factors.
+   * @return FactorValuePairs
-   * @return HybridGaussianFactor::Factors
    */
-  static Factors augment(const FactorValuePairs &factors);
+  static FactorValuePairs augment(const FactorValuePairs &factors);
 
   /// Helper struct to assist private constructor below.
   struct ConstructorHelper;

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -24,6 +24,7 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
@@ -40,7 +41,6 @@
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>
 
-#include <algorithm>
 #include <cstddef>
 #include <iostream>
 #include <memory>
@@ -48,14 +48,21 @@
 #include <utility>
 #include <vector>
 
+#include "gtsam/discrete/DecisionTreeFactor.h"
+
 namespace gtsam {
 
 /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph:
 template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
 
-using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
-
 using std::dynamic_pointer_cast;
+using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
+using Result =
+    std::pair<std::shared_ptr<GaussianConditional>, GaussianFactor::shared_ptr>;
+using ResultValuePair = std::pair<Result, double>;
+using ResultTree = DecisionTree<Key, ResultValuePair>;
+
+static const VectorValues kEmpty;
 
 /* ************************************************************************ */
 // Throw a runtime exception for method specified in string s, and factor f:
@@ -78,28 +85,57 @@ const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) {
 static void printFactor(const std::shared_ptr<Factor> &factor,
                         const DiscreteValues &assignment,
                         const KeyFormatter &keyFormatter) {
-  if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
+  if (auto hgf = dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
+    if (assignment.empty()) {
+      hgf->print("HybridGaussianFactor:", keyFormatter);
+    } else {
       hgf->operator()(assignment)
-        ->print("HybridGaussianFactor, component:", keyFormatter);
+          .first->print("HybridGaussianFactor, component:", keyFormatter);
-  } else if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
+    }
+  } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
     factor->print("GaussianFactor:\n", keyFormatter);
-  } else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
+
+  } else if (auto df = dynamic_pointer_cast<DiscreteFactor>(factor)) {
     factor->print("DiscreteFactor:\n", keyFormatter);
-  } else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(factor)) {
+  } else if (auto hc = dynamic_pointer_cast<HybridConditional>(factor)) {
     if (hc->isContinuous()) {
       factor->print("GaussianConditional:\n", keyFormatter);
     } else if (hc->isDiscrete()) {
       factor->print("DiscreteConditional:\n", keyFormatter);
+    } else {
+      if (assignment.empty()) {
+        hc->print("HybridConditional:", keyFormatter);
       } else {
         hc->asHybrid()
             ->choose(assignment)
             ->print("HybridConditional, component:\n", keyFormatter);
       }
+    }
   } else {
     factor->print("Unknown factor type\n", keyFormatter);
   }
 }
 
+/* ************************************************************************ */
+void HybridGaussianFactorGraph::print(const std::string &s,
+                                      const KeyFormatter &keyFormatter) const {
+  std::cout << (s.empty() ? "" : s + " ") << std::endl;
+  std::cout << "size: " << size() << std::endl;
+
+  for (size_t i = 0; i < factors_.size(); i++) {
+    auto &&factor = factors_[i];
+    if (factor == nullptr) {
+      std::cout << "Factor " << i << ": nullptr\n";
+      continue;
+    }
+    // Print the factor
+    std::cout << "Factor " << i << "\n";
+    printFactor(factor, {}, keyFormatter);
+    std::cout << "\n";
+  }
+  std::cout.flush();
+}
+
 /* ************************************************************************ */
 void HybridGaussianFactorGraph::printErrors(
     const HybridValues &values, const std::string &str,
@@ -128,42 +164,27 @@ void HybridGaussianFactorGraph::printErrors(
 }
 
 /* ************************************************************************ */
-static GaussianFactorGraphTree addGaussian(
+HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor()
-    const GaussianFactorGraphTree &gfgTree,
+    const {
-    const GaussianFactor::shared_ptr &factor) {
+  HybridGaussianProductFactor result;
-  // If the decision tree is not initialized, then initialize it.
-  if (gfgTree.empty()) {
-    GaussianFactorGraph result{factor};
-    return GaussianFactorGraphTree(result);
-  } else {
-    auto add = [&factor](const GaussianFactorGraph &graph) {
-      auto result = graph;
-      result.push_back(factor);
-      return result;
-    };
-    return gfgTree.apply(add);
-  }
-}
-
-/* ************************************************************************ */
-// TODO(dellaert): it's probably more efficient to first collect the discrete
-// keys, and then loop over all assignments to populate a vector.
-GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
-  GaussianFactorGraphTree result;
 
   for (auto &f : factors_) {
-    // TODO(dellaert): just use a virtual method defined in HybridFactor.
+    // TODO(dellaert): can we make this cleaner and less error-prone?
-    if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
+    if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
-      result = addGaussian(result, gf);
+      continue;  // Ignore OrphanWrapper
+    } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
+      result += gf;
+    } else if (auto gc = dynamic_pointer_cast<GaussianConditional>(f)) {
+      result += gc;
     } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
-      result = gmf->add(result);
+      result += *gmf;
     } else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
-      result = gm->add(result);
+      result += *gm;  // handled above already?
     } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
       if (auto gm = hc->asHybrid()) {
-        result = gm->add(result);
+        result += *gm;
       } else if (auto g = hc->asGaussian()) {
-        result = addGaussian(result, g);
+        result += g;
       } else {
         // Has to be discrete.
         // TODO(dellaert): in C++20, we can use std::visit.
@@ -176,7 +197,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
     } else {
       // TODO(dellaert): there was an unattributed comment here: We need to
       // handle the case where the object is actually an BayesTreeOrphanWrapper!
-      throwRuntimeError("gtsam::assembleGraphTree", f);
+      throwRuntimeError("gtsam::collectProductFactor", f);
     }
   }
 
@@ -208,22 +229,15 @@ continuousElimination(const HybridGaussianFactorGraph &factors,
 }
 
 /* ************************************************************************ */
-/**
+/// Take negative log-values, shift them so that the minimum value is 0, and
- * @brief Exponentiate (not necessarily normalized) negative log-values,
+/// then exponentiate to create a DecisionTreeFactor (not normalized yet!).
- * normalize, and then return as AlgebraicDecisionTree<Key>.
+static DecisionTreeFactor::shared_ptr DiscreteFactorFromErrors(
- *
+    const DiscreteKeys &discreteKeys,
- * @param logValues DecisionTree of (unnormalized) log values.
+    const AlgebraicDecisionTree<Key> &errors) {
- * @return AlgebraicDecisionTree<Key>
+  double min_log = errors.min();
- */
+  AlgebraicDecisionTree<Key> potentials = DecisionTree<Key, double>(
-static AlgebraicDecisionTree<Key> probabilitiesFromNegativeLogValues(
+      errors, [&min_log](const double x) { return exp(-(x - min_log)); });
-    const AlgebraicDecisionTree<Key> &logValues) {
+  return std::make_shared<DecisionTreeFactor>(discreteKeys, potentials);
-  // Perform normalization
-  double min_log = logValues.min();
-  AlgebraicDecisionTree<Key> probabilities = DecisionTree<Key, double>(
-      logValues, [&min_log](const double x) { return exp(-(x - min_log)); });
-  probabilities = probabilities.normalize(probabilities.sum());
-
-  return probabilities;
 }
 
 /* ************************************************************************ */
@@ -237,21 +251,15 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
       dfg.push_back(df);
     } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
       // Case where we have a HybridGaussianFactor with no continuous keys.
-      // In this case, compute discrete probabilities.
+      // In this case, compute a discrete factor from the remaining error.
-      auto logProbability =
+      auto calculateError = [&](const auto &pair) -> double {
-          [&](const GaussianFactor::shared_ptr &factor) -> double {
+        auto [factor, scalar] = pair;
-        // If the factor is null, it is has been pruned hence return ∞
+        // If factor is null, it has been pruned, hence return infinite error
-        // so that the exp(-∞)=0.
         if (!factor) return std::numeric_limits<double>::infinity();
-        return factor->error(VectorValues());
+        return scalar + factor->error(kEmpty);
       };
-      AlgebraicDecisionTree<Key> logProbabilities =
+      DecisionTree<Key, double> errors(gmf->factors(), calculateError);
-          DecisionTree<Key, double>(gmf->factors(), logProbability);
+      dfg.push_back(DiscreteFactorFromErrors(gmf->discreteKeys(), errors));
-
-      AlgebraicDecisionTree<Key> probabilities =
-          probabilitiesFromNegativeLogValues(logProbabilities);
-      dfg.emplace_shared<DecisionTreeFactor>(gmf->discreteKeys(),
-                                             probabilities);
 
     } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
       // Ignore orphaned clique.
@@ -272,24 +280,6 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 }
 
 /* ************************************************************************ */
-// If any GaussianFactorGraph in the decision tree contains a nullptr, convert
-// that leaf to an empty GaussianFactorGraph. Needed since the DecisionTree will
-// otherwise create a GFG with a single (null) factor,
-// which doesn't register as null.
-GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
-  auto emptyGaussian = [](const GaussianFactorGraph &graph) {
-    bool hasNull =
-        std::any_of(graph.begin(), graph.end(),
-                    [](const GaussianFactor::shared_ptr &ptr) { return !ptr; });
-    return hasNull ? GaussianFactorGraph() : graph;
-  };
-  return GaussianFactorGraphTree(sum, emptyGaussian);
-}
-
-/* ************************************************************************ */
-using Result = std::pair<std::shared_ptr<GaussianConditional>,
-                         HybridGaussianFactor::sharedFactor>;
-
 /**
  * Compute the probability p(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
  * from the residual error ||b||^2 at the mean μ.
@@ -297,50 +287,48 @@ using Result = std::pair<std::shared_ptr<GaussianConditional>,
  * depends on the discrete separator if present.
  */
 static std::shared_ptr<Factor> createDiscreteFactor(
-    const DecisionTree<Key, Result> &eliminationResults,
+    const ResultTree &eliminationResults,
     const DiscreteKeys &discreteSeparator) {
-  auto negLogProbability = [&](const Result &pair) -> double {
+  auto calculateError = [&](const auto &pair) -> double {
-    const auto &[conditional, factor] = pair;
+    const auto &[conditional, factor] = pair.first;
-    static const VectorValues kEmpty;
+    const double scalar = pair.second;
-    // If the factor is null, it has been pruned, hence return ∞
+    if (conditional && factor) {
-    // so that the exp(-∞)=0.
+      // `error` has the following contributions:
-    if (!factor) return std::numeric_limits<double>::infinity();
+      // - the scalar is the sum of all mode-dependent constants
-
+      // - factor->error(kempty) is the error remaining after elimination
-    // Negative logspace version of:
+      // - negLogK is what is given to the conditional to normalize
-    // exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
+      const double negLogK = conditional->negLogConstant();
-    // = exp(-factor->error(kEmpty)) * \sqrt{|2πΣ|};
+      return scalar + factor->error(kEmpty) - negLogK;
-    // log = -(-factor->error(kEmpty) + log(\sqrt{|2πΣ|}))
+    } else if (!conditional && !factor) {
-    // = factor->error(kEmpty) - log(\sqrt{|2πΣ|})
+      // If the factor has been pruned, return infinite error
-    // negLogConstant gives `-log(k)`
+      return std::numeric_limits<double>::infinity();
-    // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
+    } else {
-    return factor->error(kEmpty) - conditional->negLogConstant();
+      throw std::runtime_error("createDiscreteFactor has mixed NULLs");
+    }
   };
 
-  AlgebraicDecisionTree<Key> negLogProbabilities(
+  DecisionTree<Key, double> errors(eliminationResults, calculateError);
-      DecisionTree<Key, double>(eliminationResults, negLogProbability));
+  return DiscreteFactorFromErrors(discreteSeparator, errors);
-  AlgebraicDecisionTree<Key> probabilities =
-      probabilitiesFromNegativeLogValues(negLogProbabilities);
-
-  return std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities);
 }
 
+/* *******************************************************************************/
 // Create HybridGaussianFactor on the separator, taking care to correct
 // for conditional constants.
 static std::shared_ptr<Factor> createHybridGaussianFactor(
-    const DecisionTree<Key, Result> &eliminationResults,
+    const ResultTree &eliminationResults,
     const DiscreteKeys &discreteSeparator) {
   // Correct for the normalization constant used up by the conditional
-  auto correct = [&](const Result &pair) -> GaussianFactorValuePair {
+  auto correct = [&](const ResultValuePair &pair) -> GaussianFactorValuePair {
-    const auto &[conditional, factor] = pair;
+    const auto &[conditional, factor] = pair.first;
-    if (factor) {
+    const double scalar = pair.second;
-      auto hf = std::dynamic_pointer_cast<HessianFactor>(factor);
+    if (conditional && factor) {
-      if (!hf) throw std::runtime_error("Expected HessianFactor!");
+      const double negLogK = conditional->negLogConstant();
-      // Add 2.0 term since the constant term will be premultiplied by 0.5
+      return {factor, scalar - negLogK};
-      // as per the Hessian definition,
+    } else if (!conditional && !factor) {
-      // and negative since we want log(k)
+      return {nullptr, std::numeric_limits<double>::infinity()};
-      hf->constantTerm() += -2.0 * conditional->negLogConstant();
+    } else {
+      throw std::runtime_error("createHybridGaussianFactors has mixed NULLs");
     }
-    return {factor, 0.0};
   };
   DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
                                                         correct);
@@ -364,32 +352,40 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
   DiscreteKeys discreteSeparator = GetDiscreteKeys(*this);
 
   // Collect all the factors to create a set of Gaussian factor graphs in a
-  // decision tree indexed by all discrete keys involved.
+  // decision tree indexed by all discrete keys involved. Just like any hybrid
-  GaussianFactorGraphTree factorGraphTree = assembleGraphTree();
+  // factor, every assignment also has a scalar error, in this case the sum of
+  // all errors in the graph. This error is assignment-specific and accounts for
+  // any difference in noise models used.
+  HybridGaussianProductFactor productFactor = collectProductFactor();
 
   // Convert factor graphs with a nullptr to an empty factor graph.
   // This is done after assembly since it is non-trivial to keep track of which
   // FG has a nullptr as we're looping over the factors.
-  factorGraphTree = removeEmpty(factorGraphTree);
+  auto prunedProductFactor = productFactor.removeEmpty();
 
   // This is the elimination method on the leaf nodes
   bool someContinuousLeft = false;
-  auto eliminate = [&](const GaussianFactorGraph &graph) -> Result {
+  auto eliminate = [&](const std::pair<GaussianFactorGraph, double> &pair)
+      -> std::pair<Result, double> {
+    const auto &[graph, scalar] = pair;
+
     if (graph.empty()) {
-      return {nullptr, nullptr};
+      return {{nullptr, nullptr}, 0.0};
     }
 
     // Expensive elimination of product factor.
-    auto result = EliminatePreferCholesky(graph, keys);
+    auto result =
+        EliminatePreferCholesky(graph, keys);  /// <<<<<< MOST COMPUTE IS HERE
 
     // Record whether there any continuous variables left
     someContinuousLeft |= !result.second->empty();
 
-    return result;
+    // We pass on the scalar unmodified.
+    return {result, scalar};
   };
 
   // Perform elimination!
-  DecisionTree<Key, Result> eliminationResults(factorGraphTree, eliminate);
+  ResultTree eliminationResults(prunedProductFactor, eliminate);
 
   // If there are no more continuous parents we create a DiscreteFactor with the
   // error for each discrete choice. Otherwise, create a HybridGaussianFactor
@@ -401,7 +397,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
 
   // Create the HybridGaussianConditional from the conditionals
   HybridGaussianConditional::Conditionals conditionals(
-      eliminationResults, [](const Result &pair) { return pair.first; });
+      eliminationResults,
+      [](const ResultValuePair &pair) { return pair.first.first; });
   auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
       discreteSeparator, conditionals);
 
@@ -484,6 +481,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
       } else if (hybrid_factor->isHybrid()) {
         only_continuous = false;
         only_discrete = false;
+        break;
       }
     } else if (auto cont_factor =
                    std::dynamic_pointer_cast<GaussianFactor>(factor)) {
@@ -520,10 +518,11 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
     } else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
       // If discrete, just add its errorTree as well
       result = result + df->errorTree();
+    } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
+      // For a continuous only factor, just add its error
+      result = result + gf->error(continuousValues);
     } else {
-      // Everything else is a continuous only factor
+      throwRuntimeError("HybridGaussianFactorGraph::errorTree", factor);
-      HybridValues hv(continuousValues, DiscreteValues());
-      result = result + factor->error(hv);  // NOTE: yes, you can add constants
     }
   }
   return result;
@@ -557,9 +556,14 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose(
     } else if (auto gc = std::dynamic_pointer_cast<GaussianConditional>(f)) {
       gfg.push_back(gf);
     } else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
-      gfg.push_back((*hgf)(assignment));
+      gfg.push_back((*hgf)(assignment).first);
     } else if (auto hgc = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
       gfg.push_back((*hgc)(assignment));
+    } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
+      if (auto gc = hc->asGaussian())
+        gfg.push_back(gc);
+      else if (auto hgc = hc->asHybrid())
+        gfg.push_back((*hgc)(assignment));
     } else {
       continue;
     }

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -145,10 +145,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   /// @name Testable
   /// @{
 
-  // TODO(dellaert):  customize print and equals.
+  void print(
-  // void print(
+      const std::string& s = "HybridGaussianFactorGraph",
-  //     const std::string& s = "HybridGaussianFactorGraph",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
-  //     const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /**
    * @brief Print the errors of each factor in the hybrid factor graph.
@@ -218,7 +217,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
    * one for A and one for B. The leaves of the tree will be the Gaussian
    * factors that have only continuous keys.
    */
-  GaussianFactorGraphTree assembleGraphTree() const;
+  HybridGaussianProductFactor collectProductFactor() const;
 
   /**
    * @brief Eliminate the given continuous keys.

gtsam/hybrid/HybridGaussianProductFactor.cpp

@@ -0,0 +1,112 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file HybridGaussianProductFactor.h
+ *  @date Oct 2, 2024
+ *  @author Frank Dellaert
+ *  @author Varun Agrawal
+ */
+
+#include <gtsam/base/types.h>
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridGaussianProductFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+#include <string>
+
+namespace gtsam {
+
+using Y = GaussianFactorGraphValuePair;
+
+/* *******************************************************************************/
+static Y add(const Y& y1, const Y& y2) {
+  GaussianFactorGraph result = y1.first;
+  result.push_back(y2.first);
+  return {result, y1.second + y2.second};
+};
+
+/* *******************************************************************************/
+HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a,
+                                      const HybridGaussianProductFactor& b) {
+  return a.empty() ? b : HybridGaussianProductFactor(a.apply(b, add));
+}
+
+/* *******************************************************************************/
+HybridGaussianProductFactor HybridGaussianProductFactor::operator+(
+    const HybridGaussianFactor& factor) const {
+  return *this + factor.asProductFactor();
+}
+
+/* *******************************************************************************/
+HybridGaussianProductFactor HybridGaussianProductFactor::operator+(
+    const GaussianFactor::shared_ptr& factor) const {
+  return *this + HybridGaussianProductFactor(factor);
+}
+
+/* *******************************************************************************/
+HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=(
+    const GaussianFactor::shared_ptr& factor) {
+  *this = *this + factor;
+  return *this;
+}
+
+/* *******************************************************************************/
+HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=(
+    const HybridGaussianFactor& factor) {
+  *this = *this + factor;
+  return *this;
+}
+
+/* *******************************************************************************/
+void HybridGaussianProductFactor::print(const std::string& s,
+                                        const KeyFormatter& formatter) const {
+  KeySet keys;
+  auto printer = [&](const Y& y) {
+    if (keys.empty()) keys = y.first.keys();
+    return "Graph of size " + std::to_string(y.first.size()) +
+           ", scalar sum: " + std::to_string(y.second);
+  };
+  Base::print(s, formatter, printer);
+  if (!keys.empty()) {
+    std::cout << s << " Keys:";
+    for (auto&& key : keys) std::cout << " " << formatter(key);
+    std::cout << "." << std::endl;
+  }
+}
+
+/* *******************************************************************************/
+bool HybridGaussianProductFactor::equals(
+    const HybridGaussianProductFactor& other, double tol) const {
+  return Base::equals(other, [tol](const Y& a, const Y& b) {
+    return a.first.equals(b.first, tol) && std::abs(a.second - b.second) < tol;
+  });
+}
+
+/* *******************************************************************************/
+HybridGaussianProductFactor HybridGaussianProductFactor::removeEmpty() const {
+  auto emptyGaussian = [](const Y& y) {
+    bool hasNull =
+        std::any_of(y.first.begin(), y.first.end(),
+                    [](const GaussianFactor::shared_ptr& ptr) { return !ptr; });
+    return hasNull ? Y{GaussianFactorGraph(), 0.0} : y;
+  };
+  return {Base(*this, emptyGaussian)};
+}
+
+/* *******************************************************************************/
+std::istream& operator>>(std::istream& is, GaussianFactorGraphValuePair& pair) {
+  // Dummy, don't do anything
+  return is;
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianProductFactor.h

@@ -0,0 +1,147 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file HybridGaussianProductFactor.h
+ *  @date Oct 2, 2024
+ *  @author Frank Dellaert
+ *  @author Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+#include <iostream>
+
+namespace gtsam {
+
+class HybridGaussianFactor;
+
+using GaussianFactorGraphValuePair = std::pair<GaussianFactorGraph, double>;
+
+/// Alias for DecisionTree of GaussianFactorGraphs and their scalar sums
+class GTSAM_EXPORT HybridGaussianProductFactor
+    : public DecisionTree<Key, GaussianFactorGraphValuePair> {
+ public:
+  using Base = DecisionTree<Key, GaussianFactorGraphValuePair>;
+
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  HybridGaussianProductFactor() = default;
+
+  /**
+   * @brief Construct from a single factor
+   * @tparam FACTOR Factor type
+   * @param factor Shared pointer to the factor
+   */
+  template <class FACTOR>
+  HybridGaussianProductFactor(const std::shared_ptr<FACTOR>& factor)
+      : Base(GaussianFactorGraphValuePair{GaussianFactorGraph{factor}, 0.0}) {}
+
+  /**
+   * @brief Construct from DecisionTree
+   * @param tree Decision tree to construct from
+   */
+  HybridGaussianProductFactor(Base&& tree) : Base(std::move(tree)) {}
+
+  ///@}
+
+  /// @name Operators
+  ///@{
+
+  /// Add GaussianFactor into HybridGaussianProductFactor
+  HybridGaussianProductFactor operator+(
+      const GaussianFactor::shared_ptr& factor) const;
+
+  /// Add HybridGaussianFactor into HybridGaussianProductFactor
+  HybridGaussianProductFactor operator+(
+      const HybridGaussianFactor& factor) const;
+
+  /// Add-assign operator for GaussianFactor
+  HybridGaussianProductFactor& operator+=(
+      const GaussianFactor::shared_ptr& factor);
+
+  /// Add-assign operator for HybridGaussianFactor
+  HybridGaussianProductFactor& operator+=(const HybridGaussianFactor& factor);
+
+  ///@}
+
+  /// @name Testable
+  /// @{
+
+  /**
+   * @brief Print the HybridGaussianProductFactor
+   * @param s Optional string to prepend
+   * @param formatter Optional key formatter
+   */
+  void print(const std::string& s = "",
+             const KeyFormatter& formatter = DefaultKeyFormatter) const;
+
+  /**
+   * @brief Check if this HybridGaussianProductFactor is equal to another
+   * @param other The other HybridGaussianProductFactor to compare with
+   * @param tol Tolerance for floating point comparisons
+   * @return true if equal, false otherwise
+   */
+  bool equals(const HybridGaussianProductFactor& other,
+              double tol = 1e-9) const;
+
+  /// @}
+
+  /// @name Other methods
+  ///@{
+
+  /**
+   * @brief Remove empty GaussianFactorGraphs from the decision tree
+   * @return A new HybridGaussianProductFactor with empty GaussianFactorGraphs
+   * removed
+   *
+   * If any GaussianFactorGraph in the decision tree contains a nullptr, convert
+   * that leaf to an empty GaussianFactorGraph with zero scalar sum. This is
+   * needed because the DecisionTree will otherwise create a GaussianFactorGraph
+   * with a single (null) factor, which doesn't register as null.
+   */
+  HybridGaussianProductFactor removeEmpty() const;
+
+  ///@}
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+  }
+#endif
+};
+
+// Testable traits
+template <>
+struct traits<HybridGaussianProductFactor>
+    : public Testable<HybridGaussianProductFactor> {};
+
+/**
+ * Create a dummy overload of >> for GaussianFactorGraphValuePair
+ * so that HybridGaussianProductFactor compiles
+ * with the constructor
+ * `DecisionTree(const std::vector<LabelC>& labelCs, const std::string& table)`.
+ *
+ * Needed to compile on Windows.
+ */
+std::istream& operator>>(std::istream& is, GaussianFactorGraphValuePair& pair);
+
+}  // namespace gtsam

gtsam/hybrid/HybridNonlinearFactorGraph.cpp

@@ -210,4 +210,16 @@ AlgebraicDecisionTree<Key> HybridNonlinearFactorGraph::errorTree(
   return result;
 }
 
+/* ************************************************************************ */
+AlgebraicDecisionTree<Key> HybridNonlinearFactorGraph::discretePosterior(
+    const Values& continuousValues) const {
+  AlgebraicDecisionTree<Key> errors = this->errorTree(continuousValues);
+  AlgebraicDecisionTree<Key> p = errors.apply([](double error) {
+    // NOTE: The 0.5 term is handled by each factor
+    return exp(-error);
+  });
+  return p / p.sum();
+}
+
+/* ************************************************************************ */
 }  // namespace gtsam

gtsam/hybrid/HybridNonlinearFactorGraph.h

@@ -103,6 +103,19 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
    */
   AlgebraicDecisionTree<Key> errorTree(const Values& values) const;
 
+  /**
+   * @brief Computer posterior P(M|X=x) when all continuous values X are given.
+   * This is efficient as this simply takes -exp(.) of errorTree and normalizes.
+   *
+   * @note Not a DiscreteConditional as the cardinalities of the DiscreteKeys,
+   * which we would need, are hard to recover.
+   *
+   * @param continuousValues Continuous values x to condition on.
+   * @return DecisionTreeFactor
+   */
+  AlgebraicDecisionTree<Key> discretePosterior(
+      const Values& continuousValues) const;
+
   /// @}
 };
 

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -60,17 +60,6 @@ double prob_m_z(double mu0, double mu1, double sigma0, double sigma1,
   return p1 / (p0 + p1);
 };
 
-/// Given \phi(m;z)\phi(m) use eliminate to obtain P(m|z).
-DiscreteConditional SolveHFG(const HybridGaussianFactorGraph &hfg) {
-  return *hfg.eliminateSequential()->at(0)->asDiscrete();
-}
-
-/// Given p(z,m) and z, convert to HFG and solve.
-DiscreteConditional SolveHBN(const HybridBayesNet &hbn, double z) {
-  VectorValues given{{Z(0), Vector1(z)}};
-  return SolveHFG(hbn.toFactorGraph(given));
-}
-
 /*
  * Test a Gaussian Mixture Model P(m)p(z|m) with same sigma.
  * The posterior, as a function of z, should be a sigmoid function.
@@ -88,7 +77,9 @@ TEST(GaussianMixture, GaussianMixtureModel) {
 
   // At the halfway point between the means, we should get P(m|z)=0.5
   double midway = mu1 - mu0;
-  auto pMid = SolveHBN(gmm, midway);
+  auto eliminationResult =
+      gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential();
+  auto pMid = *eliminationResult->at(0)->asDiscrete();
   EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid));
 
   // Everywhere else, the result should be a sigmoid.
@@ -97,7 +88,9 @@ TEST(GaussianMixture, GaussianMixtureModel) {
     const double expected = prob_m_z(mu0, mu1, sigma, sigma, z);
 
     // Workflow 1: convert HBN to HFG and solve
-    auto posterior1 = SolveHBN(gmm, z);
+    auto eliminationResult1 =
+        gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
+    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -105,7 +98,8 @@ TEST(GaussianMixture, GaussianMixtureModel) {
     hfg1.emplace_shared<DecisionTreeFactor>(
         m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)});
     hfg1.push_back(mixing);
-    auto posterior2 = SolveHFG(hfg1);
+    auto eliminationResult2 = hfg1.eliminateSequential();
+    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
@@ -128,7 +122,23 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
   // 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;
-  auto pMax = SolveHBN(gmm, zMax);
+  const VectorValues vv{{Z(0), Vector1(zMax)}};
+  auto gfg = gmm.toFactorGraph(vv);
+
+  // Equality of posteriors asserts that the elimination is correct (same ratios
+  // for all modes)
+  const auto& expectedDiscretePosterior = gmm.discretePosterior(vv);
+  EXPECT(assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
+
+  // Eliminate the graph!
+  auto eliminationResultMax = gfg.eliminateSequential();
+
+  // Equality of posteriors asserts that the elimination is correct (same ratios
+  // for all modes)
+  EXPECT(assert_equal(expectedDiscretePosterior,
+                      eliminationResultMax->discretePosterior(vv)));
+
+  auto pMax = *eliminationResultMax->at(0)->asDiscrete();
   EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4));
 
   // Everywhere else, the result should be a bell curve like function.
@@ -137,7 +147,9 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z);
 
     // Workflow 1: convert HBN to HFG and solve
-    auto posterior1 = SolveHBN(gmm, z);
+    auto eliminationResult1 =
+        gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential();
+    auto posterior1 = *eliminationResult1->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
 
     // Workflow 2: directly specify HFG and solve
@@ -145,11 +157,11 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
     hfg.emplace_shared<DecisionTreeFactor>(
         m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)});
     hfg.push_back(mixing);
-    auto posterior2 = SolveHFG(hfg);
+    auto eliminationResult2 = hfg.eliminateSequential();
+    auto posterior2 = *eliminationResult2->at(0)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
   }
 }
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -18,9 +18,12 @@
  * @date    December 2021
  */
 
+#include <gtsam/base/Testable.h>
+#include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 #include "Switching.h"
@@ -28,6 +31,7 @@
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
+#include <memory>
 
 using namespace std;
 using namespace gtsam;
@@ -113,7 +117,7 @@ TEST(HybridBayesNet, EvaluatePureDiscrete) {
 }
 
 /* ****************************************************************************/
-// Test creation of a tiny hybrid Bayes net.
+// Test API for a tiny hybrid Bayes net.
 TEST(HybridBayesNet, Tiny) {
   auto bayesNet = tiny::createHybridBayesNet();  // P(z|x,mode)P(x)P(mode)
   EXPECT_LONGS_EQUAL(3, bayesNet.size());
@@ -189,6 +193,20 @@ TEST(HybridBayesNet, Tiny) {
   auto fg = bayesNet.toFactorGraph({{Z(0), Vector1(5.0)}});
   EXPECT_LONGS_EQUAL(3, fg.size());
 
+  // Create the product factor for eliminating x0:
+  HybridGaussianFactorGraph factors_x0;
+  factors_x0.push_back(fg.at(0));
+  factors_x0.push_back(fg.at(1));
+  auto productFactor = factors_x0.collectProductFactor();
+
+  // Check that scalars are 0 and 1.79 (regression)
+  EXPECT_DOUBLES_EQUAL(0.0, productFactor({{M(0), 0}}).second, 1e-9);
+  EXPECT_DOUBLES_EQUAL(1.791759, productFactor({{M(0), 1}}).second, 1e-5);
+
+  // Call eliminate and check scalar:
+  auto result = factors_x0.eliminate({X(0)});
+  auto df = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
+
   // Check that the ratio of probPrime to evaluate is the same for all modes.
   std::vector<double> ratio(2);
   ratio[0] = std::exp(-fg.error(zero)) / bayesNet.evaluate(zero);

gtsam/hybrid/tests/testHybridBayesTree.cpp

@@ -20,6 +20,9 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridGaussianISAM.h>
+#include <gtsam/inference/DotWriter.h>
+
+#include <numeric>
 
 #include "Switching.h"
 
@@ -28,9 +31,320 @@
 
 using namespace std;
 using namespace gtsam;
-using noiseModel::Isotropic;
+using symbol_shorthand::D;
 using symbol_shorthand::M;
 using symbol_shorthand::X;
+using symbol_shorthand::Y;
+
+static const DiscreteKey m0(M(0), 2), m1(M(1), 2), m2(M(2), 2), m3(M(3), 2);
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
+  // Test multifrontal elimination
+  HybridGaussianFactorGraph hfg;
+
+  // Add priors on x0 and c1
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(DecisionTreeFactor(m1, {2, 8}));
+
+  Ordering ordering;
+  ordering.push_back(X(0));
+  auto result = hfg.eliminatePartialMultifrontal(ordering);
+
+  EXPECT_LONGS_EQUAL(result.first->size(), 1);
+  EXPECT_LONGS_EQUAL(result.second->size(), 1);
+}
+
+/* ************************************************************************* */
+namespace two {
+std::vector<GaussianFactor::shared_ptr> components(Key key) {
+  return {std::make_shared<JacobianFactor>(key, I_3x3, Z_3x1),
+          std::make_shared<JacobianFactor>(key, I_3x3, Vector3::Ones())};
+}
+}  // namespace two
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph,
+     HybridGaussianFactorGraphEliminateFullMultifrontalSimple) {
+  HybridGaussianFactorGraph hfg;
+
+  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(m1, two::components(X(1))));
+
+  hfg.add(DecisionTreeFactor(m1, {2, 8}));
+  // TODO(Varun) Adding extra discrete variable not connected to continuous
+  // variable throws segfault
+  //  hfg.add(DecisionTreeFactor({m1, m2, "1 2 3 4"));
+
+  HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal();
+
+  // The bayes tree should have 3 cliques
+  EXPECT_LONGS_EQUAL(3, result->size());
+}
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
+  HybridGaussianFactorGraph hfg;
+
+  // Prior on x0
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  // Factor between x0-x1
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  // Hybrid factor P(x1|c1)
+  hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
+  // Prior factor on c1
+  hfg.add(DecisionTreeFactor(m1, {2, 8}));
+
+  // Get a constrained ordering keeping c1 last
+  auto ordering_full = HybridOrdering(hfg);
+
+  // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
+  HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
+
+  EXPECT_LONGS_EQUAL(3, hbt->size());
+}
+
+/* ************************************************************************* */
+// Check assembling the Bayes Tree roots after we do partial elimination
+TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
+  HybridGaussianFactorGraph hfg;
+
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
+
+  hfg.add(HybridGaussianFactor(m0, two::components(X(0))));
+  hfg.add(HybridGaussianFactor(m1, two::components(X(2))));
+
+  hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4"));
+
+  hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
+
+  hfg.add(HybridGaussianFactor(m3, two::components(X(3))));
+  hfg.add(HybridGaussianFactor(m2, two::components(X(5))));
+
+  auto ordering_full =
+      Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)});
+
+  const auto [hbt, remaining] = hfg.eliminatePartialMultifrontal(ordering_full);
+
+  // 9 cliques in the bayes tree and 0 remaining variables to eliminate.
+  EXPECT_LONGS_EQUAL(9, hbt->size());
+  EXPECT_LONGS_EQUAL(0, remaining->size());
+
+  /*
+  (Fan) Explanation: the Junction tree will need to re-eliminate to get to the
+  marginal on X(1), which is not possible because it involves eliminating
+  discrete before continuous. The solution to this, however, is in Murphy02.
+  TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable.
+  And I believe that we should do this.
+  */
+}
+
+/* ************************************************************************* */
+void dotPrint(const HybridGaussianFactorGraph::shared_ptr& hfg,
+              const HybridBayesTree::shared_ptr& hbt,
+              const Ordering& ordering) {
+  DotWriter dw;
+  dw.positionHints['c'] = 2;
+  dw.positionHints['x'] = 1;
+  std::cout << hfg->dot(DefaultKeyFormatter, dw);
+  std::cout << "\n";
+  hbt->dot(std::cout);
+
+  std::cout << "\n";
+  std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw);
+}
+
+/* ************************************************************************* */
+// TODO(fan): make a graph like Varun's paper one
+TEST(HybridGaussianFactorGraph, Switching) {
+  auto N = 12;
+  auto hfg = makeSwitchingChain(N);
+
+  // X(5) will be the center, X(1-4), X(6-9)
+  // X(3), X(7)
+  // X(2), X(8)
+  // X(1), X(4), X(6), X(9)
+  // M(5) will be the center, M(1-4), M(6-8)
+  // M(3), M(7)
+  // M(1), M(4), M(2), M(6), M(8)
+  // auto ordering_full =
+  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
+  //     X(5),
+  //                        M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
+  KeyVector ordering;
+
+  {
+    std::vector<int> naturalX(N);
+    std::iota(naturalX.begin(), naturalX.end(), 1);
+    std::vector<Key> ordX;
+    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
+                   [](int x) { return X(x); });
+
+    auto [ndX, lvls] = makeBinaryOrdering(ordX);
+    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
+    // TODO(dellaert): this has no effect!
+    for (auto& l : lvls) {
+      l = -l;
+    }
+  }
+  {
+    std::vector<int> naturalC(N - 1);
+    std::iota(naturalC.begin(), naturalC.end(), 1);
+    std::vector<Key> ordC;
+    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
+                   [](int x) { return M(x); });
+
+    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
+    const auto [ndC, lvls] = makeBinaryOrdering(ordC);
+    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
+  }
+  auto ordering_full = Ordering(ordering);
+
+  const auto [hbt, remaining] =
+      hfg->eliminatePartialMultifrontal(ordering_full);
+
+  // 12 cliques in the bayes tree and 0 remaining variables to eliminate.
+  EXPECT_LONGS_EQUAL(12, hbt->size());
+  EXPECT_LONGS_EQUAL(0, remaining->size());
+}
+
+/* ************************************************************************* */
+// TODO(fan): make a graph like Varun's paper one
+TEST(HybridGaussianFactorGraph, SwitchingISAM) {
+  auto N = 11;
+  auto hfg = makeSwitchingChain(N);
+
+  // X(5) will be the center, X(1-4), X(6-9)
+  // X(3), X(7)
+  // X(2), X(8)
+  // X(1), X(4), X(6), X(9)
+  // M(5) will be the center, M(1-4), M(6-8)
+  // M(3), M(7)
+  // M(1), M(4), M(2), M(6), M(8)
+  // auto ordering_full =
+  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
+  //     X(5),
+  //                        M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
+  KeyVector ordering;
+
+  {
+    std::vector<int> naturalX(N);
+    std::iota(naturalX.begin(), naturalX.end(), 1);
+    std::vector<Key> ordX;
+    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
+                   [](int x) { return X(x); });
+
+    auto [ndX, lvls] = makeBinaryOrdering(ordX);
+    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
+    // TODO(dellaert): this has no effect!
+    for (auto& l : lvls) {
+      l = -l;
+    }
+  }
+  {
+    std::vector<int> naturalC(N - 1);
+    std::iota(naturalC.begin(), naturalC.end(), 1);
+    std::vector<Key> ordC;
+    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
+                   [](int x) { return M(x); });
+
+    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
+    const auto [ndC, lvls] = makeBinaryOrdering(ordC);
+    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
+  }
+  auto ordering_full = Ordering(ordering);
+
+  const auto [hbt, remaining] =
+      hfg->eliminatePartialMultifrontal(ordering_full);
+
+  auto new_fg = makeSwitchingChain(12);
+  auto isam = HybridGaussianISAM(*hbt);
+
+  // Run an ISAM update.
+  HybridGaussianFactorGraph factorGraph;
+  factorGraph.push_back(new_fg->at(new_fg->size() - 2));
+  factorGraph.push_back(new_fg->at(new_fg->size() - 1));
+  isam.update(factorGraph);
+
+  // ISAM should have 12 factors after the last update
+  EXPECT_LONGS_EQUAL(12, isam.size());
+}
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
+  const int N = 7;
+  auto hfg = makeSwitchingChain(N, X);
+  hfg->push_back(*makeSwitchingChain(N, Y, D));
+
+  for (int t = 1; t <= N; t++) {
+    hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0)));
+  }
+
+  KeyVector ordering;
+
+  KeyVector naturalX(N);
+  std::iota(naturalX.begin(), naturalX.end(), 1);
+  KeyVector ordX;
+  for (size_t i = 1; i <= N; i++) {
+    ordX.emplace_back(X(i));
+    ordX.emplace_back(Y(i));
+  }
+
+  for (size_t i = 1; i <= N - 1; i++) {
+    ordX.emplace_back(M(i));
+  }
+  for (size_t i = 1; i <= N - 1; i++) {
+    ordX.emplace_back(D(i));
+  }
+
+  {
+    DotWriter dw;
+    dw.positionHints['x'] = 1;
+    dw.positionHints['c'] = 0;
+    dw.positionHints['d'] = 3;
+    dw.positionHints['y'] = 2;
+    // std::cout << hfg->dot(DefaultKeyFormatter, dw);
+    // std::cout << "\n";
+  }
+
+  {
+    DotWriter dw;
+    dw.positionHints['y'] = 9;
+    // dw.positionHints['c'] = 0;
+    // dw.positionHints['d'] = 3;
+    dw.positionHints['x'] = 1;
+    // std::cout << "\n";
+    // std::cout << hfg->eliminateSequential(Ordering(ordX))
+    //                  ->dot(DefaultKeyFormatter, dw);
+    // hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout);
+  }
+
+  Ordering ordering_partial;
+  for (size_t i = 1; i <= N; i++) {
+    ordering_partial.emplace_back(X(i));
+    ordering_partial.emplace_back(Y(i));
+  }
+  const auto [hbn, remaining] =
+      hfg->eliminatePartialSequential(ordering_partial);
+
+  EXPECT_LONGS_EQUAL(14, hbn->size());
+  EXPECT_LONGS_EQUAL(11, remaining->size());
+
+  {
+    DotWriter dw;
+    dw.positionHints['x'] = 1;
+    dw.positionHints['c'] = 0;
+    dw.positionHints['d'] = 3;
+    dw.positionHints['y'] = 2;
+    // std::cout << remaining->dot(DefaultKeyFormatter, dw);
+    // std::cout << "\n";
+  }
+}
 
 /* ****************************************************************************/
 // Test multifrontal optimize

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -37,8 +37,6 @@
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
 
-#include <bitset>
-
 #include "Switching.h"
 
 using namespace std;
@@ -517,8 +515,6 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
   // the normalizing term computed via the Bayes net determinant.
   const HybridValues sample = bn->sample(&rng);
   double expected_ratio = compute_ratio(sample);
-  // regression
-  EXPECT_DOUBLES_EQUAL(0.728588, expected_ratio, 1e-6);
 
   // 3. Do sampling
   constexpr int num_samples = 10;

gtsam/hybrid/tests/testHybridGaussianConditional.cpp

@@ -18,6 +18,8 @@
  * @date    December 2021
  */
 
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
@@ -28,9 +30,6 @@
 #include <memory>
 #include <vector>
 
-#include "gtsam/discrete/DecisionTree.h"
-#include "gtsam/discrete/DiscreteKey.h"
-
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
 
@@ -217,30 +216,16 @@ TEST(HybridGaussianConditional, Likelihood2) {
   // Check the detailed JacobianFactor calculation for mode==1.
   {
     // We have a JacobianFactor
-    const auto gf1 = (*likelihood)(assignment1);
+    const auto [gf1, _] = (*likelihood)(assignment1);
     const auto jf1 = std::dynamic_pointer_cast<JacobianFactor>(gf1);
     CHECK(jf1);
 
-    // It has 2 rows, not 1!
+    // Check that the JacobianFactor error with constants is equal to the
-    CHECK(jf1->rows() == 2);
+    // conditional error:
-
+    EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1),
-    // Check that the constant C1 is properly encoded in the JacobianFactor.
+                         jf1->error(hv1) + conditionals[1]->negLogConstant() -
-    const double C1 =
+                             hybrid_conditional.negLogConstant(),
-        conditionals[1]->negLogConstant() - hybrid_conditional.negLogConstant();
+                         1e-8);
-    const double c1 = std::sqrt(2.0 * C1);
-    Vector expected_unwhitened(2);
-    expected_unwhitened << 4.9 - 5.0, -c1;
-    Vector actual_unwhitened = jf1->unweighted_error(vv);
-    EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));
-
-    // Make sure the noise model does not touch it.
-    Vector expected_whitened(2);
-    expected_whitened << (4.9 - 5.0) / 3.0, -c1;
-    Vector actual_whitened = jf1->error_vector(vv);
-    EXPECT(assert_equal(expected_whitened, actual_whitened));
-
-    // Check that the error is equal to the conditional error:
-    EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), jf1->error(hv1), 1e-8);
   }
 
   // Check that the ratio of probPrime to evaluate is the same for all modes.

gtsam/hybrid/tests/testHybridGaussianFactor.cpp

@@ -82,40 +82,25 @@ TEST(HybridGaussianFactor, ConstructorVariants) {
 }
 
 /* ************************************************************************* */
-// "Add" two hybrid factors together.
+TEST(HybridGaussianFactor, Keys) {
-TEST(HybridGaussianFactor, Sum) {
   using namespace test_constructor;
-  DiscreteKey m2(2, 3);
-
-  auto A3 = Matrix::Zero(2, 3);
-  auto f20 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
-  auto f21 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
-  auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
-
-  // 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(m1, {f10, f11});
-  HybridGaussianFactor hybridFactorB(m2, {f20, f21, f22});
-
   // Check the number of keys matches what we expect
   EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size());
   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
+  DiscreteKey m2(2, 3);
-  // discrete variables m1 and m2:
+  auto A3 = Matrix::Zero(2, 3);
-  GaussianFactorGraphTree sum;
+  auto f20 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
-  sum += hybridFactorA;
+  auto f21 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
-  sum += hybridFactorB;
+  auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+  HybridGaussianFactor hybridFactorB(m2, {f20, f21, f22});
 
-  // Let's check that this worked:
+  // Check the number of keys matches what we expect
-  Assignment<Key> mode;
+  EXPECT_LONGS_EQUAL(3, hybridFactorB.keys().size());
-  mode[m1.first] = 1;
+  EXPECT_LONGS_EQUAL(2, hybridFactorB.continuousKeys().size());
-  mode[m2.first] = 2;
+  EXPECT_LONGS_EQUAL(1, hybridFactorB.discreteKeys().size());
-  auto actual = sum(mode);
-  EXPECT(actual.at(0) == f11);
-  EXPECT(actual.at(1) == f22);
 }
 
 /* ************************************************************************* */
@@ -124,8 +109,7 @@ TEST(HybridGaussianFactor, Printing) {
   HybridGaussianFactor hybridFactor(m1, {f10, f11});
 
   std::string expected =
-      R"(HybridGaussianFactor
+      R"(Hybrid [x1 x2; 1]{
-Hybrid [x1 x2; 1]{
  Choice(1) 
  0 Leaf :
   A[x1] = [
@@ -138,6 +122,7 @@ Hybrid [x1 x2; 1]{
 ]
   b = [ 0 0 ]
   No noise model
+scalar: 0
 
  1 Leaf :
   A[x1] = [
@@ -150,6 +135,7 @@ Hybrid [x1 x2; 1]{
 ]
   b = [ 0 0 ]
   No noise model
+scalar: 0
 
 }
 )";

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -13,38 +13,34 @@
  *  @file testHybridGaussianFactorGraph.cpp
  *  @date Mar 11, 2022
  *  @author Fan Jiang
+ *  @author Varun Agrawal
+ *  @author Frank Dellaert
  */
 
 #include <CppUnitLite/Test.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianConditional.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
-#include <gtsam/hybrid/HybridGaussianISAM.h>
+#include <gtsam/hybrid/HybridGaussianProductFactor.h>
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/inference/BayesNet.h>
-#include <gtsam/inference/DotWriter.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 
-#include <algorithm>
 #include <cstddef>
-#include <functional>
-#include <iostream>
-#include <iterator>
 #include <memory>
-#include <numeric>
 #include <vector>
 
 #include "Switching.h"
@@ -53,17 +49,15 @@
 using namespace std;
 using namespace gtsam;
 
-using gtsam::symbol_shorthand::D;
 using gtsam::symbol_shorthand::M;
 using gtsam::symbol_shorthand::N;
 using gtsam::symbol_shorthand::X;
-using gtsam::symbol_shorthand::Y;
 using gtsam::symbol_shorthand::Z;
 
 // Set up sampling
 std::mt19937_64 kRng(42);
 
-static const DiscreteKey m1(M(1), 2);
+static const DiscreteKey m0(M(0), 2), m1(M(1), 2), m2(M(2), 2);
 
 /* ************************************************************************* */
 TEST(HybridGaussianFactorGraph, Creation) {
@@ -76,7 +70,7 @@ TEST(HybridGaussianFactorGraph, Creation) {
   // Define a hybrid gaussian conditional P(x0|x1, c0)
   // and add it to the factor graph.
   HybridGaussianConditional gm(
-      {M(0), 2},
+      m0,
       {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)});
@@ -97,22 +91,6 @@ TEST(HybridGaussianFactorGraph, EliminateSequential) {
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
-/* ************************************************************************* */
-TEST(HybridGaussianFactorGraph, EliminateMultifrontal) {
-  // Test multifrontal elimination
-  HybridGaussianFactorGraph hfg;
-
-  // Add priors on x0 and c1
-  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
-  hfg.add(DecisionTreeFactor(m1, {2, 8}));
-
-  Ordering ordering;
-  ordering.push_back(X(0));
-  auto result = hfg.eliminatePartialMultifrontal(ordering);
-
-  EXPECT_LONGS_EQUAL(result.first->size(), 1);
-  EXPECT_LONGS_EQUAL(result.second->size(), 1);
-}
 /* ************************************************************************* */
 
 namespace two {
@@ -138,7 +116,8 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
   // Check that factor is discrete and correct
   auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
   CHECK(factor);
-  EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor));
+  // regression test
+  EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -178,7 +157,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
   // Discrete probability table for c1
   hfg.add(DecisionTreeFactor(m1, {2, 8}));
   // Joint discrete probability table for c1, c2
-  hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
+  hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4"));
 
   HybridBayesNet::shared_ptr result = hfg.eliminateSequential();
 
@@ -187,295 +166,219 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
 }
 
 /* ************************************************************************* */
-TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
+// Test API for the smallest switching network.
-  HybridGaussianFactorGraph hfg;
+// None of these are regression tests.
+TEST(HybridBayesNet, Switching) {
+  // Create switching network with two continuous variables and one discrete:
+  // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1;z1) ϕ(m0)
+  const double betweenSigma = 0.3, priorSigma = 0.1;
+  Switching s(2, betweenSigma, priorSigma);
 
-  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  // Check size of linearized factor graph
-  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+  const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph;
+  EXPECT_LONGS_EQUAL(4, graph.size());
 
-  hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(1))));
+  // Create some continuous and discrete values
+  const VectorValues continuousValues{{X(0), Vector1(0.1)},
+                                      {X(1), Vector1(1.2)}};
+  const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}};
 
-  hfg.add(DecisionTreeFactor(m1, {2, 8}));
+  // Get the hybrid gaussian factor and check it is as expected
-  // TODO(Varun) Adding extra discrete variable not connected to continuous
+  auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(graph.at(1));
-  // variable throws segfault
+  CHECK(hgf);
-  //  hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
 
-  HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal();
+  // Get factors and scalars for both modes
+  auto [factor0, scalar0] = (*hgf)(modeZero);
+  auto [factor1, scalar1] = (*hgf)(modeOne);
+  CHECK(factor0);
+  CHECK(factor1);
 
-  // The bayes tree should have 3 cliques
+  // Check scalars against negLogConstant of noise model
-  EXPECT_LONGS_EQUAL(3, result->size());
+  auto betweenModel = noiseModel::Isotropic::Sigma(1, betweenSigma);
-  // GTSAM_PRINT(*result);
+  EXPECT_DOUBLES_EQUAL(betweenModel->negLogConstant(), scalar0, 1e-9);
-  // GTSAM_PRINT(*result->marginalFactor(M(2)));
+  EXPECT_DOUBLES_EQUAL(betweenModel->negLogConstant(), scalar1, 1e-9);
+
+  // Check error for M(0) = 0
+  const HybridValues values0{continuousValues, modeZero};
+  double expectedError0 = 0;
+  for (const auto &factor : graph) expectedError0 += factor->error(values0);
+  EXPECT_DOUBLES_EQUAL(expectedError0, graph.error(values0), 1e-5);
+
+  // Check error for M(0) = 1
+  const HybridValues values1{continuousValues, modeOne};
+  double expectedError1 = 0;
+  for (const auto &factor : graph) expectedError1 += factor->error(values1);
+  EXPECT_DOUBLES_EQUAL(expectedError1, graph.error(values1), 1e-5);
+
+  // Check errorTree
+  AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues);
+  // Create expected error tree
+  const AlgebraicDecisionTree<Key> expectedErrors(M(0), expectedError0,
+                                                  expectedError1);
+
+  // Check that the actual error tree matches the expected one
+  EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5));
+
+  // Check probPrime
+  const double probPrime0 = graph.probPrime(values0);
+  EXPECT_DOUBLES_EQUAL(std::exp(-expectedError0), probPrime0, 1e-5);
+
+  const double probPrime1 = graph.probPrime(values1);
+  EXPECT_DOUBLES_EQUAL(std::exp(-expectedError1), probPrime1, 1e-5);
+
+  // Check discretePosterior
+  const AlgebraicDecisionTree<Key> graphPosterior =
+      graph.discretePosterior(continuousValues);
+  const double sum = probPrime0 + probPrime1;
+  const AlgebraicDecisionTree<Key> expectedPosterior(M(0), probPrime0 / sum,
+                                                     probPrime1 / sum);
+  EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5));
+
+  // Make the clique of factors connected to x0:
+  HybridGaussianFactorGraph factors_x0;
+  factors_x0.push_back(graph.at(0));
+  factors_x0.push_back(hgf);
+
+  // Test collectProductFactor
+  auto productFactor = factors_x0.collectProductFactor();
+
+  // For M(0) = 0
+  auto [gaussianFactor0, actualScalar0] = productFactor(modeZero);
+  EXPECT(gaussianFactor0.size() == 2);
+  EXPECT_DOUBLES_EQUAL((*hgf)(modeZero).second, actualScalar0, 1e-5);
+
+  // For M(0) = 1
+  auto [gaussianFactor1, actualScalar1] = productFactor(modeOne);
+  EXPECT(gaussianFactor1.size() == 2);
+  EXPECT_DOUBLES_EQUAL((*hgf)(modeOne).second, actualScalar1, 1e-5);
+
+  // Test eliminate x0
+  const Ordering ordering{X(0)};
+  auto [conditional, factor] = factors_x0.eliminate(ordering);
+
+  // Check the conditional
+  CHECK(conditional);
+  EXPECT(conditional->isHybrid());
+  auto p_x0_given_x1_m = conditional->asHybrid();
+  CHECK(p_x0_given_x1_m);
+  EXPECT(HybridGaussianConditional::CheckInvariants(*p_x0_given_x1_m, values1));
+  EXPECT_LONGS_EQUAL(1, p_x0_given_x1_m->nrFrontals());  // x0
+  EXPECT_LONGS_EQUAL(2, p_x0_given_x1_m->nrParents());   // x1, m0
+
+  // Check the remaining factor
+  EXPECT(factor);
+  EXPECT(std::dynamic_pointer_cast<HybridGaussianFactor>(factor));
+  auto phi_x1_m = std::dynamic_pointer_cast<HybridGaussianFactor>(factor);
+  EXPECT_LONGS_EQUAL(2, phi_x1_m->keys().size());  // x1, m0
+  // Check that the scalars incorporate the negative log constant of the
+  // conditional
+  EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(),
+                       (*phi_x1_m)(modeZero).second, 1e-9);
+  EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(),
+                       (*phi_x1_m)(modeOne).second, 1e-9);
+
+  // Check that the conditional and remaining factor are consistent for both
+  // modes
+  for (auto &&mode : {modeZero, modeOne}) {
+    const auto gc = (*p_x0_given_x1_m)(mode);
+    const auto [gf, scalar] = (*phi_x1_m)(mode);
+
+    // The error of the original factors should equal the sum of errors of the
+    // conditional and remaining factor, modulo the normalization constant of
+    // the conditional.
+    double originalError = factors_x0.error({continuousValues, mode});
+    const double actualError = gc->negLogConstant() +
+                               gc->error(continuousValues) +
+                               gf->error(continuousValues) + scalar;
+    EXPECT_DOUBLES_EQUAL(originalError, actualError, 1e-9);
   }
 
-/* ************************************************************************* */
+  // Create a clique for x1
-TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
+  HybridGaussianFactorGraph factors_x1;
-  HybridGaussianFactorGraph hfg;
+  factors_x1.push_back(
+      factor);  // Use the remaining factor from previous elimination
+  factors_x1.push_back(
+      graph.at(2));  // Add the factor for x1 from the original graph
 
-  // Prior on x0
+  // Test collectProductFactor for x1 clique
-  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  auto productFactor_x1 = factors_x1.collectProductFactor();
-  // Factor between x0-x1
-  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
-  // Hybrid factor P(x1|c1)
+  // For M(0) = 0
-  hfg.add(HybridGaussianFactor(m1, two::components(X(1))));
+  auto [gaussianFactor_x1_0, actualScalar_x1_0] = productFactor_x1(modeZero);
-  // Prior factor on c1
+  EXPECT_LONGS_EQUAL(2, gaussianFactor_x1_0.size());
-  hfg.add(DecisionTreeFactor(m1, {2, 8}));
+  // NOTE(Frank): prior on x1 does not contribute to the scalar
+  EXPECT_DOUBLES_EQUAL((*phi_x1_m)(modeZero).second, actualScalar_x1_0, 1e-5);
 
-  // Get a constrained ordering keeping c1 last
+  // For M(0) = 1
-  auto ordering_full = HybridOrdering(hfg);
+  auto [gaussianFactor_x1_1, actualScalar_x1_1] = productFactor_x1(modeOne);
+  EXPECT_LONGS_EQUAL(2, gaussianFactor_x1_1.size());
+  // NOTE(Frank): prior on x1 does not contribute to the scalar
+  EXPECT_DOUBLES_EQUAL((*phi_x1_m)(modeOne).second, actualScalar_x1_1, 1e-5);
 
-  // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1)
+  // Test eliminate for x1 clique
-  HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
+  Ordering ordering_x1{X(1)};
+  auto [conditional_x1, factor_x1] = factors_x1.eliminate(ordering_x1);
 
-  EXPECT_LONGS_EQUAL(3, hbt->size());
+  // Check the conditional for x1
-}
+  CHECK(conditional_x1);
+  EXPECT(conditional_x1->isHybrid());
+  auto p_x1_given_m = conditional_x1->asHybrid();
+  CHECK(p_x1_given_m);
+  EXPECT_LONGS_EQUAL(1, p_x1_given_m->nrFrontals());  // x1
+  EXPECT_LONGS_EQUAL(1, p_x1_given_m->nrParents());   // m0
 
-/* ************************************************************************* */
+  // Check the remaining factor for x1
-/*
+  CHECK(factor_x1);
- * This test is about how to assemble the Bayes Tree roots after we do partial
+  auto phi_x1 = std::dynamic_pointer_cast<DecisionTreeFactor>(factor_x1);
- * elimination
+  CHECK(phi_x1);
- */
+  EXPECT_LONGS_EQUAL(1, phi_x1->keys().size());  // m0
-TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
+  // We can't really check the error of the decision tree factor phi_x1, because
-  HybridGaussianFactorGraph hfg;
+  // the continuous factor whose error(kEmpty) we need is not available..
 
-  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+  // Now test full elimination of the graph:
-  hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
+  auto hybridBayesNet = graph.eliminateSequential();
+  CHECK(hybridBayesNet);
 
-  hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0))));
+  // Check that the posterior P(M|X=continuousValues) from the Bayes net is the
-  hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2))));
+  // same as the same posterior from the graph. This is a sanity check that the
-
+  // elimination is done correctly.
-  hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4"));
+  AlgebraicDecisionTree<Key> bnPosterior =
-
+      hybridBayesNet->discretePosterior(continuousValues);
-  hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1));
+  EXPECT(assert_equal(graphPosterior, bnPosterior));
-  hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
-
-  hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3))));
-  hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5))));
-
-  auto ordering_full =
-      Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)});
-
-  const auto [hbt, remaining] = hfg.eliminatePartialMultifrontal(ordering_full);
-
-  // 9 cliques in the bayes tree and 0 remaining variables to eliminate.
-  EXPECT_LONGS_EQUAL(9, hbt->size());
-  EXPECT_LONGS_EQUAL(0, remaining->size());
-
-  /*
-  (Fan) Explanation: the Junction tree will need to re-eliminate to get to the
-  marginal on X(1), which is not possible because it involves eliminating
-  discrete before continuous. The solution to this, however, is in Murphy02.
-  TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable.
-  And I believe that we should do this.
-  */
-}
-
-void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg,
-              const HybridBayesTree::shared_ptr &hbt,
-              const Ordering &ordering) {
-  DotWriter dw;
-  dw.positionHints['c'] = 2;
-  dw.positionHints['x'] = 1;
-  std::cout << hfg->dot(DefaultKeyFormatter, dw);
-  std::cout << "\n";
-  hbt->dot(std::cout);
-
-  std::cout << "\n";
-  std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw);
-}
-
-/* ************************************************************************* */
-// TODO(fan): make a graph like Varun's paper one
-TEST(HybridGaussianFactorGraph, Switching) {
-  auto N = 12;
-  auto hfg = makeSwitchingChain(N);
-
-  // X(5) will be the center, X(1-4), X(6-9)
-  // X(3), X(7)
-  // X(2), X(8)
-  // X(1), X(4), X(6), X(9)
-  // M(5) will be the center, M(1-4), M(6-8)
-  // M(3), M(7)
-  // M(1), M(4), M(2), M(6), M(8)
-  // auto ordering_full =
-  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
-  //     X(5),
-  //                        M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
-  KeyVector ordering;
-
-  {
-    std::vector<int> naturalX(N);
-    std::iota(naturalX.begin(), naturalX.end(), 1);
-    std::vector<Key> ordX;
-    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
-                   [](int x) { return X(x); });
-
-    auto [ndX, lvls] = makeBinaryOrdering(ordX);
-    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
-    // TODO(dellaert): this has no effect!
-    for (auto &l : lvls) {
-      l = -l;
-    }
-  }
-  {
-    std::vector<int> naturalC(N - 1);
-    std::iota(naturalC.begin(), naturalC.end(), 1);
-    std::vector<Key> ordC;
-    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
-                   [](int x) { return M(x); });
-
-    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
-    const auto [ndC, lvls] = makeBinaryOrdering(ordC);
-    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
-  }
-  auto ordering_full = Ordering(ordering);
-
-  // GTSAM_PRINT(*hfg);
-  // GTSAM_PRINT(ordering_full);
-
-  const auto [hbt, remaining] =
-      hfg->eliminatePartialMultifrontal(ordering_full);
-
-  // 12 cliques in the bayes tree and 0 remaining variables to eliminate.
-  EXPECT_LONGS_EQUAL(12, hbt->size());
-  EXPECT_LONGS_EQUAL(0, remaining->size());
-}
-
-/* ************************************************************************* */
-// TODO(fan): make a graph like Varun's paper one
-TEST(HybridGaussianFactorGraph, SwitchingISAM) {
-  auto N = 11;
-  auto hfg = makeSwitchingChain(N);
-
-  // X(5) will be the center, X(1-4), X(6-9)
-  // X(3), X(7)
-  // X(2), X(8)
-  // X(1), X(4), X(6), X(9)
-  // M(5) will be the center, M(1-4), M(6-8)
-  // M(3), M(7)
-  // M(1), M(4), M(2), M(6), M(8)
-  // auto ordering_full =
-  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
-  //     X(5),
-  //                        M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)});
-  KeyVector ordering;
-
-  {
-    std::vector<int> naturalX(N);
-    std::iota(naturalX.begin(), naturalX.end(), 1);
-    std::vector<Key> ordX;
-    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
-                   [](int x) { return X(x); });
-
-    auto [ndX, lvls] = makeBinaryOrdering(ordX);
-    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
-    // TODO(dellaert): this has no effect!
-    for (auto &l : lvls) {
-      l = -l;
-    }
-  }
-  {
-    std::vector<int> naturalC(N - 1);
-    std::iota(naturalC.begin(), naturalC.end(), 1);
-    std::vector<Key> ordC;
-    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
-                   [](int x) { return M(x); });
-
-    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
-    const auto [ndC, lvls] = makeBinaryOrdering(ordC);
-    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
-  }
-  auto ordering_full = Ordering(ordering);
-
-  const auto [hbt, remaining] =
-      hfg->eliminatePartialMultifrontal(ordering_full);
-
-  auto new_fg = makeSwitchingChain(12);
-  auto isam = HybridGaussianISAM(*hbt);
-
-  // Run an ISAM update.
-  HybridGaussianFactorGraph factorGraph;
-  factorGraph.push_back(new_fg->at(new_fg->size() - 2));
-  factorGraph.push_back(new_fg->at(new_fg->size() - 1));
-  isam.update(factorGraph);
-
-  // ISAM should have 12 factors after the last update
-  EXPECT_LONGS_EQUAL(12, isam.size());
-}
-
-/* ************************************************************************* */
-TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
-  const int N = 7;
-  auto hfg = makeSwitchingChain(N, X);
-  hfg->push_back(*makeSwitchingChain(N, Y, D));
-
-  for (int t = 1; t <= N; t++) {
-    hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0)));
-  }
-
-  KeyVector ordering;
-
-  KeyVector naturalX(N);
-  std::iota(naturalX.begin(), naturalX.end(), 1);
-  KeyVector ordX;
-  for (size_t i = 1; i <= N; i++) {
-    ordX.emplace_back(X(i));
-    ordX.emplace_back(Y(i));
-  }
-
-  for (size_t i = 1; i <= N - 1; i++) {
-    ordX.emplace_back(M(i));
-  }
-  for (size_t i = 1; i <= N - 1; i++) {
-    ordX.emplace_back(D(i));
-  }
-
-  {
-    DotWriter dw;
-    dw.positionHints['x'] = 1;
-    dw.positionHints['c'] = 0;
-    dw.positionHints['d'] = 3;
-    dw.positionHints['y'] = 2;
-    // std::cout << hfg->dot(DefaultKeyFormatter, dw);
-    // std::cout << "\n";
-  }
-
-  {
-    DotWriter dw;
-    dw.positionHints['y'] = 9;
-    // dw.positionHints['c'] = 0;
-    // dw.positionHints['d'] = 3;
-    dw.positionHints['x'] = 1;
-    // std::cout << "\n";
-    // std::cout << hfg->eliminateSequential(Ordering(ordX))
-    //                  ->dot(DefaultKeyFormatter, dw);
-    // hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout);
-  }
-
-  Ordering ordering_partial;
-  for (size_t i = 1; i <= N; i++) {
-    ordering_partial.emplace_back(X(i));
-    ordering_partial.emplace_back(Y(i));
-  }
-  const auto [hbn, remaining] =
-      hfg->eliminatePartialSequential(ordering_partial);
-
-  EXPECT_LONGS_EQUAL(14, hbn->size());
-  EXPECT_LONGS_EQUAL(11, remaining->size());
-
-  {
-    DotWriter dw;
-    dw.positionHints['x'] = 1;
-    dw.positionHints['c'] = 0;
-    dw.positionHints['d'] = 3;
-    dw.positionHints['y'] = 2;
-    // std::cout << remaining->dot(DefaultKeyFormatter, dw);
-    // std::cout << "\n";
-  }
 }
 
 /* ****************************************************************************/
+// Test subset of API for switching network with 3 states.
+// None of these are regression tests.
+TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
+  // Create switching network with three continuous variables and two discrete:
+  // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1)
+  Switching s(3);
+
+  // Check size of linearized factor graph
+  const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph;
+  EXPECT_LONGS_EQUAL(7, graph.size());
+
+  // Eliminate the graph
+  const HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
+
+  const HybridValues delta = hybridBayesNet->optimize();
+  const double error = graph.error(delta);
+
+  // Check that the probability prime is the exponential of the error
+  EXPECT(assert_equal(graph.probPrime(delta), exp(-error), 1e-7));
+
+  // Check that the posterior P(M|X=continuousValues) from the Bayes net is the
+  // same as the same posterior from the graph. This is a sanity check that the
+  // elimination is done correctly.
+  const AlgebraicDecisionTree<Key> graphPosterior =
+      graph.discretePosterior(delta.continuous());
+  const AlgebraicDecisionTree<Key> bnPosterior =
+      hybridBayesNet->discretePosterior(delta.continuous());
+  EXPECT(assert_equal(graphPosterior, bnPosterior));
+}
+
+/* ************************************************************************* */
 // Select a particular continuous factor graph given a discrete assignment
 TEST(HybridGaussianFactorGraph, DiscreteSelection) {
   Switching s(3);
@@ -546,23 +449,43 @@ TEST(HybridGaussianFactorGraph, optimize) {
 // Test adding of gaussian conditional and re-elimination.
 TEST(HybridGaussianFactorGraph, Conditionals) {
   Switching switching(4);
-  HybridGaussianFactorGraph hfg;
 
-  hfg.push_back(switching.linearizedFactorGraph.at(0));  // P(X1)
+  HybridGaussianFactorGraph hfg;
+  hfg.push_back(switching.linearizedFactorGraph.at(0));  // P(X0)
   Ordering ordering;
   ordering.push_back(X(0));
   HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering);
 
-  hfg.push_back(switching.linearizedFactorGraph.at(1));  // P(X1, X2 | M1)
+  HybridGaussianFactorGraph hfg2;
-  hfg.push_back(*bayes_net);
+  hfg2.push_back(*bayes_net);                             // P(X0)
-  hfg.push_back(switching.linearizedFactorGraph.at(2));  // P(X2, X3 | M2)
+  hfg2.push_back(switching.linearizedFactorGraph.at(1));  // P(X0, X1 | M0)
-  hfg.push_back(switching.linearizedFactorGraph.at(5));  // P(M1)
+  hfg2.push_back(switching.linearizedFactorGraph.at(2));  // P(X1, X2 | M1)
-  ordering.push_back(X(1));
+  hfg2.push_back(switching.linearizedFactorGraph.at(5));  // P(M1)
-  ordering.push_back(X(2));
+  ordering += X(1), X(2), M(0), M(1);
-  ordering.push_back(M(0));
-  ordering.push_back(M(1));
 
-  bayes_net = hfg.eliminateSequential(ordering);
+  // Created product of first two factors and check eliminate:
+  HybridGaussianFactorGraph fragment;
+  fragment.push_back(hfg2[0]);
+  fragment.push_back(hfg2[1]);
+
+  // Check that product
+  HybridGaussianProductFactor product = fragment.collectProductFactor();
+  auto leaf = fragment(DiscreteValues{{M(0), 0}});
+  EXPECT_LONGS_EQUAL(2, leaf.size());
+
+  // Check product and that pruneEmpty does not touch it
+  auto pruned = product.removeEmpty();
+  LONGS_EQUAL(2, pruned.nrLeaves());
+
+  // Test eliminate
+  auto [hybridConditional, factor] = fragment.eliminate({X(0)});
+  EXPECT(hybridConditional->isHybrid());
+  EXPECT(hybridConditional->keys() == KeyVector({X(0), X(1), M(0)}));
+
+  EXPECT(dynamic_pointer_cast<HybridGaussianFactor>(factor));
+  EXPECT(factor->keys() == KeyVector({X(1), M(0)}));
+
+  bayes_net = hfg2.eliminateSequential(ordering);
 
   HybridValues result = bayes_net->optimize();
 
@@ -581,51 +504,6 @@ TEST(HybridGaussianFactorGraph, Conditionals) {
   EXPECT(assert_equal(expected_discrete, result.discrete()));
 }
 
-/* ****************************************************************************/
-// Test hybrid gaussian factor graph error and unnormalized probabilities
-TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) {
-  Switching s(3);
-
-  HybridGaussianFactorGraph graph = s.linearizedFactorGraph;
-
-  HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
-
-  const HybridValues delta = hybridBayesNet->optimize();
-  const double error = graph.error(delta);
-
-  // regression
-  EXPECT(assert_equal(1.58886, error, 1e-5));
-
-  // Real test:
-  EXPECT(assert_equal(graph.probPrime(delta), exp(-error), 1e-7));
-}
-
-/* ****************************************************************************/
-// Test hybrid gaussian factor graph error and unnormalized probabilities
-TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) {
-  // Create switching network with three continuous variables and two discrete:
-  // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x0;z0) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1)
-  Switching s(3);
-
-  const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph;
-
-  const HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
-
-  const HybridValues delta = hybridBayesNet->optimize();
-
-  // regression test for errorTree
-  std::vector<double> leaves = {2.7916153, 1.5888555, 1.7233422, 1.6191947};
-  AlgebraicDecisionTree<Key> expectedErrors(s.modes, leaves);
-  const auto error_tree = graph.errorTree(delta.continuous());
-  EXPECT(assert_equal(expectedErrors, error_tree, 1e-7));
-
-  // regression test for discretePosterior
-  const AlgebraicDecisionTree<Key> expectedPosterior(
-      s.modes, std::vector{0.095516068, 0.31800092, 0.27798511, 0.3084979});
-  auto posterior = graph.discretePosterior(delta.continuous());
-  EXPECT(assert_equal(expectedPosterior, posterior, 1e-7));
-}
-
 /* ****************************************************************************/
 // Test hybrid gaussian factor graph errorTree during incremental operation
 TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
@@ -643,15 +521,13 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
   HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential();
   EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
 
+  // Check discrete posterior at optimum
   HybridValues delta = hybridBayesNet->optimize();
-  auto error_tree = graph.errorTree(delta.continuous());
+  AlgebraicDecisionTree<Key> graphPosterior =
-
+      graph.discretePosterior(delta.continuous());
-  std::vector<DiscreteKey> discrete_keys = {{M(0), 2}, {M(1), 2}};
+  AlgebraicDecisionTree<Key> bnPosterior =
-  std::vector<double> leaves = {2.7916153, 1.5888555, 1.7233422, 1.6191947};
+      hybridBayesNet->discretePosterior(delta.continuous());
-  AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
+  EXPECT(assert_equal(graphPosterior, bnPosterior));
-
-  // regression
-  EXPECT(assert_equal(expected_error, error_tree, 1e-7));
 
   graph = HybridGaussianFactorGraph();
   graph.push_back(*hybridBayesNet);
@@ -662,26 +538,21 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) {
   EXPECT_LONGS_EQUAL(7, hybridBayesNet->size());
 
   delta = hybridBayesNet->optimize();
-  auto error_tree2 = graph.errorTree(delta.continuous());
+  graphPosterior = graph.discretePosterior(delta.continuous());
-
+  bnPosterior = hybridBayesNet->discretePosterior(delta.continuous());
-  // regression
+  EXPECT(assert_equal(graphPosterior, bnPosterior));
-  leaves = {0.50985198, 0.0097577296, 0.50009425, 0,
-            0.52922138, 0.029127133,  0.50985105, 0.0097567964};
-  AlgebraicDecisionTree<Key> expected_error2(s.modes, leaves);
-  EXPECT(assert_equal(expected_error, error_tree, 1e-7));
 }
 
 /* ****************************************************************************/
-// Check that assembleGraphTree assembles Gaussian factor graphs for each
+// Check that collectProductFactor works correctly.
-// assignment.
+TEST(HybridGaussianFactorGraph, collectProductFactor) {
-TEST(HybridGaussianFactorGraph, assembleGraphTree) {
   const int num_measurements = 1;
-  auto fg = tiny::createHybridGaussianFactorGraph(
+  VectorValues vv{{Z(0), Vector1(5.0)}};
-      num_measurements, VectorValues{{Z(0), Vector1(5.0)}});
+  auto fg = tiny::createHybridGaussianFactorGraph(num_measurements, vv);
   EXPECT_LONGS_EQUAL(3, fg.size());
 
   // Assemble graph tree:
-  auto actual = fg.assembleGraphTree();
+  auto actual = fg.collectProductFactor();
 
   // Create expected decision tree with two factor graphs:
 
@@ -700,13 +571,15 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
   DiscreteValues d0{{M(0), 0}}, d1{{M(0), 1}};
 
   // Expected decision tree with two factor graphs:
-  // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0)
+  // f(x0;mode=0)P(x0)
-  GaussianFactorGraphTree expected{
+  GaussianFactorGraph expectedFG0{(*hybrid)(d0).first, prior};
-      M(0), GaussianFactorGraph(std::vector<GF>{(*hybrid)(d0), prior}),
+  EXPECT(assert_equal(expectedFG0, actual(d0).first, 1e-5));
-      GaussianFactorGraph(std::vector<GF>{(*hybrid)(d1), prior})};
+  EXPECT(assert_equal(0.0, actual(d0).second, 1e-5));
 
-  EXPECT(assert_equal(expected(d0), actual(d0), 1e-5));
+  // f(x0;mode=1)P(x0)
-  EXPECT(assert_equal(expected(d1), actual(d1), 1e-5));
+  GaussianFactorGraph expectedFG1{(*hybrid)(d1).first, prior};
+  EXPECT(assert_equal(expectedFG1, actual(d1).first, 1e-5));
+  EXPECT(assert_equal(1.79176, actual(d1).second, 1e-5));
 }
 
 /* ****************************************************************************/
@@ -746,7 +619,6 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements,
   // Test ratios for a number of independent samples:
   for (size_t i = 0; i < num_samples; i++) {
     HybridValues sample = bn.sample(&kRng);
-    // GTSAM_PRINT(sample);
     // std::cout << "ratio: " << compute_ratio(&sample) << std::endl;
     if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6) return false;
   }

gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp

@@ -0,0 +1,196 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testHybridGaussianProductFactor.cpp
+ * @brief   Unit tests for HybridGaussianProductFactor
+ * @author  Frank Dellaert
+ * @date    October 2024
+ */
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridGaussianProductFactor.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/JacobianFactor.h>
+
+// Include for test suite
+#include <CppUnitLite/TestHarness.h>
+
+#include <memory>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::M;
+using symbol_shorthand::X;
+
+/* ************************************************************************* */
+namespace examples {
+static const DiscreteKey m1(M(1), 2), m2(M(2), 3);
+
+const auto A1 = Matrix::Zero(2, 1);
+const auto A2 = Matrix::Zero(2, 2);
+const auto b = Matrix::Zero(2, 1);
+
+const auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+const auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+const HybridGaussianFactor hybridFactorA(m1, {{f10, 10}, {f11, 11}});
+
+const auto A3 = Matrix::Zero(2, 3);
+const auto f20 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+const auto f21 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+const auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+
+const HybridGaussianFactor hybridFactorB(m2, {{f20, 20}, {f21, 21}, {f22, 22}});
+// Simulate a pruned hybrid factor, in this case m2==1 is nulled out.
+const HybridGaussianFactor prunedFactorB(
+    m2, {{f20, 20}, {nullptr, 1000}, {f22, 22}});
+}  // namespace examples
+
+/* ************************************************************************* */
+// Constructor
+TEST(HybridGaussianProductFactor, Construct) {
+  HybridGaussianProductFactor product;
+}
+
+/* ************************************************************************* */
+// Add two Gaussian factors and check only one leaf in tree
+TEST(HybridGaussianProductFactor, AddTwoGaussianFactors) {
+  using namespace examples;
+
+  HybridGaussianProductFactor product;
+  product += f10;
+  product += f11;
+
+  // Check that the product has only one leaf and no discrete variables.
+  EXPECT_LONGS_EQUAL(1, product.nrLeaves());
+  EXPECT(product.labels().empty());
+
+  // Retrieve the single leaf
+  auto leaf = product(Assignment<Key>());
+
+  // Check that the leaf contains both factors
+  EXPECT_LONGS_EQUAL(2, leaf.first.size());
+  EXPECT(leaf.first.at(0) == f10);
+  EXPECT(leaf.first.at(1) == f11);
+  EXPECT_DOUBLES_EQUAL(0, leaf.second, 1e-9);
+}
+
+/* ************************************************************************* */
+// Add two GaussianConditionals and check the resulting tree
+TEST(HybridGaussianProductFactor, AddTwoGaussianConditionals) {
+  // Create two GaussianConditionals
+  Vector1 d(1.0);
+  Matrix11 R = I_1x1, S = I_1x1;
+  auto gc1 = std::make_shared<GaussianConditional>(X(1), d, R, X(2), S);
+  auto gc2 = std::make_shared<GaussianConditional>(X(2), d, R);
+
+  // Create a HybridGaussianProductFactor and add the conditionals
+  HybridGaussianProductFactor product;
+  product += std::static_pointer_cast<GaussianFactor>(gc1);
+  product += std::static_pointer_cast<GaussianFactor>(gc2);
+
+  // Check that the product has only one leaf and no discrete variables
+  EXPECT_LONGS_EQUAL(1, product.nrLeaves());
+  EXPECT(product.labels().empty());
+
+  // Retrieve the single leaf
+  auto leaf = product(Assignment<Key>());
+
+  // Check that the leaf contains both conditionals
+  EXPECT_LONGS_EQUAL(2, leaf.first.size());
+  EXPECT(leaf.first.at(0) == gc1);
+  EXPECT(leaf.first.at(1) == gc2);
+  EXPECT_DOUBLES_EQUAL(0, leaf.second, 1e-9);
+}
+
+/* ************************************************************************* */
+// Check AsProductFactor
+TEST(HybridGaussianProductFactor, AsProductFactor) {
+  using namespace examples;
+  auto product = hybridFactorA.asProductFactor();
+
+  // Let's check that this worked:
+  Assignment<Key> mode;
+  mode[m1.first] = 0;
+  auto actual = product(mode);
+  EXPECT(actual.first.at(0) == f10);
+  EXPECT_DOUBLES_EQUAL(10, actual.second, 1e-9);
+
+  // TODO(Frank): when killed hiding, f11 should also be there
+}
+
+/* ************************************************************************* */
+// "Add" one hybrid factors together.
+TEST(HybridGaussianProductFactor, AddOne) {
+  using namespace examples;
+  HybridGaussianProductFactor product;
+  product += hybridFactorA;
+
+  // Let's check that this worked:
+  Assignment<Key> mode;
+  mode[m1.first] = 0;
+  auto actual = product(mode);
+  EXPECT(actual.first.at(0) == f10);
+  EXPECT_DOUBLES_EQUAL(10, actual.second, 1e-9);
+
+  // TODO(Frank): when killed hiding, f11 should also be there
+}
+
+/* ************************************************************************* */
+// "Add" two HFG together.
+TEST(HybridGaussianProductFactor, AddTwo) {
+  using namespace examples;
+
+  // Create product of two hybrid factors: it will be a decision tree now on
+  // both discrete variables m1 and m2:
+  HybridGaussianProductFactor product;
+  product += hybridFactorA;
+  product += hybridFactorB;
+
+  // Let's check that this worked:
+  auto actual00 = product({{M(1), 0}, {M(2), 0}});
+  EXPECT(actual00.first.at(0) == f10);
+  EXPECT(actual00.first.at(1) == f20);
+  EXPECT_DOUBLES_EQUAL(10 + 20, actual00.second, 1e-9);
+
+  auto actual12 = product({{M(1), 1}, {M(2), 2}});
+  // TODO(Frank): when killed hiding, these should also equal:
+  // EXPECT(actual12.first.at(0) == f11);
+  // EXPECT(actual12.first.at(1) == f22);
+  EXPECT_DOUBLES_EQUAL(11 + 22, actual12.second, 1e-9);
+}
+
+/* ************************************************************************* */
+// "Add" two HFG together.
+TEST(HybridGaussianProductFactor, AddPruned) {
+  using namespace examples;
+
+  // Create product of two hybrid factors: it will be a decision tree now on
+  // both discrete variables m1 and m2:
+  HybridGaussianProductFactor product;
+  product += hybridFactorA;
+  product += prunedFactorB;
+  EXPECT_LONGS_EQUAL(6, product.nrLeaves());
+
+  auto pruned = product.removeEmpty();
+  EXPECT_LONGS_EQUAL(5, pruned.nrLeaves());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/hybrid/tests/testHybridMotionModel.cpp

@@ -192,24 +192,29 @@ TEST(HybridGaussianFactor, TwoStateModel2) {
     HybridBayesNet hbn = CreateBayesNet(hybridMotionModel);
     HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
 
-    // Check that ratio of Bayes net and factor graph for different modes is
+    HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential();
-    // equal for several values of {x0,x1}.
+
     for (VectorValues vv :
          {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
           VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
       vv.insert(given);  // add measurements for HBN
-      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
+      const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
-      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
-                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
-    }
 
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+      // Equality of posteriors asserts that the factor graph is correct (same
+      // ratios for all modes)
+      EXPECT(
+          assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
+
+      // This one asserts that HBN resulting from elimination is correct.
+      EXPECT(assert_equal(expectedDiscretePosterior,
+                          eliminated->discretePosterior(vv)));
+    }
 
     // Importance sampling run with 100k samples gives 50.095/49.905
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
 
     // Since no measurement on x1, we a 50/50 probability
-    auto p_m = bn->at(2)->asDiscrete();
+    auto p_m = eliminated->at(2)->asDiscrete();
     EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9);
     EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9);
   }
@@ -221,6 +226,7 @@ TEST(HybridGaussianFactor, TwoStateModel2) {
 
     HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true);
     HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
+    HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential();
 
     // Check that ratio of Bayes net and factor graph for different modes is
     // equal for several values of {x0,x1}.
@@ -228,17 +234,22 @@ TEST(HybridGaussianFactor, TwoStateModel2) {
          {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}},
           VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) {
       vv.insert(given);  // add measurements for HBN
-      HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}});
+      const auto& expectedDiscretePosterior = hbn.discretePosterior(vv);
-      EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0),
-                           gfg.error(hv1) / hbn.error(hv1), 1e-9);
-    }
 
-    HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
+      // Equality of posteriors asserts that the factor graph is correct (same
+      // ratios for all modes)
+      EXPECT(
+          assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv)));
+
+      // This one asserts that HBN resulting from elimination is correct.
+      EXPECT(assert_equal(expectedDiscretePosterior,
+                          eliminated->discretePosterior(vv)));
+    }
 
     // Values taken from an importance sampling run with 100k samples:
     // approximateDiscreteMarginal(hbn, hybridMotionModel, given);
     DiscreteConditional expected(m1, "48.3158/51.6842");
-    EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002));
+    EXPECT(assert_equal(expected, *(eliminated->at(2)->asDiscrete()), 0.002));
   }
 
   {

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -545,14 +545,17 @@ TEST(HybridNonlinearFactorGraph, Printing) {
 #ifdef GTSAM_DT_MERGING
   string expected_hybridFactorGraph = R"(
 size: 7
-factor 0: 
+Factor 0
+GaussianFactor:
+
   A[x0] = [
 	10
 ]
   b = [ -10 ]
   No noise model
-factor 1: 
+
-HybridGaussianFactor
+Factor 1
+HybridGaussianFactor:
 Hybrid [x0 x1; m0]{
  Choice(m0) 
  0 Leaf :
@@ -564,6 +567,7 @@ Hybrid [x0 x1; m0]{
 ]
   b = [ -1 ]
   No noise model
+scalar: 0.918939
 
  1 Leaf :
   A[x0] = [
@@ -574,10 +578,12 @@ Hybrid [x0 x1; m0]{
 ]
   b = [ -0 ]
   No noise model
+scalar: 0.918939
 
 }
-factor 2: 
+
-HybridGaussianFactor
+Factor 2
+HybridGaussianFactor:
 Hybrid [x1 x2; m1]{
  Choice(m1) 
  0 Leaf :
@@ -589,6 +595,7 @@ Hybrid [x1 x2; m1]{
 ]
   b = [ -1 ]
   No noise model
+scalar: 0.918939
 
  1 Leaf :
   A[x1] = [
@@ -599,24 +606,37 @@ Hybrid [x1 x2; m1]{
 ]
   b = [ -0 ]
   No noise model
+scalar: 0.918939
 
 }
-factor 3: 
+
+Factor 3
+GaussianFactor:
+
   A[x1] = [
 	10
 ]
   b = [ -10 ]
   No noise model
-factor 4: 
+
+Factor 4
+GaussianFactor:
+
   A[x2] = [
 	10
 ]
   b = [ -10 ]
   No noise model
-factor 5:  P( m0 ):
+
+Factor 5
+DiscreteFactor:
+ P( m0 ):
  Leaf  0.5
 
-factor 6:  P( m1 | m0 ):
+
+Factor 6
+DiscreteFactor:
+ P( m1 | m0 ):
  Choice(m1) 
  0 Choice(m0) 
  0 0 Leaf 0.33333333
@@ -625,6 +645,7 @@ factor 6:  P( m1 | m0 ):
  1 0 Leaf 0.66666667
  1 1 Leaf  0.4
 
+
 )";
 #else
   string expected_hybridFactorGraph = R"(
@@ -717,7 +738,7 @@ factor 6:  P( m1 | m0 ):
   // Expected output for hybridBayesNet.
   string expected_hybridBayesNet = R"(
 size: 3
-conditional 0: Hybrid  P( x0 | x1 m0)
+conditional 0:  P( x0 | x1 m0)
  Discrete Keys = (m0, 2), 
  logNormalizationConstant: 1.38862
 
@@ -736,7 +757,7 @@ conditional 0: Hybrid  P( x0 | x1 m0)
   logNormalizationConstant: 1.38862
   No noise model
 
-conditional 1: Hybrid  P( x1 | x2 m0 m1)
+conditional 1:  P( x1 | x2 m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
  logNormalizationConstant: 1.3935
 
@@ -771,7 +792,7 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
   logNormalizationConstant: 1.3935
   No noise model
 
-conditional 2: Hybrid  P( x2 | m0 m1)
+conditional 2:  P( x2 | m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
  logNormalizationConstant: 1.38857
 

gtsam/hybrid/tests/testSerializationHybrid.cpp

@@ -52,13 +52,18 @@ BOOST_CLASS_EXPORT_GUID(ADT::Leaf, "gtsam_AlgebraicDecisionTree_Leaf");
 BOOST_CLASS_EXPORT_GUID(ADT::Choice, "gtsam_AlgebraicDecisionTree_Choice")
 
 BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor, "gtsam_HybridGaussianFactor");
-BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors,
+BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs,
                         "gtsam_HybridGaussianFactor_Factors");
-BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Leaf,
+BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs::Leaf,
                         "gtsam_HybridGaussianFactor_Factors_Leaf");
-BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice,
+BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs::Choice,
                         "gtsam_HybridGaussianFactor_Factors_Choice");
 
+BOOST_CLASS_EXPORT_GUID(GaussianFactorGraphValuePair,
+                        "gtsam_GaussianFactorGraphValuePair");
+BOOST_CLASS_EXPORT_GUID(HybridGaussianProductFactor,
+                        "gtsam_HybridGaussianProductFactor");
+
 BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional,
                         "gtsam_HybridGaussianConditional");
 BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals,