Hybrid Multifrontal

gtsam/hybrid/GaussianMixture.cpp

@@ -105,7 +105,7 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
 /* *******************************************************************************/
 void GaussianMixture::print(const std::string &s,
                             const KeyFormatter &formatter) const {
-  std::cout << s;
+  std::cout << (s.empty() ? "" : s + "\n");
   if (isContinuous()) std::cout << "Continuous ";
   if (isDiscrete()) std::cout << "Discrete ";
   if (isHybrid()) std::cout << "Hybrid ";

gtsam/hybrid/HybridBayesTree.cpp

@@ -73,6 +73,8 @@ struct HybridAssignmentData {
   GaussianBayesTree::sharedNode parentClique_;
   // The gaussian bayes tree that will be recursively created.
   GaussianBayesTree* gaussianbayesTree_;
+  // Flag indicating if all the nodes are valid. Used in optimize().
+  bool valid_;
 
   /**
    * @brief Construct a new Hybrid Assignment Data object.
@@ -83,10 +85,13 @@ struct HybridAssignmentData {
    */
   HybridAssignmentData(const DiscreteValues& assignment,
                        const GaussianBayesTree::sharedNode& parentClique,
-                       GaussianBayesTree* gbt)
+                       GaussianBayesTree* gbt, bool valid = true)
       : assignment_(assignment),
         parentClique_(parentClique),
-        gaussianbayesTree_(gbt) {}
+        gaussianbayesTree_(gbt),
+        valid_(valid) {}
+
+  bool isValid() const { return valid_; }
 
   /**
    * @brief A function used during tree traversal that operates on each node
@@ -101,6 +106,7 @@ struct HybridAssignmentData {
       HybridAssignmentData& parentData) {
     // Extract the gaussian conditional from the Hybrid clique
     HybridConditional::shared_ptr hybrid_conditional = node->conditional();
+
     GaussianConditional::shared_ptr conditional;
     if (hybrid_conditional->isHybrid()) {
       conditional = (*hybrid_conditional->asMixture())(parentData.assignment_);
@@ -111,15 +117,21 @@ struct HybridAssignmentData {
       conditional = boost::make_shared<GaussianConditional>();
     }
 
-    // Create the GaussianClique for the current node
-    auto clique = boost::make_shared<GaussianBayesTree::Node>(conditional);
-    // Add the current clique to the GaussianBayesTree.
-    parentData.gaussianbayesTree_->addClique(clique, parentData.parentClique_);
+    GaussianBayesTree::sharedNode clique;
+    if (conditional) {
+      // Create the GaussianClique for the current node
+      clique = boost::make_shared<GaussianBayesTree::Node>(conditional);
+      // Add the current clique to the GaussianBayesTree.
+      parentData.gaussianbayesTree_->addClique(clique,
+                                               parentData.parentClique_);
+    } else {
+      parentData.valid_ = false;
+    }
 
     // Create new HybridAssignmentData where the current node is the parent
     // This will be passed down to the children nodes
     HybridAssignmentData data(parentData.assignment_, clique,
-                              parentData.gaussianbayesTree_);
+                              parentData.gaussianbayesTree_, parentData.valid_);
     return data;
   }
 };
@@ -138,6 +150,9 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
         visitorPost);
   }
 
+  if (!rootData.isValid()) {
+    return VectorValues();
+  }
   VectorValues result = gbt.optimize();
 
   // Return the optimized bayes net result.
@@ -151,6 +166,8 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
 
   DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves);
   decisionTree->root_ = prunedDecisionTree.root_;
+  // this->print();
+  // decisionTree->print("", DefaultKeyFormatter);
 
   /// Helper struct for pruning the hybrid bayes tree.
   struct HybridPrunerData {

gtsam/hybrid/HybridBayesTree.h

@@ -50,9 +50,12 @@ class GTSAM_EXPORT HybridBayesTreeClique
   typedef boost::shared_ptr<This> shared_ptr;
   typedef boost::weak_ptr<This> weak_ptr;
   HybridBayesTreeClique() {}
-  virtual ~HybridBayesTreeClique() {}
   HybridBayesTreeClique(const boost::shared_ptr<HybridConditional>& conditional)
       : Base(conditional) {}
+  ///< Copy constructor
+  HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {}
+
+  virtual ~HybridBayesTreeClique() {}
 };
 
 /* ************************************************************************* */

gtsam/hybrid/HybridEliminationTree.h

@@ -24,7 +24,7 @@
 namespace gtsam {
 
 /**
- * Elimination Tree type for Hybrid
+ * Elimination Tree type for Hybrid Factor Graphs.
  *
  * @ingroup hybrid
  */

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -257,13 +257,14 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   // If there are no more continuous parents, then we should create here a
   // DiscreteFactor, with the error for each discrete choice.
   if (keysOfSeparator.empty()) {
-    // TODO(Varun) Use the math from the iMHS_Math-1-indexed document
     VectorValues empty_values;
     auto factorProb = [&](const GaussianFactor::shared_ptr &factor) {
       if (!factor) {
         return 0.0;  // If nullptr, return 0.0 probability
       } else {
-        return 1.0;
+        double error =
+            0.5 * std::abs(factor->augmentedInformation().determinant());
+        return std::exp(-error);
       }
     };
     DecisionTree<Key, double> fdt(separatorFactors, factorProb);
@@ -529,122 +530,13 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::probPrime(
 }
 
 /* ************************************************************************ */
-DecisionTree<Key, VectorValues::shared_ptr>
-HybridGaussianFactorGraph::continuousDelta(
-    const DiscreteKeys &discrete_keys,
-    const boost::shared_ptr<BayesNetType> &continuousBayesNet,
-    const std::vector<DiscreteValues> &assignments) const {
-  // Create a decision tree of all the different VectorValues
-  std::vector<VectorValues::shared_ptr> vector_values;
-  for (const DiscreteValues &assignment : assignments) {
-    VectorValues values = continuousBayesNet->optimize(assignment);
-    vector_values.push_back(boost::make_shared<VectorValues>(values));
-  }
-  DecisionTree<Key, VectorValues::shared_ptr> delta_tree(discrete_keys,
-                                                         vector_values);
-
-  return delta_tree;
-}
-
-/* ************************************************************************ */
-AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::continuousProbPrimes(
-    const DiscreteKeys &orig_discrete_keys,
-    const boost::shared_ptr<BayesNetType> &continuousBayesNet) const {
-  // Generate all possible assignments.
-  const std::vector<DiscreteValues> assignments =
-      DiscreteValues::CartesianProduct(orig_discrete_keys);
-
-  // Save a copy of the original discrete key ordering
-  DiscreteKeys discrete_keys(orig_discrete_keys);
-  // Reverse discrete keys order for correct tree construction
-  std::reverse(discrete_keys.begin(), discrete_keys.end());
-
-  // Create a decision tree of all the different VectorValues
-  DecisionTree<Key, VectorValues::shared_ptr> delta_tree =
-      this->continuousDelta(discrete_keys, continuousBayesNet, assignments);
-
-  // Get the probPrime tree with the correct leaf probabilities
-  std::vector<double> probPrimes;
-  for (const DiscreteValues &assignment : assignments) {
-    VectorValues delta = *delta_tree(assignment);
-
-    // If VectorValues is empty, it means this is a pruned branch.
-    // Set thr probPrime to 0.0.
-    if (delta.size() == 0) {
-      probPrimes.push_back(0.0);
-      continue;
-    }
-
-    // Compute the error given the delta and the assignment.
-    double error = this->error(delta, assignment);
-    probPrimes.push_back(exp(-error));
-  }
-
-  AlgebraicDecisionTree<Key> probPrimeTree(discrete_keys, probPrimes);
-  return probPrimeTree;
-}
-
-/* ************************************************************************ */
-boost::shared_ptr<HybridGaussianFactorGraph::BayesNetType>
-HybridGaussianFactorGraph::eliminateHybridSequential(
-    const boost::optional<Ordering> continuous,
-    const boost::optional<Ordering> discrete, const Eliminate &function,
-    OptionalVariableIndex variableIndex) const {
-  Ordering continuous_ordering =
-      continuous ? *continuous : Ordering(this->continuousKeys());
-  Ordering discrete_ordering =
-      discrete ? *discrete : Ordering(this->discreteKeys());
-
-  // Eliminate continuous
-  HybridBayesNet::shared_ptr bayesNet;
-  HybridGaussianFactorGraph::shared_ptr discreteGraph;
-  std::tie(bayesNet, discreteGraph) =
-      BaseEliminateable::eliminatePartialSequential(continuous_ordering,
-                                                    function, variableIndex);
-
-  // Get the last continuous conditional which will have all the discrete keys
-  auto last_conditional = bayesNet->at(bayesNet->size() - 1);
-  DiscreteKeys discrete_keys = last_conditional->discreteKeys();
-
-  // If not discrete variables, return the eliminated bayes net.
-  if (discrete_keys.size() == 0) {
-    return bayesNet;
-  }
-
-  AlgebraicDecisionTree<Key> probPrimeTree =
-      this->continuousProbPrimes(discrete_keys, bayesNet);
-
-  discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
-
-  // Perform discrete elimination
-  HybridBayesNet::shared_ptr discreteBayesNet =
-      discreteGraph->BaseEliminateable::eliminateSequential(
-          discrete_ordering, function, variableIndex);
-
-  bayesNet->add(*discreteBayesNet);
-
-  return bayesNet;
-}
-
-/* ************************************************************************ */
-boost::shared_ptr<HybridGaussianFactorGraph::BayesNetType>
-HybridGaussianFactorGraph::eliminateSequential(
-    OptionalOrderingType orderingType, const Eliminate &function,
-    OptionalVariableIndex variableIndex) const {
-  return BaseEliminateable::eliminateSequential(orderingType, function,
-                                                variableIndex);
-}
-
-/* ************************************************************************ */
-boost::shared_ptr<HybridGaussianFactorGraph::BayesNetType>
-HybridGaussianFactorGraph::eliminateSequential(
-    const Ordering &ordering, const Eliminate &function,
-    OptionalVariableIndex variableIndex) const {
+std::pair<Ordering, Ordering>
+HybridGaussianFactorGraph::separateContinuousDiscreteOrdering(
+    const Ordering &ordering) const {
   KeySet all_continuous_keys = this->continuousKeys();
   KeySet all_discrete_keys = this->discreteKeys();
   Ordering continuous_ordering, discrete_ordering;
 
-  // Segregate the continuous and the discrete keys
   for (auto &&key : ordering) {
     if (std::find(all_continuous_keys.begin(), all_continuous_keys.end(),
                   key) != all_continuous_keys.end()) {
@@ -657,8 +549,7 @@ HybridGaussianFactorGraph::eliminateSequential(
     }
   }
 
-  return this->eliminateHybridSequential(continuous_ordering,
-                                         discrete_ordering);
+  return std::make_pair(continuous_ordering, discrete_ordering);
 }
 
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -217,57 +217,92 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
                    const DiscreteValues& discreteValues) const;
 
   /**
-   * @brief Compute the VectorValues solution for the continuous variables for
-   * each mode.
+   * @brief Helper method to compute the VectorValues solution for the
+   * continuous variables for each discrete mode.
+   * Used as a helper to compute q(\mu | M, Z) which is used by
+   * both P(X | M, Z) and P(M | Z).
    *
+   * @tparam BAYES Template on the type of Bayes graph, either a bayes net or a
+   * bayes tree.
    * @param discrete_keys The discrete keys which form all the modes.
-   * @param continuousBayesNet The Bayes Net representing the continuous
+   * @param continuousBayesNet The Bayes Net/Tree representing the continuous
    * eliminated variables.
    * @param assignments List of all discrete assignments to create the final
    * decision tree.
    * @return DecisionTree<Key, VectorValues::shared_ptr>
    */
+  template <typename BAYES>
   DecisionTree<Key, VectorValues::shared_ptr> continuousDelta(
       const DiscreteKeys& discrete_keys,
-      const boost::shared_ptr<BayesNetType>& continuousBayesNet,
-      const std::vector<DiscreteValues>& assignments) const;
+      const boost::shared_ptr<BAYES>& continuousBayesNet,
+      const std::vector<DiscreteValues>& assignments) const {
+    // Create a decision tree of all the different VectorValues
+    std::vector<VectorValues::shared_ptr> vector_values;
+    for (const DiscreteValues& assignment : assignments) {
+      VectorValues values = continuousBayesNet->optimize(assignment);
+      vector_values.push_back(boost::make_shared<VectorValues>(values));
+    }
+    DecisionTree<Key, VectorValues::shared_ptr> delta_tree(discrete_keys,
+                                                           vector_values);
+
+    return delta_tree;
+  }
 
   /**
    * @brief Compute the unnormalized probabilities of the continuous variables
    * for each of the modes.
    *
+   * @tparam BAYES  Template on the type of Bayes graph, either a bayes net or a
+   * bayes tree.
    * @param discrete_keys The discrete keys which form all the modes.
    * @param continuousBayesNet The Bayes Net representing the continuous
    * eliminated variables.
    * @return AlgebraicDecisionTree<Key>
    */
+  template <typename BAYES>
   AlgebraicDecisionTree<Key> continuousProbPrimes(
       const DiscreteKeys& discrete_keys,
-      const boost::shared_ptr<BayesNetType>& continuousBayesNet) const;
+      const boost::shared_ptr<BAYES>& continuousBayesNet) const {
+    // Generate all possible assignments.
+    const std::vector<DiscreteValues> assignments =
+        DiscreteValues::CartesianProduct(discrete_keys);
+
+    // Save a copy of the original discrete key ordering
+    DiscreteKeys reversed_discrete_keys(discrete_keys);
+    // Reverse discrete keys order for correct tree construction
+    std::reverse(reversed_discrete_keys.begin(), reversed_discrete_keys.end());
+
+    // Create a decision tree of all the different VectorValues
+    DecisionTree<Key, VectorValues::shared_ptr> delta_tree =
+        this->continuousDelta(reversed_discrete_keys, continuousBayesNet,
+                              assignments);
+
+    // Get the probPrime tree with the correct leaf probabilities
+    std::vector<double> probPrimes;
+    for (const DiscreteValues& assignment : assignments) {
+      VectorValues delta = *delta_tree(assignment);
+
+      // If VectorValues is empty, it means this is a pruned branch.
+      // Set thr probPrime to 0.0.
+      if (delta.size() == 0) {
+        probPrimes.push_back(0.0);
+        continue;
+      }
+
+      // Compute the error given the delta and the assignment.
+      double error = this->error(delta, assignment);
+      probPrimes.push_back(exp(-error));
+    }
 
-  /**
-   * @brief Custom elimination function which computes the correct
-   * continuous probabilities.
-   *
-   * @param continuous Optional ordering for all continuous variables.
-   * @param discrete Optional ordering for all discrete variables.
-   * @return boost::shared_ptr<BayesNetType>
-   */
-  boost::shared_ptr<BayesNetType> eliminateHybridSequential(
-      const boost::optional<Ordering> continuous = boost::none,
-      const boost::optional<Ordering> discrete = boost::none,
-      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-      OptionalVariableIndex variableIndex = boost::none) const;
-
-  boost::shared_ptr<BayesNetType> eliminateSequential(
-      OptionalOrderingType orderingType = boost::none,
-      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-      OptionalVariableIndex variableIndex = boost::none) const;
-
-  boost::shared_ptr<BayesNetType> eliminateSequential(
-      const Ordering& ordering,
-      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-      OptionalVariableIndex variableIndex = boost::none) const;
+    AlgebraicDecisionTree<Key> probPrimeTree(reversed_discrete_keys,
+                                             probPrimes);
+    return probPrimeTree;
+  }
+
+  std::pair<Ordering, Ordering> separateContinuousDiscreteOrdering(
+      const Ordering& ordering) const;
+
+
 
   /**
    * @brief Return a Colamd constrained ordering where the discrete keys are

gtsam/hybrid/HybridJunctionTree.h

@@ -51,10 +51,11 @@ class HybridEliminationTree;
  */
 class GTSAM_EXPORT HybridJunctionTree
     : public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
+
  public:
   typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
       Base;                                    ///< Base class
-  typedef HybridJunctionTree This;     ///< This class
+  typedef HybridJunctionTree This;             ///< This class
   typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
 
   /**