Add full elimination

gtsam/hybrid/HybridFactorGraph.h

@@ -70,12 +70,14 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>, public Eliminateable
  public:
   HybridFactorGraph() = default;
 
-  /** Construct from container of factors (shared_ptr or plain objects) */
+//  /** Construct from container of factors (shared_ptr or plain objects) */
-  template <class CONTAINER>
+//  template <class CONTAINER>
-  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
+//  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
 
   /** Implicit copy/downcast constructor to override explicit template container
-   * constructor */
+   * constructor. In BayesTree this is used for:
+   * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
+   * */
   template <class DERIVEDFACTOR>
   HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 

gtsam/hybrid/tests/testHybridConditional.cpp

@@ -73,6 +73,19 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
   GTSAM_PRINT(*result.second);
 }
 
+TEST(HybridFactorGraph, eliminateFullMultifrontal) {
+  HybridFactorGraph hfg;
+
+  DiscreteKey x(X(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+
+  auto result = hfg.eliminateMultifrontal();
+
+  GTSAM_PRINT(*result);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;