Proper Hybrid Elimination

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/HybridBayesNet.cpp

@@ -99,6 +99,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
 }
 
 /* ************************************************************************* */
+// TODO(dellaert): what is this non-const method used for? Abolish it?
 void HybridBayesNet::updateDiscreteConditionals(
     const DecisionTreeFactor::shared_ptr &prunedDecisionTree) {
   KeyVector prunedTreeKeys = prunedDecisionTree->keys();
@@ -150,9 +151,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
 
   // Go through all the conditionals in the
   // Bayes Net and prune them as per decisionTree.
-  for (size_t i = 0; i < this->size(); i++) {
-    HybridConditional::shared_ptr conditional = this->at(i);
-
+  for (auto &&conditional : *this) {
     if (conditional->isHybrid()) {
       GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture();
 
@@ -232,6 +231,12 @@ HybridValues HybridBayesNet::optimize() const {
 /* ************************************************************************* */
 VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
   GaussianBayesNet gbn = this->choose(assignment);
+
+  // Check if there exists a nullptr in the GaussianBayesNet
+  // If yes, return an empty VectorValues
+  if (std::find(gbn.begin(), gbn.end(), nullptr) != gbn.end()) {
+    return VectorValues();
+  }
   return gbn.optimize();
 }
 

gtsam/hybrid/HybridBayesTree.cpp

@@ -14,7 +14,7 @@
  * @brief   Hybrid Bayes Tree, the result of eliminating a
  * HybridJunctionTree
  * @date Mar 11, 2022
- * @author  Fan Jiang
+ * @author  Fan Jiang, Varun Agrawal
  */
 
 #include <gtsam/base/treeTraversal-inst.h>
@@ -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.

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
  */