From efa37ff315fb0ad9f837e2bb8a589afcd2cb9256 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 11 Mar 2022 19:20:39 -0500 Subject: [PATCH] Add better mocking for actual elimination --- gtsam/hybrid/HybridBayesTree.cpp | 5 ++- gtsam/hybrid/HybridBayesTree.h | 5 ++- gtsam/hybrid/HybridConditional.h | 4 +++ gtsam/hybrid/HybridDiscreteFactor.cpp | 11 ++++-- gtsam/hybrid/HybridDiscreteFactor.h | 5 +++ gtsam/hybrid/HybridFactorGraph.cpp | 37 ++++++++++++++------ gtsam/hybrid/HybridGaussianFactor.cpp | 2 +- gtsam/hybrid/HybridJunctionTree.cpp | 5 ++- gtsam/hybrid/tests/testHybridConditional.cpp | 5 +++ 9 files changed, 57 insertions(+), 22 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 72f3fd794..85e6c84ae 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/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 diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 2ea40cecc..06df66112 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/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 diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index d6dd8250f..e9aa5f616 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/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. diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index fded0a2df..0bd2c9a64 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/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(std::move(dtf))) { +HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) + : Base(dtf.keys()), + inner(boost::make_shared(std::move(dtf))) { +} + +bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { + return false; }; } \ No newline at end of file diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 4b1c00672..cfb94bc69 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/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; }; } diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 686fddc51..0f9d8ddca 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -7,17 +7,19 @@ #include #include +#include #include namespace gtsam { -template class EliminateableFactorGraph; +template +class EliminateableFactorGraph; /* ************************************************************************ */ std::pair // -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(), boost::make_shared(product)); + return std::make_pair(boost::make_shared(frontalKeys.size(), + orderedKeys), + boost::make_shared(std::move(sum))); } } diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 1910c3307..1e87cbbc3 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/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; } diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index eabfc2e7c..0e3d2ea00 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/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 diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 295d6011d..6eef100c1 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -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); } /* ************************************************************************* */