From fe5dde7e27e080f5a9abcf4692bdf16dc3164fce Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 14 Mar 2022 19:22:20 -0400
Subject: [PATCH] even better printing and comments

---
 gtsam/hybrid/HybridFactorGraph.cpp           | 30 ++++++++++++++++----
 gtsam/hybrid/tests/testHybridConditional.cpp | 26 ++++++++++++-----
 2 files changed, 44 insertions(+), 12 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index af4c993df..03610166e 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -2,6 +2,7 @@
 // Created by Fan Jiang on 3/11/22.
 //
 
+#include "gtsam/inference/Key.h"
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
@@ -12,6 +13,7 @@
 
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 
+#include <iostream>
 #include <unordered_map>
 
 namespace gtsam {
@@ -57,6 +59,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
   KeySet continuousSeparator;
 
   // TODO: we do a mock by just doing the correct key thing
+  std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
+  frontalKeys.print();
 
   // This initializes separatorKeys and discreteCardinalities
   for (auto &&factor : factors) {
@@ -94,12 +98,28 @@ EliminateHybrid(const HybridFactorGraph &factors,
     }
   }
 
-  std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
-  frontalKeys.print();
+  std::cout << RED_BOLD << "Keys: " << RESET;
+  for (auto &f : frontalKeys) {
+    if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
+      auto &key = discreteCardinalities.at(f);
+      std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
+    } else {
+      std::cout << " " << DefaultKeyFormatter(f) << ",";
+    }
+  }
 
-  std::cout << RED_BOLD << "Discrete Keys: " << RESET;
-  for (auto &&key : discreteCardinalities)
-    std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.second.first) % key.second.second;
+  if (separatorKeys.size() > 0) {
+    std::cout << " | ";
+  }
+
+  for (auto &f : separatorKeys) {
+    if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
+      auto &key = discreteCardinalities.at(f);
+      std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
+    } else {
+      std::cout << DefaultKeyFormatter(f) << ",";
+    }
+  }
   std::cout << "\n" << RESET;
   // PRODUCT: multiply all factors
   gttic(product);
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index f97989506..c682f5216 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -15,6 +15,7 @@
  *  @author Fan Jiang
  */
 
+#include "Test.h"
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
@@ -94,7 +95,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
 
   HybridFactorGraph hfg;
 
-  DiscreteKey x(C(1), 2);
+  DiscreteKey c1(C(1), 2);
 
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
@@ -107,9 +108,12 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
                                                                                       I_3x3,
                                                                                       Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, {x}, dt));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+  hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
+  hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1 2 3 4")));
+  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2}, {C(1), 2}}, "1 2 2 1")));
 
   auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
 
@@ -123,7 +127,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
 
   HybridFactorGraph hfg;
 
-  DiscreteKey x(C(1), 2);
+  DiscreteKey c(C(1), 2);
 
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
@@ -136,14 +140,22 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
                                                                                       I_3x3,
                                                                                       Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, {x}, dt));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+  hfg.add(CGMixtureFactor({X(1)}, {c}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
 //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
 
   auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
 
   GTSAM_PRINT(*result);
-  GTSAM_PRINT(*result->marginalFactor(C(1)));
+
+  // We immediately need to escape the CLG domain if we do this!!!
+  GTSAM_PRINT(*result->marginalFactor(X(1)));
+  /*
+  Explanation: the Junction tree will need to reeliminate to get to the marginal on X(1), which
+  is not possible because it involves eliminating discrete before continuous. The solution to this,
+  however, is in Murphy02. TLDR is that this is 1. expensive and 2. inexact. neverless it is doable.
+  And I believe that we should do this.
+  */
 }
 
 /* ************************************************************************* */