add more comments

gtsam/hybrid/HybridConditional.cpp

@@ -7,8 +7,25 @@
 
 namespace gtsam {
 void HybridConditional::print(const std::string &s,
-                                     const KeyFormatter &formatter) const {
-  Conditional::print(s, formatter);
+                              const KeyFormatter &formatter) const {
+  std::cout << s << "P(";
+  int index = 0;
+  const size_t N = keys().size();
+  const size_t contN = N - discreteKeys_.size();
+  while (index < N) {
+    if (index > 0) {
+      if (index == nrFrontals_) std::cout << " | "; else std::cout << ", ";
+    }
+    if (index < contN) {
+      std::cout << formatter(keys()[index]);
+    } else {
+      auto &dk = discreteKeys_[index - contN];
+      std::cout << "(" << formatter(dk.first) << ", " << dk.second << ")";
+    }
+    index++;
+  }
+  std::cout << ")\n";
+  if (inner) inner->print("", formatter);
 }
 
 bool HybridConditional::equals(const HybridFactor &other,

gtsam/hybrid/HybridFactorGraph.cpp

@@ -30,11 +30,16 @@ std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
 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;
+  // a 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;
   // discrete variable, no continuous factor is allowed (escapes CG regime), so
   // we panic, if discrete only we do the discrete elimination.
 
+  // However it is not that simple. During elimination it is possible that the multifrontal needs
+  // to eliminate an ordering that contains both Gaussian and hybrid variables, for example x1, c1.
+  // In this scenario, we will have a density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02)
+
   // The issue here is that, how can we know which variable is discrete if we
   // unify Values? Obviously we can tell using the factors, but is that fast?
 

gtsam/hybrid/tests/testHybridConditional.cpp

@@ -88,7 +88,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
-TEST(HybridFactorGraph, eliminateFullMultifrontal) {
+TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
 
   std::cout << ">>>>>>>>>>>>>>\n";
 
@@ -117,6 +117,35 @@ TEST(HybridFactorGraph, eliminateFullMultifrontal) {
   GTSAM_PRINT(*result->marginalFactor(C(1)));
 }
 
+TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
+
+  std::cout << ">>>>>>>>>>>>>>\n";
+
+  HybridFactorGraph hfg;
+
+  DiscreteKey x(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));
+
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(C(1),
+                                                   boost::make_shared<JacobianFactor>(X(1),
+                                                                                      I_3x3,
+                                                                                      Z_3x1),
+                                                   boost::make_shared<JacobianFactor>(X(1),
+                                                                                      I_3x3,
+                                                                                      Vector3::Ones()));
+
+  hfg.add(CGMixtureFactor({X(1)}, {x}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {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)));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;