diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index e6b2eb47f..b3009d5b9 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/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, diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 177513f60..af4c993df 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -30,11 +30,16 @@ std::pair // 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? diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 7292cb3f5..f97989506 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/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 dt(C(1), + boost::make_shared(X(1), + I_3x3, + Z_3x1), + boost::make_shared(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;