Add better mocking for actual elimination

gtsam/hybrid/HybridBayesTree.cpp

@@ -12,9 +12,8 @@
 /**
  * @file    HybridBayesTree.cpp
  * @brief   Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
- * @brief   HybridBayesTree
- * @author  Frank Dellaert
- * @author  Richard Roberts
+ * @date Mar 11, 2022
+ * @author  Fan Jiang
  */
 
 #include <gtsam/base/treeTraversal-inst.h>

gtsam/hybrid/HybridBayesTree.h

@@ -13,9 +13,8 @@
  * @file    HybridBayesTree.h
  * @brief   Hybrid Bayes Tree, the result of eliminating a
  * HybridJunctionTree
- * @brief   HybridBayesTree
- * @author  Frank Dellaert
- * @author  Richard Roberts
+ * @date Mar 11, 2022
+ * @author  Fan Jiang
  */
 
 #pragma once

gtsam/hybrid/HybridConditional.h

@@ -59,6 +59,10 @@ public:
 /// Default constructor needed for serialization.
 HybridConditional() = default;
 
+HybridConditional(size_t nFrontals, const KeyVector& keys) : BaseFactor(keys), BaseConditional(nFrontals) {
+
+}
+
 /**
  * @brief Combine two conditionals, yielding a new conditional with the union
  * of the frontal keys, ordered by gtsam::Key.

gtsam/hybrid/HybridDiscreteFactor.cpp

@@ -8,12 +8,19 @@
 
 namespace gtsam {
 
-HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) {
+HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
+    : Base(other->keys()) {
   inner = other;
 }
 
-HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
+HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
+    : Base(dtf.keys()),
+      inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
 
+}
+
+bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
+  return false;
 };
 
 }

gtsam/hybrid/HybridDiscreteFactor.h

@@ -23,6 +23,8 @@ namespace gtsam {
 
 class HybridDiscreteFactor : public HybridFactor {
  public:
+  using Base = HybridFactor;
+
   DiscreteFactor::shared_ptr inner;
 
   // Implicit conversion from a shared ptr of GF
@@ -30,5 +32,8 @@ class HybridDiscreteFactor : public HybridFactor {
 
   // Forwarding constructor from concrete JacobianFactor
   HybridDiscreteFactor(DecisionTreeFactor &&dtf);
+
+ public:
+  virtual bool equals(const HybridFactor& lf, double tol) const override;
 };
 }

gtsam/hybrid/HybridFactorGraph.cpp

@@ -7,17 +7,19 @@
 #include <gtsam/hybrid/HybridFactorGraph.h>
 
 #include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
 
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 
 namespace gtsam {
 
-template class EliminateableFactorGraph<HybridFactorGraph>;
+template
+class EliminateableFactorGraph<HybridFactorGraph>;
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
-EliminateHybrid(const HybridFactorGraph& factors,
-                  const Ordering& frontalKeys) {
+EliminateHybrid(const HybridFactorGraph &factors,
+                const Ordering &frontalKeys) {
   // NOTE(fan): Because we are in the Conditional Gaussian regime there are only
   // few cases: continuous variable, we make a GM if there are hybrid factors;
   // continuous variable, we make a GF if there are no hybrid factors;
@@ -29,7 +31,20 @@ EliminateHybrid(const HybridFactorGraph& factors,
 
   // PRODUCT: multiply all factors
   gttic(product);
-  HybridGaussianFactor product(JacobianFactor(0, I_3x3, Z_3x1));
+  KeySet allKeys;
+  // TODO: we do a mock by just doing the correct key thing
+  std::cout << "Begin Eliminate\n";
+  for (auto &&factor : factors) {
+    std::cout << ">>> Eliminating: ";
+    factor->printKeys();
+    allKeys.insert(factor->begin(), factor->end());
+  }
+  for (auto &k : frontalKeys) {
+    allKeys.erase(k);
+  }
+
+  HybridConditional sum(allKeys.size(), Ordering(allKeys));
+//  HybridDiscreteFactor product(DiscreteConditional());
 //  for (auto&& factor : factors) product = (*factor) * product;
   gttoc(product);
 
@@ -39,11 +54,11 @@ EliminateHybrid(const HybridFactorGraph& factors,
   gttoc(sum);
 
   // Ordering keys for the conditional so that frontalKeys are really in front
-//  Ordering orderedKeys;
-//  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
-//                     frontalKeys.end());
-//  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(),
-//                     sum->keys().end());
+  Ordering orderedKeys;
+  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
+                     frontalKeys.end());
+  orderedKeys.insert(orderedKeys.end(), sum.keys().begin(),
+                     sum.keys().end());
 
   // now divide product/sum to get conditional
   gttic(divide);
@@ -52,7 +67,9 @@ EliminateHybrid(const HybridFactorGraph& factors,
   gttoc(divide);
 
 //  return std::make_pair(conditional, sum);
-  return std::make_pair(boost::make_shared<HybridConditional>(), boost::make_shared<HybridGaussianFactor>(product));
+  return std::make_pair(boost::make_shared<HybridConditional>(frontalKeys.size(),
+                                                              orderedKeys),
+                        boost::make_shared<HybridConditional>(std::move(sum)));
 }
 
 }

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -8,7 +8,7 @@
 
 namespace gtsam {
 
-HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()){
+HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) {
   inner = other;
 }
 

gtsam/hybrid/HybridJunctionTree.cpp

@@ -11,9 +11,8 @@
 
 /**
  * @file HybridJunctionTree.cpp
- * @date Mar 29, 2013
- * @author Frank Dellaert
- * @author Richard Roberts
+ * @date Mar 11, 2022
+ * @author Fan Jiang
  */
 
 #include <gtsam/inference/JunctionTree-inst.h>

gtsam/hybrid/tests/testHybridConditional.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 
@@ -57,11 +58,15 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) {
 TEST(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
+  DiscreteKey X(1, 2);
+
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(X, {2, 8})));
 
   auto result = hfg.eliminatePartialMultifrontal({0});
 
   GTSAM_PRINT(*result.first);
+  GTSAM_PRINT(*result.second);
 }
 
 /* ************************************************************************* */