AlgebraicDecisionTree Helpers

gtsam/discrete/AlgebraicDecisionTree.h

@@ -196,6 +196,42 @@ namespace gtsam {
       return this->apply(g, &Ring::div);
     }
 
+    /// Compute sum of all values
+    double sum() const {
+      double sum = 0;
+      auto visitor = [&](double y) { sum += y; };
+      this->visit(visitor);
+      return sum;
+    }
+
+    /**
+     * @brief Helper method to perform normalization such that all leaves in the
+     * tree sum to 1
+     *
+     * @param sum
+     * @return AlgebraicDecisionTree
+     */
+    AlgebraicDecisionTree normalize(double sum) const {
+      return this->apply([&sum](const double& x) { return x / sum; });
+    }
+
+    /// Find the minimum values amongst all leaves
+    double min() const {
+      double min = std::numeric_limits<double>::max();
+      auto visitor = [&](double x) { min = x < min ? x : min; };
+      this->visit(visitor);
+      return min;
+    }
+
+    /// Find the maximum values amongst all leaves
+    double max() const {
+      // Get the most negative value
+      double max = -std::numeric_limits<double>::max();
+      auto visitor = [&](double x) { max = x > max ? x : max; };
+      this->visit(visitor);
+      return max;
+    }
+
     /** sum out variable */
     AlgebraicDecisionTree sum(const L& label, size_t cardinality) const {
       return this->combine(label, cardinality, &Ring::add);

gtsam/hybrid/GaussianMixture.cpp

@@ -24,6 +24,7 @@
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/inference/Conditional-inst.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
@@ -92,6 +93,34 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
   return {conditionals_, wrap};
 }
 
+/* *******************************************************************************/
+GaussianBayesNetTree GaussianMixture::add(
+    const GaussianBayesNetTree &sum) const {
+  using Y = GaussianBayesNet;
+  auto add = [](const Y &graph1, const Y &graph2) {
+    auto result = graph1;
+    if (graph2.size() == 0) {
+      return GaussianBayesNet();
+    }
+    result.push_back(graph2);
+    return result;
+  };
+  const auto tree = asGaussianBayesNetTree();
+  return sum.empty() ? tree : sum.apply(tree, add);
+}
+
+/* *******************************************************************************/
+GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const {
+  auto wrap = [](const GaussianConditional::shared_ptr &gc) {
+    if (gc) {
+      return GaussianBayesNet{gc};
+    } else {
+      return GaussianBayesNet();
+    }
+  };
+  return {conditionals_, wrap};
+}
+
 /* *******************************************************************************/
 size_t GaussianMixture::nrComponents() const {
   size_t total = 0;
@@ -316,19 +345,48 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
 AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
     const VectorValues &continuousValues) const {
   auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) {
-    return conditional->error(continuousValues) +  //
-           logConstant_ - conditional->logNormalizationConstant();
+    // Check if valid pointer
+    if (conditional) {
+      return conditional->error(continuousValues) +  //
+             logConstant_ - conditional->logNormalizationConstant();
+    } else {
+      // If not valid, pointer, it means this conditional was pruned,
+      // so we return maximum error.
+      return std::numeric_limits<double>::max();
+    }
   };
   DecisionTree<Key, double> error_tree(conditionals_, errorFunc);
   return error_tree;
 }
 
 /* *******************************************************************************/
 double GaussianMixture::error(const HybridValues &values) const {
+  // Check if discrete keys in discrete assignment are
+  // present in the GaussianMixture
+  KeyVector dKeys = this->discreteKeys_.indices();
+  bool valid_assignment = false;
+  for (auto &&kv : values.discrete()) {
+    if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) {
+      valid_assignment = true;
+      break;
+    }
+  }
+
+  // The discrete assignment is not valid so we return 0.0 erorr.
+  if (!valid_assignment) {
+    return 0.0;
+  }
+
   // Directly index to get the conditional, no need to build the whole tree.
   auto conditional = conditionals_(values.discrete());
-  return conditional->error(values.continuous()) +  //
-         logConstant_ - conditional->logNormalizationConstant();
+  if (conditional) {
+    return conditional->error(values.continuous()) +  //
+           logConstant_ - conditional->logNormalizationConstant();
+  } else {
+    // If not valid, pointer, it means this conditional was pruned,
+    // so we return maximum error.
+    return std::numeric_limits<double>::max();
+  }
 }
 
 /* *******************************************************************************/

gtsam/hybrid/GaussianMixture.h

@@ -67,10 +67,17 @@ class GTSAM_EXPORT GaussianMixture
   double logConstant_;         ///< log of the normalization constant.
 
   /**
-   * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+   * @brief Convert a DecisionTree of factors into
+   * a DecisionTree of Gaussian factor graphs.
    */
   GaussianFactorGraphTree asGaussianFactorGraphTree() const;
 
+  /**
+   * @brief Convert a DecisionTree of conditionals into
+   * a DecisionTree of Gaussian Bayes nets.
+   */
+  GaussianBayesNetTree asGaussianBayesNetTree() const;
+
   /**
    * @brief Helper function to get the pruner functor.
    *
@@ -248,6 +255,15 @@ class GTSAM_EXPORT GaussianMixture
    * @return GaussianFactorGraphTree
    */
   GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
+
+  /**
+   * @brief Merge the Gaussian Bayes Nets in `this` and `sum` while
+   * maintaining the decision tree structure.
+   *
+   * @param sum Decision Tree of Gaussian Bayes Nets
+   * @return GaussianBayesNetTree
+   */
+  GaussianBayesNetTree add(const GaussianBayesNetTree &sum) const;
   /// @}
 
  private:

gtsam/hybrid/HybridBayesNet.cpp

@@ -26,6 +26,16 @@ static std::mt19937_64 kRandomNumberGenerator(42);
 
 namespace gtsam {
 
+/* ************************************************************************ */
+// Throw a runtime exception for method specified in string s,
+// and conditional f:
+static void throwRuntimeError(const std::string &s,
+                              const std::shared_ptr<HybridConditional> &f) {
+  auto &fr = *f;
+  throw std::runtime_error(s + " not implemented for conditional type " +
+                           demangle(typeid(fr).name()) + ".");
+}
+
 /* ************************************************************************* */
 void HybridBayesNet::print(const std::string &s,
                            const KeyFormatter &formatter) const {
@@ -217,18 +227,160 @@ GaussianBayesNet HybridBayesNet::choose(
   return gbn;
 }
 
+/* ************************************************************************ */
+static GaussianBayesNetTree addGaussian(
+    const GaussianBayesNetTree &gfgTree,
+    const GaussianConditional::shared_ptr &factor) {
+  // If the decision tree is not initialized, then initialize it.
+  if (gfgTree.empty()) {
+    GaussianBayesNet result{factor};
+    return GaussianBayesNetTree(result);
+  } else {
+    auto add = [&factor](const GaussianBayesNet &graph) {
+      auto result = graph;
+      result.push_back(factor);
+      return result;
+    };
+    return gfgTree.apply(add);
+  }
+}
+
+/* ************************************************************************ */
+GaussianBayesNetValTree HybridBayesNet::assembleTree() const {
+  GaussianBayesNetTree result;
+
+  for (auto &f : factors_) {
+    // TODO(dellaert): just use a virtual method defined in HybridFactor.
+    if (auto gm = std::dynamic_pointer_cast<GaussianMixture>(f)) {
+      result = gm->add(result);
+    } else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(f)) {
+      if (auto gm = hc->asMixture()) {
+        result = gm->add(result);
+      } else if (auto g = hc->asGaussian()) {
+        result = addGaussian(result, g);
+      } else {
+        // Has to be discrete.
+        // TODO(dellaert): in C++20, we can use std::visit.
+        continue;
+      }
+    } else if (std::dynamic_pointer_cast<DiscreteFactor>(f)) {
+      // Don't do anything for discrete-only factors
+      // since we want to evaluate continuous values only.
+      continue;
+    } else {
+      // We need to handle the case where the object is actually an
+      // BayesTreeOrphanWrapper!
+      throwRuntimeError("HybridBayesNet::assembleTree", f);
+    }
+  }
+
+  GaussianBayesNetValTree resultTree(result, [](const GaussianBayesNet &gbn) {
+    return std::make_pair(gbn, 0.0);
+  });
+  return resultTree;
+}
+
+/* ************************************************************************* */
+AlgebraicDecisionTree<Key> HybridBayesNet::model_selection() const {
+  /*
+      To perform model selection, we need:
+      q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma))
+
+      If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma))
+      thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k))
+      = exp(log(q) - log(k)) = exp(-error - log(k))
+      = exp(-(error + log(k))),
+      where error is computed at the corresponding MAP point, gbn.error(mu).
+
+      So we compute (error + log(k)) and exponentiate later
+    */
+
+  GaussianBayesNetValTree bnTree = assembleTree();
+
+  GaussianBayesNetValTree bn_error = bnTree.apply(
+      [this](const Assignment<Key> &assignment,
+             const std::pair<GaussianBayesNet, double> &gbnAndValue) {
+        //  Compute the X* of each assignment
+        VectorValues mu = gbnAndValue.first.optimize();
+
+        // mu is empty if gbn had nullptrs
+        if (mu.size() == 0) {
+          return std::make_pair(gbnAndValue.first,
+                                std::numeric_limits<double>::max());
+        }
+
+        // Compute the error for X* and the assignment
+        double error =
+            this->error(HybridValues(mu, DiscreteValues(assignment)));
+
+        return std::make_pair(gbnAndValue.first, error);
+      });
+
+  auto trees = unzip(bn_error);
+  AlgebraicDecisionTree<Key> errorTree = trees.second;
+
+  // Only compute logNormalizationConstant
+  AlgebraicDecisionTree<Key> log_norm_constants = DecisionTree<Key, double>(
+      bnTree, [](const std::pair<GaussianBayesNet, double> &gbnAndValue) {
+        GaussianBayesNet gbn = gbnAndValue.first;
+        if (gbn.size() == 0) {
+          return 0.0;
+        }
+        return gbn.logNormalizationConstant();
+      });
+
+  // Compute model selection term (with help from ADT methods)
+  AlgebraicDecisionTree<Key> model_selection_term =
+      (errorTree + log_norm_constants) * -1;
+
+  double max_log = model_selection_term.max();
+  AlgebraicDecisionTree<Key> model_selection = DecisionTree<Key, double>(
+      model_selection_term,
+      [&max_log](const double &x) { return std::exp(x - max_log); });
+  model_selection = model_selection.normalize(model_selection.sum());
+
+  return model_selection;
+}
+
 /* ************************************************************************* */
 HybridValues HybridBayesNet::optimize() const {
   // Collect all the discrete factors to compute MPE
-  DiscreteBayesNet discrete_bn;
+  DiscreteFactorGraph discrete_fg;
+
+  // Compute model selection term
+  AlgebraicDecisionTree<Key> model_selection_term = model_selection();
+
+  // Get the set of all discrete keys involved in model selection
+  std::set<DiscreteKey> discreteKeySet;
   for (auto &&conditional : *this) {
     if (conditional->isDiscrete()) {
-      discrete_bn.push_back(conditional->asDiscrete());
+      discrete_fg.push_back(conditional->asDiscrete());
+    } else {
+      if (conditional->isContinuous()) {
+        /*
+        If we are here, it means there are no discrete variables in
+        the Bayes net (due to strong elimination ordering).
+        This is a continuous-only problem hence model selection doesn't matter.
+        */
+
+      } else if (conditional->isHybrid()) {
+        auto gm = conditional->asMixture();
+        // Include the discrete keys
+        std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(),
+                  std::inserter(discreteKeySet, discreteKeySet.end()));
+      }
     }
   }
 
+  // Only add model_selection if we have discrete keys
+  if (discreteKeySet.size() > 0) {
+    discrete_fg.push_back(DecisionTreeFactor(
+        DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()),
+        model_selection_term));
+  }
+
   // Solve for the MPE
-  DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize();
+  DiscreteValues mpe = discrete_fg.optimize();
 
   // Given the MPE, compute the optimal continuous values.
   return HybridValues(optimize(mpe), mpe);

gtsam/hybrid/HybridBayesNet.h

@@ -118,6 +118,29 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
     return evaluate(values);
   }
 
+  /**
+   * @brief Assemble a DecisionTree of (GaussianBayesNet, double) leaves for
+   * each discrete assignment.
+   * The included double value is used to make
+   * constructing the model selection term cleaner and more efficient.
+   *
+   * @return GaussianBayesNetValTree
+   */
+  GaussianBayesNetValTree assembleTree() const;
+
+  /*
+    Perform the integration of L(X;M,Z)P(X|M)
+    which is the model selection term.
+
+    By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M),
+    hence L(X;M,Z)P(X|M) is the unnormalized probabilty of
+    the joint Gaussian distribution.
+
+    This can be computed by multiplying all the exponentiated errors
+    of each of the conditionals.
+  */
+  AlgebraicDecisionTree<Key> model_selection() const;
+
   /**
    * @brief Solve the HybridBayesNet by first computing the MPE of all the
    * discrete variables and then optimizing the continuous variables based on

gtsam/hybrid/HybridFactor.h

@@ -33,6 +33,14 @@ class HybridValues;
 
 /// Alias for DecisionTree of GaussianFactorGraphs
 using GaussianFactorGraphTree = DecisionTree<Key, GaussianFactorGraph>;
+/// Alias for DecisionTree of GaussianBayesNets
+using GaussianBayesNetTree = DecisionTree<Key, GaussianBayesNet>;
+/**
+ * Alias for DecisionTree of (GaussianBayesNet, double) pairs.
+ * Used for model selection in BayesNet::optimize
+ */
+using GaussianBayesNetValTree =
+    DecisionTree<Key, std::pair<GaussianBayesNet, double>>;
 
 KeyVector CollectKeys(const KeyVector &continuousKeys,
                       const DiscreteKeys &discreteKeys);