More refactoring

gtsam/hybrid/HybridBayesNet.h

@@ -44,9 +44,14 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /// @name Standard Constructors
   /// @{
 
-  /** Construct empty Bayes net */
+  /// Construct empty Bayes net.
   HybridBayesNet() = default;
 
+  /// Constructor that takes an initializer list of shared pointers.
+  HybridBayesNet(
+      std::initializer_list<HybridConditional::shared_ptr> conditionals)
+      : Base(conditionals) {}
+
   /// @}
   /// @name Testable
   /// @{

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -26,21 +26,49 @@
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/JacobianFactor.h>
 
 #include <cstddef>
 
 namespace gtsam {
 /* *******************************************************************************/
-struct HybridGaussianConditional::ConstructorHelper {
+struct HybridGaussianConditional::Helper {
   std::optional<size_t> nrFrontals;
-  HybridGaussianFactor::FactorValuePairs pairs;
+  FactorValuePairs pairs;
+  Conditionals conditionals;
   double minNegLogConstant;
 
-  /// Compute all variables needed for the private constructor below.
-  ConstructorHelper(const Conditionals &conditionals)
-      : minNegLogConstant(std::numeric_limits<double>::infinity()) {
-    auto func = [this](const GaussianConditional::shared_ptr &c)
-        -> GaussianFactorValuePair {
+  using GC = GaussianConditional;
+  using P = std::vector<std::pair<Vector, double>>;
+
+  /// Construct from a vector of mean and sigma pairs, plus extra args.
+  template <typename... Args>
+  explicit Helper(const DiscreteKey &mode, const P &p, Args &&...args) {
+    nrFrontals = 1;
+    minNegLogConstant = std::numeric_limits<double>::infinity();
+
+    std::vector<GaussianFactorValuePair> fvs;
+    std::vector<GC::shared_ptr> gcs;
+    fvs.reserve(p.size());
+    gcs.reserve(p.size());
+    for (const auto &[mean, sigma] : p) {
+      auto gaussianConditional =
+          GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma);
+      double value = gaussianConditional->negLogConstant();
+      minNegLogConstant = std::min(minNegLogConstant, value);
+      fvs.emplace_back(gaussianConditional, value);
+      gcs.push_back(gaussianConditional);
+    }
+
+    conditionals = Conditionals({mode}, gcs);
+    pairs = FactorValuePairs({mode}, fvs);
+  }
+
+  /// Construct from tree of GaussianConditionals.
+  explicit Helper(const Conditionals &conditionals)
+      : conditionals(conditionals),
+        minNegLogConstant(std::numeric_limits<double>::infinity()) {
+    auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair {
       double value = 0.0;
       if (c) {
         if (!nrFrontals.has_value()) {
@@ -51,37 +79,54 @@ struct HybridGaussianConditional::ConstructorHelper {
       }
       return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
     };
-    pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
+    pairs = FactorValuePairs(conditionals, func);
     if (!nrFrontals.has_value()) {
       throw std::runtime_error(
-          "HybridGaussianConditional: need at least one frontal variable.");
+          "HybridGaussianConditional: need at least one frontal variable. "
+          "Provided conditionals do not contain any frontal variables.");
     }
   }
 };
 
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const DiscreteKeys &discreteParents,
-    const HybridGaussianConditional::Conditionals &conditionals,
-    const ConstructorHelper &helper)
+    const DiscreteKeys &discreteParents, const Helper &helper)
     : BaseFactor(discreteParents, helper.pairs),
       BaseConditional(*helper.nrFrontals),
-      conditionals_(conditionals),
+      conditionals_(helper.conditionals),
       negLogConstant_(helper.minNegLogConstant) {}
 
-/* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const DiscreteKeys &discreteParents,
-    const HybridGaussianConditional::Conditionals &conditionals)
-    : HybridGaussianConditional(discreteParents, conditionals,
-                                ConstructorHelper(conditionals)) {}
+    const DiscreteKey &mode,
+    const std::vector<GaussianConditional::shared_ptr> &conditionals)
+    : HybridGaussianConditional(DiscreteKeys{mode},
+                                Conditionals({mode}, conditionals)) {}
 
-/* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const DiscreteKey &discreteParent,
-    const std::vector<GaussianConditional::shared_ptr> &conditionals)
-    : HybridGaussianConditional(DiscreteKeys{discreteParent},
-                                Conditionals({discreteParent}, conditionals)) {}
+    const DiscreteKey &mode, Key key,  //
+    const std::vector<std::pair<Vector, double>> &parameters)
+    : HybridGaussianConditional(DiscreteKeys{mode},
+                                Helper(mode, parameters, key)) {}
+
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKey &mode, Key key,  //
+    const Matrix &A, Key parent,
+    const std::vector<std::pair<Vector, double>> &parameters)
+    : HybridGaussianConditional(DiscreteKeys{mode},
+                                Helper(mode, parameters, key, A, parent)) {}
+
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKey &mode, Key key,  //
+    const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
+    const std::vector<std::pair<Vector, double>> &parameters)
+    : HybridGaussianConditional(
+          DiscreteKeys{mode},
+          Helper(mode, parameters, key, A1, parent1, A2, parent2)) {}
+
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKeys &discreteParents,
+    const HybridGaussianConditional::Conditionals &conditionals)
+    : HybridGaussianConditional(discreteParents, Helper(conditionals)) {}
 
 /* *******************************************************************************/
 const HybridGaussianConditional::Conditionals &

gtsam/hybrid/HybridGaussianConditional.h

@@ -79,14 +79,57 @@ class GTSAM_EXPORT HybridGaussianConditional
   /**
    * @brief Construct from one discrete key and vector of conditionals.
    *
-   * @param discreteParent Single discrete parent variable
+   * @param mode Single discrete parent variable
    * @param conditionals Vector of conditionals with the same size as the
    * cardinality of the discrete parent.
    */
   HybridGaussianConditional(
-      const DiscreteKey &discreteParent,
+      const DiscreteKey &mode,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
+  /**
+   * @brief Constructs a HybridGaussianConditional with means mu_i and
+   * standard deviations sigma_i.
+   *
+   * @param mode The discrete mode key.
+   * @param key The key for this conditional variable.
+   * @param parameters A vector of pairs (mu_i, sigma_i).
+   */
+  HybridGaussianConditional(
+      const DiscreteKey &mode, Key key,
+      const std::vector<std::pair<Vector, double>> &parameters);
+
+  /**
+   * @brief Constructs a HybridGaussianConditional with conditional means
+   * A × parent + b_i and standard deviations sigma_i.
+   *
+   * @param mode The discrete mode key.
+   * @param key The key for this conditional variable.
+   * @param A The matrix A.
+   * @param parent The key of the parent variable.
+   * @param parameters A vector of pairs (b_i, sigma_i).
+   */
+  HybridGaussianConditional(
+      const DiscreteKey &mode, Key key, const Matrix &A, Key parent,
+      const std::vector<std::pair<Vector, double>> &parameters);
+
+  /**
+   * @brief Constructs a HybridGaussianConditional with conditional means
+   * A1 × parent1 + A2 × parent2 + b_i and standard deviations sigma_i.
+   *
+   * @param mode The discrete mode key.
+   * @param key The key for this conditional variable.
+   * @param A1 The first matrix.
+   * @param parent1 The key of the first parent variable.
+   * @param A2 The second matrix.
+   * @param parent2 The key of the second parent variable.
+   * @param parameters A vector of pairs (b_i, sigma_i).
+   */
+  HybridGaussianConditional(
+      const DiscreteKey &mode, Key key,  //
+      const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
+      const std::vector<std::pair<Vector, double>> &parameters);
+
   /**
    * @brief Construct from multiple discrete keys and conditional tree.
    *
@@ -183,13 +226,11 @@ class GTSAM_EXPORT HybridGaussianConditional
 
  private:
   /// Helper struct for private constructor.
-  struct ConstructorHelper;
+  struct Helper;
 
   /// Private constructor that uses helper struct above.
-  HybridGaussianConditional(
-      const DiscreteKeys &discreteParents,
-      const HybridGaussianConditional::Conditionals &conditionals,
-      const ConstructorHelper &helper);
+  HybridGaussianConditional(const DiscreteKeys &discreteParents,
+                            const Helper &helper);
 
   /// Convert to a DecisionTree of Gaussian factor graphs.
   GaussianFactorGraphTree asGaussianFactorGraphTree() const;

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -366,18 +366,17 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
   return std::make_shared<HybridGaussianFactor>(discreteSeparator, newFactors);
 }
 
-static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
-hybridElimination(const HybridGaussianFactorGraph &factors,
-                  const Ordering &frontalKeys,
-                  const std::set<DiscreteKey> &discreteSeparatorSet) {
-  // NOTE: since we use the special JunctionTree,
-  // only possibility is continuous conditioned on discrete.
-  DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(),
-                                 discreteSeparatorSet.end());
+std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
+HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
+  // Since we eliminate all continuous variables first,
+  // the discrete separator will be *all* the discrete keys.
+  const std::set<DiscreteKey> keysForDiscreteVariables = discreteKeys();
+  DiscreteKeys discreteSeparator(keysForDiscreteVariables.begin(),
+                                 keysForDiscreteVariables.end());
 
   // Collect all the factors to create a set of Gaussian factor graphs in a
   // decision tree indexed by all discrete keys involved.
-  GaussianFactorGraphTree factorGraphTree = factors.assembleGraphTree();
+  GaussianFactorGraphTree factorGraphTree = assembleGraphTree();
 
   // 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
@@ -392,7 +391,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
     }
 
     // Expensive elimination of product factor.
-    auto result = EliminatePreferCholesky(graph, frontalKeys);
+    auto result = EliminatePreferCholesky(graph, keys);
 
     // Record whether there any continuous variables left
     someContinuousLeft |= !result.second->empty();
@@ -436,7 +435,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
  */
 std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>  //
 EliminateHybrid(const HybridGaussianFactorGraph &factors,
-                const Ordering &frontalKeys) {
+                const Ordering &keys) {
   // NOTE: Because we are in the Conditional Gaussian regime there are only
   // a few cases:
   // 1. continuous variable, make a hybrid Gaussian conditional if there are
@@ -510,20 +509,13 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
 
   if (only_discrete) {
     // Case 1: we are only dealing with discrete
-    return discreteElimination(factors, frontalKeys);
+    return discreteElimination(factors, keys);
   } else if (only_continuous) {
     // Case 2: we are only dealing with continuous
-    return continuousElimination(factors, frontalKeys);
+    return continuousElimination(factors, keys);
   } else {
     // Case 3: We are now in the hybrid land!
-    KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end());
-
-    // Find all discrete keys.
-    // Since we eliminate all continuous variables first,
-    // the discrete separator will be *all* the discrete keys.
-    std::set<DiscreteKey> discreteSeparator = factors.discreteKeys();
-
-    return hybridElimination(factors, frontalKeys, discreteSeparator);
+    return factors.eliminate(keys);
   }
 }
 

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -217,6 +217,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
    */
   GaussianFactorGraphTree assembleGraphTree() const;
 
+  /**
+   * @brief Eliminate the given continuous keys.
+   *
+   * @param keys The continuous keys to eliminate.
+   * @return The conditional on the  keys and a factor on the separator.
+   */
+  std::pair<std::shared_ptr<HybridConditional>, std::shared_ptr<Factor>>
+  eliminate(const Ordering& keys) const;
   /// @}
 
   /// Get the GaussianFactorGraph at a given discrete assignment.

gtsam/hybrid/tests/TinyHybridExample.h

@@ -41,12 +41,12 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
   HybridBayesNet bayesNet;
 
   // Create hybrid Gaussian factor z_i = x0 + noise for each measurement.
+  std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 0.5},
+                                                           {Z_1x1, 3.0}};
   for (size_t i = 0; i < num_measurements; i++) {
     const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
-    std::vector<GaussianConditional::shared_ptr> conditionals{
-        GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
-        GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
-    bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, conditionals);
+    bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, Z(i), I_1x1,
+                                                       X(0), measurementModels);
   }
 
   // Create prior on X(0).

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -43,26 +43,6 @@ const DiscreteValues m1Assignment{{M(0), 1}};
 DiscreteConditional::shared_ptr mixing =
     std::make_shared<DiscreteConditional>(m, "60/40");
 
-/**
- * Create a simple Gaussian Mixture Model represented as p(z|m)P(m)
- * where m is a discrete variable and z is a continuous variable.
- * The "mode" m is binary and depending on m, we have 2 different means
- * μ1 and μ2 for the Gaussian density p(z|m).
- */
-HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0,
-                                    double sigma1) {
-  HybridBayesNet hbn;
-  auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
-  auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
-  auto c0 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu0), I_1x1,
-                                                  model0),
-       c1 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1,
-                                                  model1);
-  hbn.emplace_shared<HybridGaussianConditional>(m, std::vector{c0, c1});
-  hbn.push_back(mixing);
-  return hbn;
-}
-
 /// Gaussian density function
 double Gaussian(double mu, double sigma, double z) {
   return exp(-0.5 * pow((z - mu) / sigma, 2)) / sqrt(2 * M_PI * sigma * sigma);
@@ -99,7 +79,11 @@ TEST(GaussianMixture, GaussianMixtureModel) {
   double mu0 = 1.0, mu1 = 3.0;
   double sigma = 2.0;
 
-  auto hbn = GaussianMixtureModel(mu0, mu1, sigma, sigma);
+  HybridBayesNet hbn;
+  std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma},
+                                                    {Vector1(mu1), sigma}};
+  hbn.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
+  hbn.push_back(mixing);
 
   // At the halfway point between the means, we should get P(m|z)=0.5
   double midway = mu1 - mu0;
@@ -133,7 +117,11 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
   double mu0 = 1.0, mu1 = 3.0;
   double sigma0 = 8.0, sigma1 = 4.0;
 
-  auto hbn = GaussianMixtureModel(mu0, mu1, sigma0, sigma1);
+  HybridBayesNet hbn;
+  std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma0},
+                                                    {Vector1(mu1), sigma1}};
+  hbn.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
+  hbn.push_back(mixing);
 
   // 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.