From 2bae2865d74efc139c8c77fcfe8a4d60bcb8eaab Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 12 Mar 2022 16:29:26 -0500 Subject: [PATCH] More mock-ups added --- gtsam/hybrid/CGMixtureFactor.cpp | 14 ++++++++- gtsam/hybrid/CGMixtureFactor.h | 18 ++++++++---- gtsam/hybrid/CLGaussianConditional.cpp | 20 +++++++++++++ gtsam/hybrid/CLGaussianConditional.h | 31 ++++++++++++++++++++ gtsam/hybrid/HybridConditional.h | 2 +- gtsam/hybrid/HybridFactorGraph.cpp | 14 +++++---- gtsam/hybrid/HybridGaussianFactor.h | 6 ++-- gtsam/hybrid/tests/testHybridConditional.cpp | 25 +++++++++++----- 8 files changed, 107 insertions(+), 23 deletions(-) create mode 100644 gtsam/hybrid/CLGaussianConditional.cpp create mode 100644 gtsam/hybrid/CLGaussianConditional.h diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp index d789825f7..2ddf80ec2 100644 --- a/gtsam/hybrid/CGMixtureFactor.cpp +++ b/gtsam/hybrid/CGMixtureFactor.cpp @@ -2,4 +2,16 @@ // Created by Fan Jiang on 3/11/22. // -#include "CGMixtureFactor.h" +#include + +namespace gtsam { + +CGMixtureFactor::CGMixtureFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys, + const Factors &factors) : Base(continuousKeys, discreteKeys), + factors_(factors) {} +bool CGMixtureFactor::equals(const HybridFactor &lf, double tol) const { + return false; +} + +} \ No newline at end of file diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h index b46146eb3..9c9e43ec2 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -1,15 +1,21 @@ /* ---------------------------------------------------------------------------- - * Copyright 2021 The Ambitious Folks of the MRG + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ /** * @file CGMixtureFactor.h * @brief A set of Gaussian factors indexed by a set of discrete keys. - * @author Varun Agrawal * @author Fan Jiang + * @author Varun Agrawal * @author Frank Dellaert - * @date December 2021 + * @date Mar 12, 2022 */ #include @@ -31,9 +37,11 @@ public: CGMixtureFactor() = default; - CGMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) : Base(continuousKeys, discreteKeys) { + CGMixtureFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys, + const Factors &factors); - } + bool equals(const HybridFactor &lf, double tol = 1e-9) const override; }; } diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp new file mode 100644 index 000000000..09babc4e2 --- /dev/null +++ b/gtsam/hybrid/CLGaussianConditional.cpp @@ -0,0 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CLGaussianConditional.cpp + * @brief A hybrid conditional in the Conditional Linear Gaussian scheme + * @author Fan Jiang + * @date Mar 12, 2022 + */ + +#include + diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h new file mode 100644 index 000000000..03a2d99e7 --- /dev/null +++ b/gtsam/hybrid/CLGaussianConditional.h @@ -0,0 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CLGaussianConditional.h + * @brief A hybrid conditional in the Conditional Linear Gaussian scheme + * @author Fan Jiang + * @date Mar 12, 2022 + */ + +#include +#include + +namespace gtsam { +class CLGaussianConditional : public HybridFactor, public Conditional { +public: + using This = CLGaussianConditional; + using shared_ptr = boost::shared_ptr; + using BaseFactor = HybridFactor; + + +}; +} \ No newline at end of file diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index e9aa5f616..26083e13e 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -34,7 +34,7 @@ namespace gtsam { * * As a type-erased variant of: * - DiscreteConditional - * - GaussianMixture + * - CLGaussianConditional * - GaussianConditional */ class GTSAM_EXPORT HybridConditional diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 080efc0e9..cd5bc651d 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -32,20 +32,24 @@ EliminateHybrid(const HybridFactorGraph &factors, // In the case of multifrontal, we will need to use a constrained ordering // so that the discrete parts will be guaranteed to be eliminated last! - // PRODUCT: multiply all factors - gttic(product); + // PREPROCESS: Identify the nature of the current elimination KeySet allKeys; // TODO: we do a mock by just doing the correct key thing - std::cout << "Begin Eliminate\n"; + std::cout << "Begin Eliminate: "; + frontalKeys.print(); + for (auto &&factor : factors) { - std::cout << ">>> Eliminating: "; - factor->printKeys(); + std::cout << ">>> Adding factor: "; + factor->print(); allKeys.insert(factor->begin(), factor->end()); } for (auto &k : frontalKeys) { allKeys.erase(k); } + // PRODUCT: multiply all factors + gttic(product); + HybridConditional sum(allKeys.size(), Ordering(allKeys)); // HybridDiscreteFactor product(DiscreteConditional()); // for (auto&& factor : factors) product = (*factor) * product; diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index d8a97ba30..34a7c0004 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -29,11 +29,11 @@ class HybridGaussianFactor : public HybridFactor { GaussianFactor::shared_ptr inner; - // Implicit conversion from a shared ptr of GF - HybridGaussianFactor(GaussianFactor::shared_ptr other); + // Explicit conversion from a shared ptr of GF + explicit HybridGaussianFactor(GaussianFactor::shared_ptr other); // Forwarding constructor from concrete JacobianFactor - HybridGaussianFactor(JacobianFactor &&jf); + explicit HybridGaussianFactor(JacobianFactor &&jf); public: virtual bool equals(const HybridFactor& lf, double tol) const override; diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index d5a21f34d..46ec40475 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -37,16 +38,15 @@ using namespace std; using namespace gtsam; using gtsam::symbol_shorthand::X; +using gtsam::symbol_shorthand::C; /* ************************************************************************* */ -TEST_UNSAFE(HybridFactorGraph, test) { +TEST_UNSAFE(HybridFactorGraph, creation) { HybridConditional test; - GTSAM_PRINT(test); HybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GTSAM_PRINT(hfg); } TEST_UNSAFE(HybridFactorGraph, eliminate) { @@ -56,7 +56,7 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) { auto result = hfg.eliminatePartialSequential({0}); - GTSAM_PRINT(*result.first); + EXPECT_LONGS_EQUAL(result.first->size(), 1); } TEST(HybridFactorGraph, eliminateMultifrontal) { @@ -69,21 +69,30 @@ TEST(HybridFactorGraph, eliminateMultifrontal) { auto result = hfg.eliminatePartialMultifrontal({X(0)}); - GTSAM_PRINT(*result.first); - GTSAM_PRINT(*result.second); + EXPECT_LONGS_EQUAL(result.first->size(), 1); + EXPECT_LONGS_EQUAL(result.second->size(), 1); } TEST(HybridFactorGraph, eliminateFullMultifrontal) { + + std::cout << ">>>>>>>>>>>>>>\n"; + HybridFactorGraph hfg; - DiscreteKey x(X(1), 2); + 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; + + hfg.add(CGMixtureFactor({X(1)}, { x }, dt)); hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8}))); - auto result = hfg.eliminateMultifrontal(); + auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)})); GTSAM_PRINT(*result); + GTSAM_PRINT(*result->marginalFactor(C(1))); } /* ************************************************************************* */