From ea4ebb6843daec9cd42bb7579e431d6686d1d8f7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 11 Mar 2022 15:51:34 -0500 Subject: [PATCH 01/70] Fix typo --- gtsam/inference/EliminateableFactorGraph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index c904d2f7f..900346f7f 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -204,7 +204,7 @@ namespace gtsam { OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to - * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) + * produce a Bayes tree and a remaining factor graph. This computes the factorization \f$ p(X) * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > From 0567303ed3334508cf5ab0c19903b22a5d3fd9e1 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 11 Mar 2022 15:52:28 -0500 Subject: [PATCH 02/70] Prototype a type-erased hybrid machinery --- gtsam/CMakeLists.txt | 1 + gtsam/hybrid/CGMixtureFactor.cpp | 5 + gtsam/hybrid/CGMixtureFactor.h | 23 +++++ gtsam/hybrid/CMakeLists.txt | 8 ++ gtsam/hybrid/HybridBayesNet.cpp | 16 ++++ gtsam/hybrid/HybridBayesNet.h | 43 +++++++++ gtsam/hybrid/HybridBayesTree.cpp | 38 ++++++++ gtsam/hybrid/HybridBayesTree.h | 77 +++++++++++++++ gtsam/hybrid/HybridConditional.cpp | 23 +++++ gtsam/hybrid/HybridConditional.h | 99 ++++++++++++++++++++ gtsam/hybrid/HybridDiscreteFactor.cpp | 19 ++++ gtsam/hybrid/HybridDiscreteFactor.h | 34 +++++++ gtsam/hybrid/HybridEliminationTree.cpp | 43 +++++++++ gtsam/hybrid/HybridEliminationTree.h | 62 ++++++++++++ gtsam/hybrid/HybridFactor.cpp | 18 ++++ gtsam/hybrid/HybridFactor.h | 83 ++++++++++++++++ gtsam/hybrid/HybridFactorGraph.cpp | 58 ++++++++++++ gtsam/hybrid/HybridFactorGraph.h | 83 ++++++++++++++++ gtsam/hybrid/HybridGaussianFactor.cpp | 23 +++++ gtsam/hybrid/HybridGaussianFactor.h | 39 ++++++++ gtsam/hybrid/HybridJunctionTree.cpp | 34 +++++++ gtsam/hybrid/HybridJunctionTree.h | 67 +++++++++++++ gtsam/hybrid/tests/CMakeLists.txt | 1 + gtsam/hybrid/tests/testHybridConditional.cpp | 73 +++++++++++++++ 24 files changed, 970 insertions(+) create mode 100644 gtsam/hybrid/CGMixtureFactor.cpp create mode 100644 gtsam/hybrid/CGMixtureFactor.h create mode 100644 gtsam/hybrid/CMakeLists.txt create mode 100644 gtsam/hybrid/HybridBayesNet.cpp create mode 100644 gtsam/hybrid/HybridBayesNet.h create mode 100644 gtsam/hybrid/HybridBayesTree.cpp create mode 100644 gtsam/hybrid/HybridBayesTree.h create mode 100644 gtsam/hybrid/HybridConditional.cpp create mode 100644 gtsam/hybrid/HybridConditional.h create mode 100644 gtsam/hybrid/HybridDiscreteFactor.cpp create mode 100644 gtsam/hybrid/HybridDiscreteFactor.h create mode 100644 gtsam/hybrid/HybridEliminationTree.cpp create mode 100644 gtsam/hybrid/HybridEliminationTree.h create mode 100644 gtsam/hybrid/HybridFactor.cpp create mode 100644 gtsam/hybrid/HybridFactor.h create mode 100644 gtsam/hybrid/HybridFactorGraph.cpp create mode 100644 gtsam/hybrid/HybridFactorGraph.h create mode 100644 gtsam/hybrid/HybridGaussianFactor.cpp create mode 100644 gtsam/hybrid/HybridGaussianFactor.h create mode 100644 gtsam/hybrid/HybridJunctionTree.cpp create mode 100644 gtsam/hybrid/HybridJunctionTree.h create mode 100644 gtsam/hybrid/tests/CMakeLists.txt create mode 100644 gtsam/hybrid/tests/testHybridConditional.cpp diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index a293c6ec2..845ac7cd0 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -10,6 +10,7 @@ set (gtsam_subdirs inference symbolic discrete + hybrid linear nonlinear sam diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp new file mode 100644 index 000000000..d789825f7 --- /dev/null +++ b/gtsam/hybrid/CGMixtureFactor.cpp @@ -0,0 +1,5 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include "CGMixtureFactor.h" diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h new file mode 100644 index 000000000..f2fa1e54a --- /dev/null +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -0,0 +1,23 @@ +/* ---------------------------------------------------------------------------- + * Copyright 2021 The Ambitious Folks of the MRG + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file CGMixtureFactor.h + * @brief Custom hybrid factor graph for discrete + continuous factors + * @author Kevin Doherty, kdoherty@mit.edu + * @author Varun Agrawal + * @author Fan Jiang + * @date December 2021 + */ + +#include + +namespace gtsam { + +class CGMixtureFactor : HybridFactor { + +}; + +} diff --git a/gtsam/hybrid/CMakeLists.txt b/gtsam/hybrid/CMakeLists.txt new file mode 100644 index 000000000..f1cfcd5c4 --- /dev/null +++ b/gtsam/hybrid/CMakeLists.txt @@ -0,0 +1,8 @@ +# Install headers +set(subdir hybrid) +file(GLOB hybrid_headers "*.h") +# FIXME: exclude headers +install(FILES ${hybrid_headers} DESTINATION include/gtsam/hybrid) + +# Add all tests +add_subdirectory(tests) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp new file mode 100644 index 000000000..1292711d8 --- /dev/null +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -0,0 +1,16 @@ +/* ---------------------------------------------------------------------------- + * 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 HybridBayesNet.cpp + * @brief A bayes net of Gaussian Conditionals indexed by discrete keys. + * @author Fan Jiang + * @date January 2022 + */ + +#include diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h new file mode 100644 index 000000000..53d10518f --- /dev/null +++ b/gtsam/hybrid/HybridBayesNet.h @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + * 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 HybridBayesNet.h + * @brief A bayes net of Gaussian Conditionals indexed by discrete keys. + * @author Varun Agrawal + * @author Fan Jiang + * @author Frank Dellaert + * @date December 2021 + */ + +#pragma once + +#include +#include + +#include // TODO! + +namespace gtsam { + +/** + * A hybrid Bayes net can have discrete conditionals, Gaussian mixtures, + * or pure Gaussian conditionals. + */ +class GTSAM_EXPORT HybridBayesNet : public BayesNet { + public: + using Base = BayesNet; + using This = HybridBayesNet; + using ConditionalType = HybridConditional; + using shared_ptr = boost::shared_ptr; + using sharedConditional = boost::shared_ptr; + + /** Construct empty bayes net */ + HybridBayesNet() : Base() {} +}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp new file mode 100644 index 000000000..72f3fd794 --- /dev/null +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -0,0 +1,38 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridBayesTree.cpp + * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief HybridBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +// Instantiate base class +template class BayesTreeCliqueBase; +template class BayesTree; + +/* ************************************************************************* */ +bool HybridBayesTree::equals(const This& other, double tol) const { + return Base::equals(other, tol); +} + +/* **************************************************************************/ +} // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h new file mode 100644 index 000000000..2ea40cecc --- /dev/null +++ b/gtsam/hybrid/HybridBayesTree.h @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridBayesTree.h + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridJunctionTree + * @brief HybridBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +// Forward declarations +class HybridConditional; +class VectorValues; + +/* ************************************************************************* */ +/** A clique in a HybridBayesTree */ +class GTSAM_EXPORT HybridBayesTreeClique + : public BayesTreeCliqueBase { + public: + typedef HybridBayesTreeClique This; + typedef BayesTreeCliqueBase + Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + HybridBayesTreeClique() {} + virtual ~HybridBayesTreeClique() {} + HybridBayesTreeClique( + const boost::shared_ptr& conditional) + : Base(conditional) {} + +}; + +/* ************************************************************************* */ +/** A Bayes tree representing a Hybrid density */ +class GTSAM_EXPORT HybridBayesTree + : public BayesTree { + private: + typedef BayesTree Base; + + public: + typedef HybridBayesTree This; + typedef boost::shared_ptr shared_ptr; + + /// @name Standard interface + /// @{ + /** Default constructor, creates an empty Bayes tree */ + HybridBayesTree() = default; + + /** Check equality */ + bool equals(const This& other, double tol = 1e-9) const; + + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp new file mode 100644 index 000000000..e6b2eb47f --- /dev/null +++ b/gtsam/hybrid/HybridConditional.cpp @@ -0,0 +1,23 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include +#include + +namespace gtsam { +void HybridConditional::print(const std::string &s, + const KeyFormatter &formatter) const { + Conditional::print(s, formatter); +} + +bool HybridConditional::equals(const HybridFactor &other, + double tol) const { + return false; +} + +HybridConditional HybridConditional::operator*(const HybridConditional &other) const { + return {}; +} +} + diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h new file mode 100644 index 000000000..d6dd8250f --- /dev/null +++ b/gtsam/hybrid/HybridConditional.h @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridConditional.h + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#pragma once + +#include +#include +#include + + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Hybrid Conditional Density + * + * As a type-erased variant of: + * - DiscreteConditional + * - GaussianMixture + * - GaussianConditional + */ +class GTSAM_EXPORT HybridConditional +: public HybridFactor, +public Conditional { +public: +// typedefs needed to play nice with gtsam +typedef HybridConditional This; ///< Typedef to this class +typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class +typedef HybridFactor BaseFactor; ///< Typedef to our factor base class +typedef Conditional + BaseConditional; ///< Typedef to our conditional base class + +private: +// Type-erased pointer to the inner type +std::unique_ptr inner; + +public: +/// @name Standard Constructors +/// @{ + +/// Default constructor needed for serialization. +HybridConditional() = default; + +/** + * @brief Combine two conditionals, yielding a new conditional with the union + * of the frontal keys, ordered by gtsam::Key. + * + * The two conditionals must make a valid Bayes net fragment, i.e., + * the frontal variables cannot overlap, and must be acyclic: + * Example of correct use: + * P(A,B) = P(A|B) * P(B) + * P(A,B|C) = P(A|B) * P(B|C) + * P(A,B,C) = P(A,B|C) * P(C) + * Example of incorrect use: + * P(A|B) * P(A|C) = ? + * P(A|B) * P(B|A) = ? + * We check for overlapping frontals, but do *not* check for cyclic. + */ +HybridConditional operator*(const HybridConditional& other) const; + +/// @} +/// @name Testable +/// @{ + +/// GTSAM-style print +void print( + const std::string& s = "Hybrid Conditional: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; + +/// GTSAM-style equals +bool equals(const HybridFactor& other, double tol = 1e-9) const override; + +/// @} +}; +// DiscreteConditional + +// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp new file mode 100644 index 000000000..fded0a2df --- /dev/null +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -0,0 +1,19 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include + +#include + +namespace gtsam { + +HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) { + inner = other; +} + +HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : inner(boost::make_shared(std::move(dtf))) { + +}; + +} \ No newline at end of file diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h new file mode 100644 index 000000000..4b1c00672 --- /dev/null +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -0,0 +1,34 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridDiscreteFactor.h + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#include +#include +#include + +namespace gtsam { + +class HybridDiscreteFactor : public HybridFactor { + public: + DiscreteFactor::shared_ptr inner; + + // Implicit conversion from a shared ptr of GF + HybridDiscreteFactor(DiscreteFactor::shared_ptr other); + + // Forwarding constructor from concrete JacobianFactor + HybridDiscreteFactor(DecisionTreeFactor &&dtf); +}; +} diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp new file mode 100644 index 000000000..ff106095a --- /dev/null +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridEliminationTree.cpp + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#include +#include + +namespace gtsam { + +// Instantiate base class +template class EliminationTree; + +/* ************************************************************************* */ +HybridEliminationTree::HybridEliminationTree( + const HybridFactorGraph& factorGraph, const VariableIndex& structure, + const Ordering& order) : + Base(factorGraph, structure, order) {} + +/* ************************************************************************* */ +HybridEliminationTree::HybridEliminationTree( + const HybridFactorGraph& factorGraph, const Ordering& order) : + Base(factorGraph, order) {} + +/* ************************************************************************* */ +bool HybridEliminationTree::equals(const This& other, double tol) const +{ + return Base::equals(other, tol); +} + +} diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h new file mode 100644 index 000000000..e218ce9f6 --- /dev/null +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridEliminationTree.h + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +class GTSAM_EXPORT HybridEliminationTree : + public EliminationTree +{ + public: + typedef EliminationTree Base; ///< Base class + typedef HybridEliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + HybridEliminationTree(const HybridFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order); + + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + HybridEliminationTree(const HybridFactorGraph& factorGraph, + const Ordering& order); + + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; + + private: + + friend class ::EliminationTreeTester; + +}; + +} diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp new file mode 100644 index 000000000..907350e83 --- /dev/null +++ b/gtsam/hybrid/HybridFactor.cpp @@ -0,0 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridFactor.cpp + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#include diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h new file mode 100644 index 000000000..5af1a23a9 --- /dev/null +++ b/gtsam/hybrid/HybridFactor.h @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridFactor.h + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#pragma once + +#include +#include +#include + +#include +namespace gtsam { + +/** + * Base class for hybrid probabilistic factors + */ +class GTSAM_EXPORT HybridFactor: public Factor { + +public: + +// typedefs needed to play nice with gtsam +typedef HybridFactor This; ///< This class +typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class +typedef Factor Base; ///< Our base class + +using Values = Values; ///< backwards compatibility + +public: + +/// @name Standard Constructors +/// @{ + +/** Default constructor creates empty factor */ +HybridFactor() {} + +/** Construct from container of keys. This constructor is used internally from derived factor + * constructors, either from a container of keys or from a boost::assign::list_of. */ +template +HybridFactor(const CONTAINER& keys) : Base(keys) {} + +/// Virtual destructor +virtual ~HybridFactor() { +} + +/// @} +/// @name Testable +/// @{ + +/// equals +virtual bool equals(const HybridFactor& lf, double tol = 1e-9) const = 0; + +/// print +void print( + const std::string& s = "HybridFactor\n", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { +Base::print(s, formatter); +} + +/// @} +/// @name Standard Interface +/// @{ + +/// @} +}; +// HybridFactor + +// traits +template<> struct traits : public Testable {}; + +}// namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp new file mode 100644 index 000000000..686fddc51 --- /dev/null +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -0,0 +1,58 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include +#include +#include + +#include + +#include + +namespace gtsam { + +template class EliminateableFactorGraph; + +/* ************************************************************************ */ +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; + // 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. + + // 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? + + // PRODUCT: multiply all factors + gttic(product); + HybridGaussianFactor product(JacobianFactor(0, I_3x3, Z_3x1)); +// for (auto&& factor : factors) product = (*factor) * product; + gttoc(product); + + // sum out frontals, this is the factor on the separator + gttic(sum); +// HybridFactor::shared_ptr sum = product.sum(frontalKeys); + 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()); + + // now divide product/sum to get conditional + gttic(divide); +// auto conditional = +// boost::make_shared(product, *sum, orderedKeys); + gttoc(divide); + +// return std::make_pair(conditional, sum); + return std::make_pair(boost::make_shared(), boost::make_shared(product)); +} + +} diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h new file mode 100644 index 000000000..fb0de644d --- /dev/null +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridFactorGraph.h + * @brief Hybrid factor graph that uses type erasure + * @author Fan Jiang + * @date Mar 11, 2022 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +// Forward declarations +class HybridFactorGraph; +class HybridConditional; +class HybridBayesNet; +class HybridEliminationTree; +class HybridBayesTree; +class HybridJunctionTree; + +/** Main elimination function for HybridFactorGraph */ +GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> +EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); + +/* ************************************************************************* */ +template<> struct EliminationTraits +{ + typedef HybridFactor FactorType; ///< Type of factors in factor graph + typedef HybridFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) + typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination + typedef HybridBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination + typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree + typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree + typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree + /// The default dense elimination function + static std::pair, boost::shared_ptr > + DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { + return EliminateHybrid(factors, keys); } +}; + + +class HybridFactorGraph : public FactorGraph, public EliminateableFactorGraph { + public: + using Base = FactorGraph; + using This = HybridFactorGraph; ///< this class + using BaseEliminateable = + EliminateableFactorGraph; ///< for elimination + using shared_ptr = boost::shared_ptr; ///< shared_ptr to This + + using Values = gtsam::Values; ///< backwards compatibility + using Indices = KeyVector; ///> map from keys to values + + public: + HybridFactorGraph() = default; + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} + + /** Implicit copy/downcast constructor to override explicit template container + * constructor */ + template + HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + +}; + +} + diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp new file mode 100644 index 000000000..1910c3307 --- /dev/null +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -0,0 +1,23 @@ +// +// Created by Fan Jiang on 3/11/22. +// + +#include + +#include + +namespace gtsam { + +HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()){ + inner = other; +} + +HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner(boost::make_shared(std::move(jf))) { + +} + +bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { + return false; +}; + +} \ No newline at end of file diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h new file mode 100644 index 000000000..e40682dc1 --- /dev/null +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridGaussianFactor.h + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#include +#include +#include + +namespace gtsam { + +class HybridGaussianFactor : public HybridFactor { + public: + using Base = HybridFactor; + + GaussianFactor::shared_ptr inner; + + // Implicit conversion from a shared ptr of GF + HybridGaussianFactor(GaussianFactor::shared_ptr other); + + // Forwarding constructor from concrete JacobianFactor + HybridGaussianFactor(JacobianFactor &&jf); + + public: + virtual bool equals(const HybridFactor& lf, double tol) const override; +}; +} diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp new file mode 100644 index 000000000..eabfc2e7c --- /dev/null +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -0,0 +1,34 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridJunctionTree.cpp + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +namespace gtsam { + +// Instantiate base classes +template class EliminatableClusterTree; +template class JunctionTree; + +/* ************************************************************************* */ +HybridJunctionTree::HybridJunctionTree( + const HybridEliminationTree& eliminationTree) : + Base(eliminationTree) {} + +} diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h new file mode 100644 index 000000000..1901e7007 --- /dev/null +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridJunctionTree.h + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +// Forward declarations +class HybridEliminationTree; + +/** + * An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, + * with the additional property that it represents the clique tree associated with a Bayes net. + * + * In GTSAM a junction tree is an intermediate data structure in multifrontal + * variable elimination. Each node is a cluster of factors, along with a + * clique of variables that are eliminated all at once. In detail, every node k represents + * a clique (maximal fully connected subset) of an associated chordal graph, such as a + * chordal Bayes net resulting from elimination. + * + * The difference with the BayesTree is that a JunctionTree stores factors, whereas a + * BayesTree stores conditionals, that are the product of eliminating the factors in the + * corresponding JunctionTree cliques. + * + * The tree structure and elimination method are exactly analogous to the EliminationTree, + * except that in the JunctionTree, at each node multiple variables are eliminated at a time. + * + * \addtogroup Multifrontal + * \nosubgrouping + */ +class GTSAM_EXPORT HybridJunctionTree : + public JunctionTree { + public: + typedef JunctionTree Base; ///< Base class + typedef HybridJunctionTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** + * Build the elimination tree of a factor graph using precomputed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + HybridJunctionTree(const HybridEliminationTree& eliminationTree); +}; + +} diff --git a/gtsam/hybrid/tests/CMakeLists.txt b/gtsam/hybrid/tests/CMakeLists.txt new file mode 100644 index 000000000..b52e18586 --- /dev/null +++ b/gtsam/hybrid/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam") \ No newline at end of file diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp new file mode 100644 index 000000000..295d6011d --- /dev/null +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 testHybridConditional.cpp + * @date Mar 11, 2022 + * @author Fan Jiang + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +using namespace boost::assign; + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST_UNSAFE(HybridFactorGraph, test) { + HybridConditional test; + GTSAM_PRINT(test); + + HybridFactorGraph hfg; + + hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); + GTSAM_PRINT(hfg); +} + +TEST_UNSAFE(HybridFactorGraph, eliminate) { + HybridFactorGraph hfg; + + hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); + + auto result = hfg.eliminatePartialSequential({0}); + + GTSAM_PRINT(*result.first); +} + +TEST(HybridFactorGraph, eliminateMultifrontal) { + HybridFactorGraph hfg; + + hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); + + auto result = hfg.eliminatePartialMultifrontal({0}); + + GTSAM_PRINT(*result.first); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From efa37ff315fb0ad9f837e2bb8a589afcd2cb9256 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 11 Mar 2022 19:20:39 -0500 Subject: [PATCH 03/70] 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); } /* ************************************************************************* */ From 3aac52c3d3b6ec692dbc7f0d7874a0cd335814a1 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 12 Mar 2022 10:49:11 -0500 Subject: [PATCH 04/70] Fix compilation error --- gtsam/hybrid/HybridFactor.h | 49 +++++++++++++++--------------- gtsam/hybrid/HybridFactorGraph.cpp | 3 ++ 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 5af1a23a9..eb28bc168 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -27,47 +27,45 @@ namespace gtsam { /** * Base class for hybrid probabilistic factors */ -class GTSAM_EXPORT HybridFactor: public Factor { +class GTSAM_EXPORT HybridFactor : public Factor { public: -// typedefs needed to play nice with gtsam -typedef HybridFactor This; ///< This class -typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class -typedef Factor Base; ///< Our base class - -using Values = Values; ///< backwards compatibility + // typedefs needed to play nice with gtsam + typedef HybridFactor This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Factor Base; ///< Our base class public: /// @name Standard Constructors /// @{ -/** Default constructor creates empty factor */ -HybridFactor() {} + /** Default constructor creates empty factor */ + HybridFactor() {} -/** Construct from container of keys. This constructor is used internally from derived factor - * constructors, either from a container of keys or from a boost::assign::list_of. */ -template -HybridFactor(const CONTAINER& keys) : Base(keys) {} + /** Construct from container of keys. This constructor is used internally from derived factor + * constructors, either from a container of keys or from a boost::assign::list_of. */ + template + HybridFactor(const CONTAINER &keys) : Base(keys) {} -/// Virtual destructor -virtual ~HybridFactor() { -} + /// Virtual destructor + virtual ~HybridFactor() { + } /// @} /// @name Testable /// @{ -/// equals -virtual bool equals(const HybridFactor& lf, double tol = 1e-9) const = 0; + /// equals + virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0; -/// print -void print( - const std::string& s = "HybridFactor\n", - const KeyFormatter& formatter = DefaultKeyFormatter) const override { -Base::print(s, formatter); -} + /// print + void print( + const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } /// @} /// @name Standard Interface @@ -78,6 +76,7 @@ Base::print(s, formatter); // HybridFactor // traits -template<> struct traits : public Testable {}; +template<> +struct traits : public Testable {}; }// namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 0f9d8ddca..bd245b16a 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -29,6 +29,9 @@ EliminateHybrid(const HybridFactorGraph &factors, // 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? + // 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); KeySet allKeys; From 53551e051dedae2c245f47dbf69bf390844bb3ab Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 12 Mar 2022 11:51:48 -0500 Subject: [PATCH 05/70] Add shorthand for inserting raw JacobianFactor --- gtsam/hybrid/CGMixtureFactor.h | 22 +++++++++++-- gtsam/hybrid/HybridDiscreteFactor.cpp | 2 +- gtsam/hybrid/HybridDiscreteFactor.h | 2 ++ gtsam/hybrid/HybridFactor.h | 34 ++++++++++++++++++-- gtsam/hybrid/HybridFactorGraph.cpp | 3 ++ gtsam/hybrid/HybridFactorGraph.h | 6 ++++ gtsam/hybrid/HybridGaussianFactor.h | 2 ++ gtsam/hybrid/tests/testHybridConditional.cpp | 12 ++++--- 8 files changed, 72 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h index f2fa1e54a..b46146eb3 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -5,19 +5,35 @@ /** * @file CGMixtureFactor.h - * @brief Custom hybrid factor graph for discrete + continuous factors - * @author Kevin Doherty, kdoherty@mit.edu + * @brief A set of Gaussian factors indexed by a set of discrete keys. * @author Varun Agrawal * @author Fan Jiang + * @author Frank Dellaert * @date December 2021 */ #include +#include +#include +#include namespace gtsam { -class CGMixtureFactor : HybridFactor { +class CGMixtureFactor : public HybridFactor { +public: + using Base = HybridFactor; + using This = CGMixtureFactor; + using shared_ptr = boost::shared_ptr; + using Factors = DecisionTree; + + Factors factors_; + + CGMixtureFactor() = default; + + CGMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) : Base(continuousKeys, discreteKeys) { + + } }; } diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 0bd2c9a64..13766933b 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -14,7 +14,7 @@ HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) } HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) - : Base(dtf.keys()), + : Base(dtf.discreteKeys()), inner(boost::make_shared(std::move(dtf))) { } diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index cfb94bc69..0395c9512 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -24,6 +24,8 @@ namespace gtsam { class HybridDiscreteFactor : public HybridFactor { public: using Base = HybridFactor; + using This = HybridDiscreteFactor; + using shared_ptr = boost::shared_ptr; DiscreteFactor::shared_ptr inner; diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index eb28bc168..64b49f605 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -36,6 +37,12 @@ public: typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Factor Base; ///< Our base class + bool isDiscrete_ = false; + bool isContinuous_ = false; + bool isHybrid_ = false; + + DiscreteKeys discreteKeys_; + public: /// @name Standard Constructors @@ -46,8 +53,25 @@ public: /** Construct from container of keys. This constructor is used internally from derived factor * constructors, either from a container of keys or from a boost::assign::list_of. */ - template - HybridFactor(const CONTAINER &keys) : Base(keys) {} +// template +// HybridFactor(const CONTAINER &keys) : Base(keys) {} + + HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {} + + static KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) { + KeyVector allKeys; + std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys)); + std::transform(discreteKeys.begin(), + discreteKeys.end(), + std::back_inserter(allKeys), + [](const DiscreteKey &k) { return k.first; }); + return allKeys; + } + + HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) : Base( + CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true) {} + + HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true) {} /// Virtual destructor virtual ~HybridFactor() { @@ -64,7 +88,11 @@ public: void print( const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override { - Base::print(s, formatter); + std::cout << s; + if (isContinuous_) std::cout << "Cont. "; + if (isDiscrete_) std::cout << "Disc. "; + if (isHybrid_) std::cout << "Hybr. "; + this->printKeys("", formatter); } /// @} diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index bd245b16a..080efc0e9 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -75,4 +75,7 @@ EliminateHybrid(const HybridFactorGraph &factors, boost::make_shared(std::move(sum))); } +void HybridFactorGraph::add(JacobianFactor &&factor) { + FactorGraph::add(boost::make_shared(std::move(factor))); +} } diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index fb0de644d..13bbd005d 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -33,6 +33,8 @@ class HybridEliminationTree; class HybridBayesTree; class HybridJunctionTree; +class JacobianFactor; + /** Main elimination function for HybridFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); @@ -77,6 +79,10 @@ class HybridFactorGraph : public FactorGraph, public Eliminateable template HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + using FactorGraph::add; + + /// Add a factor directly using a shared_ptr. + void add(JacobianFactor &&factor); }; } diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index e40682dc1..d8a97ba30 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -24,6 +24,8 @@ namespace gtsam { class HybridGaussianFactor : public HybridFactor { public: using Base = HybridFactor; + using This = HybridGaussianFactor; + using shared_ptr = boost::shared_ptr; GaussianFactor::shared_ptr inner; diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 6eef100c1..a56bcadf0 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -26,6 +26,8 @@ #include #include +#include + #include #include @@ -34,6 +36,8 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using gtsam::symbol_shorthand::X; + /* ************************************************************************* */ TEST_UNSAFE(HybridFactorGraph, test) { HybridConditional test; @@ -58,12 +62,12 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) { TEST(HybridFactorGraph, eliminateMultifrontal) { HybridFactorGraph hfg; - DiscreteKey X(1, 2); + DiscreteKey x(X(1), 2); - hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - hfg.add(HybridDiscreteFactor(DecisionTreeFactor(X, {2, 8}))); + hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); + hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8}))); - auto result = hfg.eliminatePartialMultifrontal({0}); + auto result = hfg.eliminatePartialMultifrontal({X(0)}); GTSAM_PRINT(*result.first); GTSAM_PRINT(*result.second); From 095f6ad7cc96d284fb663a4cfb62f0d5f48f9204 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 12 Mar 2022 12:42:58 -0500 Subject: [PATCH 06/70] Add full elimination --- gtsam/hybrid/HybridFactorGraph.h | 12 +++++++----- gtsam/hybrid/tests/testHybridConditional.cpp | 13 +++++++++++++ 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 13bbd005d..9a57d36e6 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -70,17 +70,19 @@ class HybridFactorGraph : public FactorGraph, public Eliminateable public: HybridFactorGraph() = default; - /** Construct from container of factors (shared_ptr or plain objects) */ - template - explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} +// /** Construct from container of factors (shared_ptr or plain objects) */ +// template +// explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} /** Implicit copy/downcast constructor to override explicit template container - * constructor */ + * constructor. In BayesTree this is used for: + * `cachedSeparatorMarginal_.reset(*separatorMarginal)` + * */ template HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} using FactorGraph::add; - + /// Add a factor directly using a shared_ptr. void add(JacobianFactor &&factor); }; diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index a56bcadf0..d5a21f34d 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -73,6 +73,19 @@ TEST(HybridFactorGraph, eliminateMultifrontal) { GTSAM_PRINT(*result.second); } +TEST(HybridFactorGraph, eliminateFullMultifrontal) { + HybridFactorGraph hfg; + + DiscreteKey x(X(1), 2); + + hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); + hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8}))); + + auto result = hfg.eliminateMultifrontal(); + + GTSAM_PRINT(*result); +} + /* ************************************************************************* */ int main() { TestResult tr; From 2bae2865d74efc139c8c77fcfe8a4d60bcb8eaab Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 12 Mar 2022 16:29:26 -0500 Subject: [PATCH 07/70] 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))); } /* ************************************************************************* */ From ee4f9d19f0ff7486076b647327d0c7c3d1274c6a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 13 Mar 2022 11:42:36 -0400 Subject: [PATCH 08/70] Added mixture factor functionality --- gtsam/hybrid/CGMixtureFactor.cpp | 17 ++++++++++ gtsam/hybrid/CGMixtureFactor.h | 3 ++ gtsam/hybrid/CLGaussianConditional.cpp | 31 +++++++++++++++++ gtsam/hybrid/CLGaussianConditional.h | 17 ++++++++++ gtsam/hybrid/HybridDiscreteFactor.cpp | 5 +++ gtsam/hybrid/HybridDiscreteFactor.h | 2 ++ gtsam/hybrid/HybridFactor.cpp | 35 ++++++++++++++++++++ gtsam/hybrid/HybridFactor.h | 25 +++++--------- gtsam/hybrid/HybridFactorGraph.cpp | 15 +++++---- gtsam/hybrid/HybridGaussianFactor.cpp | 4 +++ gtsam/hybrid/HybridGaussianFactor.h | 2 ++ gtsam/hybrid/tests/testHybridConditional.cpp | 21 ++++++++++-- 12 files changed, 151 insertions(+), 26 deletions(-) diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp index 2ddf80ec2..16ead783e 100644 --- a/gtsam/hybrid/CGMixtureFactor.cpp +++ b/gtsam/hybrid/CGMixtureFactor.cpp @@ -4,6 +4,9 @@ #include +#include +#include + namespace gtsam { CGMixtureFactor::CGMixtureFactor(const KeyVector &continuousKeys, @@ -14,4 +17,18 @@ bool CGMixtureFactor::equals(const HybridFactor &lf, double tol) const { return false; } +void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { + HybridFactor::print(s, formatter); + factors_.print( + "mixture = ", + [&](Key k) { + return formatter(k); + }, [&](const GaussianFactor::shared_ptr &gf) -> std::string { + RedirectCout rd; + if (!gf->empty()) gf->print("", formatter); + else return {"nullptr"}; + return rd.str(); + }); +} + } \ No newline at end of file diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h index 9c9e43ec2..7ff53b7ed 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -42,6 +42,9 @@ public: const Factors &factors); bool equals(const HybridFactor &lf, double tol = 1e-9) const override; + + void print(const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; } diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp index 09babc4e2..dbc9631c8 100644 --- a/gtsam/hybrid/CLGaussianConditional.cpp +++ b/gtsam/hybrid/CLGaussianConditional.cpp @@ -18,3 +18,34 @@ #include +#include + +namespace gtsam { + +CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals, + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const CLGaussianConditional::Conditionals &factors) + : BaseFactor( + CollectKeys(continuousFrontals, continuousParents), discreteParents), + BaseConditional(continuousFrontals.size()) { + +} + +bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const { + return false; +} + +void CLGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const { + std::cout << s << ": "; + if (isContinuous_) std::cout << "Cont. "; + if (isDiscrete_) std::cout << "Disc. "; + if (isHybrid_) std::cout << "Hybr. "; + BaseConditional::print("", formatter); + std::cout << "Discrete Keys = "; + for (auto &dk : discreteKeys_) { + std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; + } + std::cout << "\n"; +} +} \ No newline at end of file diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h index 03a2d99e7..14989df72 100644 --- a/gtsam/hybrid/CLGaussianConditional.h +++ b/gtsam/hybrid/CLGaussianConditional.h @@ -19,13 +19,30 @@ #include #include +#include +#include + namespace gtsam { class CLGaussianConditional : public HybridFactor, public Conditional { public: using This = CLGaussianConditional; using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; + using BaseConditional = Conditional; + using Conditionals = DecisionTree; +public: + + CLGaussianConditional(const KeyVector &continuousFrontals, + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const Conditionals &factors); + + bool equals(const HybridFactor &lf, double tol = 1e-9) const override; + + void print( + const std::string &s = "CLGaussianConditional\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; } \ No newline at end of file diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 13766933b..1758e9025 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -21,6 +21,11 @@ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { return false; +} + +void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const { + HybridFactor::print(s, formatter); + inner->print("inner: ", formatter); }; } \ No newline at end of file diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 0395c9512..9d574b736 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -37,5 +37,7 @@ class HybridDiscreteFactor : public HybridFactor { public: virtual bool equals(const HybridFactor& lf, double tol) const override; + + void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; } diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 907350e83..3095136a4 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -16,3 +16,38 @@ */ #include + +namespace gtsam { + +KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) { + KeyVector allKeys; + std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys)); + std::transform(discreteKeys.begin(), + discreteKeys.end(), + std::back_inserter(allKeys), + [](const DiscreteKey &k) { return k.first; }); + return allKeys; +} + +KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { + KeyVector allKeys; + std::copy(keys1.begin(), keys1.end(), std::back_inserter(allKeys)); + std::copy(keys2.begin(), keys2.end(), std::back_inserter(allKeys)); + return allKeys; +} + +HybridFactor::HybridFactor() = default; + +HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {} + +HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) + : Base( + CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true), discreteKeys_(discreteKeys) {} + +HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), + isDiscrete_(true), + discreteKeys_(discreteKeys) {} + +HybridFactor::~HybridFactor() = default; + +} \ No newline at end of file diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 64b49f605..619d16078 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -25,6 +25,9 @@ #include namespace gtsam { +KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); +KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2); + /** * Base class for hybrid probabilistic factors */ @@ -49,33 +52,21 @@ public: /// @{ /** Default constructor creates empty factor */ - HybridFactor() {} + HybridFactor(); /** Construct from container of keys. This constructor is used internally from derived factor * constructors, either from a container of keys or from a boost::assign::list_of. */ // template // HybridFactor(const CONTAINER &keys) : Base(keys) {} - HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {} + explicit HybridFactor(const KeyVector &keys); - static KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) { - KeyVector allKeys; - std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys)); - std::transform(discreteKeys.begin(), - discreteKeys.end(), - std::back_inserter(allKeys), - [](const DiscreteKey &k) { return k.first; }); - return allKeys; - } + HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); - HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) : Base( - CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true) {} - - HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true) {} + explicit HybridFactor(const DiscreteKeys &discreteKeys); /// Virtual destructor - virtual ~HybridFactor() { - } + virtual ~HybridFactor(); /// @} /// @name Testable diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index cd5bc651d..2dc54d75d 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -34,6 +34,7 @@ EliminateHybrid(const HybridFactorGraph &factors, // 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: "; frontalKeys.print(); @@ -43,6 +44,7 @@ EliminateHybrid(const HybridFactorGraph &factors, factor->print(); allKeys.insert(factor->begin(), factor->end()); } + for (auto &k : frontalKeys) { allKeys.erase(k); } @@ -51,13 +53,14 @@ EliminateHybrid(const HybridFactorGraph &factors, gttic(product); HybridConditional sum(allKeys.size(), Ordering(allKeys)); -// HybridDiscreteFactor product(DiscreteConditional()); -// for (auto&& factor : factors) product = (*factor) * product; + + // HybridDiscreteFactor product(DiscreteConditional()); + // for (auto&& factor : factors) product = (*factor) * product; gttoc(product); // sum out frontals, this is the factor on the separator gttic(sum); -// HybridFactor::shared_ptr sum = product.sum(frontalKeys); + // HybridFactor::shared_ptr sum = product.sum(frontalKeys); gttoc(sum); // Ordering keys for the conditional so that frontalKeys are really in front @@ -69,11 +72,11 @@ EliminateHybrid(const HybridFactorGraph &factors, // now divide product/sum to get conditional gttic(divide); -// auto conditional = -// boost::make_shared(product, *sum, orderedKeys); + // auto conditional = + // boost::make_shared(product, *sum, orderedKeys); gttoc(divide); -// return std::make_pair(conditional, sum); + // return std::make_pair(conditional, sum); 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 1e87cbbc3..faa4ba998 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -18,6 +18,10 @@ HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys() bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { return false; +} +void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { + HybridFactor::print(s, formatter); + inner->print("inner: ", formatter); }; } \ No newline at end of file diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 34a7c0004..8562075b4 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -37,5 +37,7 @@ class HybridGaussianFactor : public HybridFactor { public: virtual bool equals(const HybridFactor& lf, double tol) const override; + + void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; } diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 46ec40475..4611026b3 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -47,6 +48,13 @@ TEST_UNSAFE(HybridFactorGraph, creation) { HybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); + + CLGaussianConditional clgc( + {X(0)}, {X(1)}, + DiscreteKeys(DiscreteKey{C(0), 2}), + CLGaussianConditional::Conditionals() + ); + GTSAM_PRINT(clgc); } TEST_UNSAFE(HybridFactorGraph, eliminate) { @@ -84,12 +92,19 @@ TEST(HybridFactorGraph, eliminateFullMultifrontal) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - DecisionTree dt; + 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(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)})); + auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); GTSAM_PRINT(*result); GTSAM_PRINT(*result->marginalFactor(C(1))); From 5ea614a82a0528a78758d5d6782cfbb948a43179 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 13 Mar 2022 20:09:53 -0400 Subject: [PATCH 09/70] Added more mockups and color output of the elimination process --- gtsam/hybrid/CLGaussianConditional.cpp | 17 +++- gtsam/hybrid/CLGaussianConditional.h | 4 +- gtsam/hybrid/HybridConditional.h | 71 ++++++++------- gtsam/hybrid/HybridFactor.cpp | 7 ++ gtsam/hybrid/HybridFactor.h | 1 + gtsam/hybrid/HybridFactorGraph.cpp | 93 ++++++++++++++++---- gtsam/hybrid/tests/testHybridConditional.cpp | 15 +++- 7 files changed, 151 insertions(+), 57 deletions(-) diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp index dbc9631c8..37c82a0d5 100644 --- a/gtsam/hybrid/CLGaussianConditional.cpp +++ b/gtsam/hybrid/CLGaussianConditional.cpp @@ -18,17 +18,20 @@ #include +#include + #include +#include namespace gtsam { CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const CLGaussianConditional::Conditionals &factors) + const CLGaussianConditional::Conditionals &conditionals) : BaseFactor( CollectKeys(continuousFrontals, continuousParents), discreteParents), - BaseConditional(continuousFrontals.size()) { + BaseConditional(continuousFrontals.size()), conditionals_(conditionals) { } @@ -47,5 +50,15 @@ void CLGaussianConditional::print(const std::string &s, const KeyFormatter &form std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } std::cout << "\n"; + conditionals_.print( + "", + [&](Key k) { + return formatter(k); + }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { + RedirectCout rd; + if (!gf->empty()) gf->print("", formatter); + else return {"nullptr"}; + return rd.str(); + }); } } \ No newline at end of file diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h index 14989df72..0319c60a7 100644 --- a/gtsam/hybrid/CLGaussianConditional.h +++ b/gtsam/hybrid/CLGaussianConditional.h @@ -32,12 +32,14 @@ public: using Conditionals = DecisionTree; + Conditionals conditionals_; + public: CLGaussianConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const Conditionals &factors); + const Conditionals &conditionals); bool equals(const HybridFactor &lf, double tol = 1e-9) const override; diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 26083e13e..92856cae2 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -41,56 +41,59 @@ class GTSAM_EXPORT HybridConditional : public HybridFactor, public Conditional { public: -// typedefs needed to play nice with gtsam -typedef HybridConditional This; ///< Typedef to this class -typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class -typedef HybridFactor BaseFactor; ///< Typedef to our factor base class -typedef Conditional - BaseConditional; ///< Typedef to our conditional base class + // typedefs needed to play nice with gtsam + typedef HybridConditional This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef HybridFactor BaseFactor; ///< Typedef to our factor base class + typedef Conditional + BaseConditional; ///< Typedef to our conditional base class private: -// Type-erased pointer to the inner type -std::unique_ptr inner; + // Type-erased pointer to the inner type + std::unique_ptr inner; public: /// @name Standard Constructors /// @{ -/// Default constructor needed for serialization. -HybridConditional() = default; + /// Default constructor needed for serialization. + HybridConditional() = default; -HybridConditional(size_t nFrontals, const KeyVector& keys) : BaseFactor(keys), BaseConditional(nFrontals) { + HybridConditional(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys, + size_t nFrontals) + : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) { -} + } -/** - * @brief Combine two conditionals, yielding a new conditional with the union - * of the frontal keys, ordered by gtsam::Key. - * - * The two conditionals must make a valid Bayes net fragment, i.e., - * the frontal variables cannot overlap, and must be acyclic: - * Example of correct use: - * P(A,B) = P(A|B) * P(B) - * P(A,B|C) = P(A|B) * P(B|C) - * P(A,B,C) = P(A,B|C) * P(C) - * Example of incorrect use: - * P(A|B) * P(A|C) = ? - * P(A|B) * P(B|A) = ? - * We check for overlapping frontals, but do *not* check for cyclic. - */ -HybridConditional operator*(const HybridConditional& other) const; + /** + * @brief Combine two conditionals, yielding a new conditional with the union + * of the frontal keys, ordered by gtsam::Key. + * + * The two conditionals must make a valid Bayes net fragment, i.e., + * the frontal variables cannot overlap, and must be acyclic: + * Example of correct use: + * P(A,B) = P(A|B) * P(B) + * P(A,B|C) = P(A|B) * P(B|C) + * P(A,B,C) = P(A,B|C) * P(C) + * Example of incorrect use: + * P(A|B) * P(A|C) = ? + * P(A|B) * P(B|A) = ? + * We check for overlapping frontals, but do *not* check for cyclic. + */ + HybridConditional operator*(const HybridConditional& other) const; /// @} /// @name Testable /// @{ -/// GTSAM-style print -void print( - const std::string& s = "Hybrid Conditional: ", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; + /// GTSAM-style print + void print( + const std::string& s = "Hybrid Conditional: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; -/// GTSAM-style equals -bool equals(const HybridFactor& other, double tol = 1e-9) const override; + /// GTSAM-style equals + bool equals(const HybridFactor& other, double tol = 1e-9) const override; /// @} }; diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 3095136a4..171ea790a 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -36,6 +36,13 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { return allKeys; } +DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) { + DiscreteKeys allKeys; + std::copy(key1.begin(), key1.end(), std::back_inserter(allKeys)); + std::copy(key2.begin(), key2.end(), std::back_inserter(allKeys)); + return allKeys; +} + HybridFactor::HybridFactor() = default; HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {} diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 619d16078..dd925fee2 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -27,6 +27,7 @@ namespace gtsam { KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2); +DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2); /** * Base class for hybrid probabilistic factors diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 2dc54d75d..177513f60 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -5,17 +5,26 @@ #include #include #include +#include #include #include #include +#include + namespace gtsam { template class EliminateableFactorGraph; +static std::string BLACK_BOLD = "\033[1;30m"; +static std::string RED_BOLD = "\033[1;31m"; +static std::string GREEN = "\033[0;32m"; +static std::string GREEN_BOLD = "\033[1;32m"; +static std::string RESET = "\033[0m"; + /* ************************************************************************ */ std::pair // EliminateHybrid(const HybridFactorGraph &factors, @@ -33,26 +42,66 @@ EliminateHybrid(const HybridFactorGraph &factors, // so that the discrete parts will be guaranteed to be eliminated last! // PREPROCESS: Identify the nature of the current elimination - KeySet allKeys; + std::unordered_map discreteCardinalities; + std::set discreteSeparator; + std::set discreteFrontals; + + KeySet separatorKeys; + KeySet allContinuousKeys; + KeySet continuousFrontals; + KeySet continuousSeparator; // TODO: we do a mock by just doing the correct key thing - std::cout << "Begin Eliminate: "; + + // This initializes separatorKeys and discreteCardinalities + for (auto &&factor : factors) { + std::cout << ">>> Adding factor: " << GREEN; + factor->print(); + std::cout << RESET; + separatorKeys.insert(factor->begin(), factor->end()); + if (!factor->isContinuous_) { + for (auto &k : factor->discreteKeys_) { + discreteCardinalities[k.first] = k; + } + } + } + + // remove frontals from separator + for (auto &k : frontalKeys) { + separatorKeys.erase(k); + } + + // Fill in discrete frontals and continuous frontals for the end result + for (auto &k : frontalKeys) { + if (discreteCardinalities.find(k) != discreteCardinalities.end()) { + discreteFrontals.insert(discreteCardinalities.at(k)); + } else { + continuousFrontals.insert(k); + } + } + + // Fill in discrete frontals and continuous frontals for the end result + for (auto &k : separatorKeys) { + if (discreteCardinalities.find(k) != discreteCardinalities.end()) { + discreteSeparator.insert(discreteCardinalities.at(k)); + } else { + continuousSeparator.insert(k); + } + } + + std::cout << RED_BOLD << "Begin Eliminate: " << RESET; frontalKeys.print(); - for (auto &&factor : factors) { - std::cout << ">>> Adding factor: "; - factor->print(); - allKeys.insert(factor->begin(), factor->end()); - } - - for (auto &k : frontalKeys) { - allKeys.erase(k); - } - + std::cout << RED_BOLD << "Discrete Keys: " << RESET; + for (auto &&key : discreteCardinalities) + std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.second.first) % key.second.second; + std::cout << "\n" << RESET; // PRODUCT: multiply all factors gttic(product); - HybridConditional sum(allKeys.size(), Ordering(allKeys)); + HybridConditional sum(KeyVector(continuousSeparator.begin(), continuousSeparator.end()), + DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()), + separatorKeys.size()); // HybridDiscreteFactor product(DiscreteConditional()); // for (auto&& factor : factors) product = (*factor) * product; @@ -76,10 +125,22 @@ EliminateHybrid(const HybridFactorGraph &factors, // boost::make_shared(product, *sum, orderedKeys); gttoc(divide); + auto conditional = boost::make_shared( + CollectKeys({continuousFrontals.begin(), continuousFrontals.end()}, + {continuousSeparator.begin(), continuousSeparator.end()}), + CollectDiscreteKeys({discreteFrontals.begin(), discreteFrontals.end()}, + {discreteSeparator.begin(), discreteSeparator.end()}), + continuousFrontals.size() + discreteFrontals.size()); + std::cout << GREEN_BOLD << "[Conditional]\n" << RESET; + conditional->print(); + std::cout << GREEN_BOLD << "[Separator]\n" << RESET; + sum.print(); + std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; + // return std::make_pair(conditional, sum); - return std::make_pair(boost::make_shared(frontalKeys.size(), - orderedKeys), - boost::make_shared(std::move(sum))); + return std::make_pair( + conditional, + boost::make_shared(std::move(sum))); } void HybridFactorGraph::add(JacobianFactor &&factor) { diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 4611026b3..7292cb3f5 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -42,7 +42,7 @@ using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::C; /* ************************************************************************* */ -TEST_UNSAFE(HybridFactorGraph, creation) { +TEST(HybridFactorGraph, creation) { HybridConditional test; HybridFactorGraph hfg; @@ -52,12 +52,19 @@ TEST_UNSAFE(HybridFactorGraph, creation) { CLGaussianConditional clgc( {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - CLGaussianConditional::Conditionals() + CLGaussianConditional::Conditionals( + C(0), + boost::make_shared( + X(0), Z_3x1, I_3x3, X(1), I_3x3), + boost::make_shared( + X(0), Vector3::Ones(), + I_3x3, X(1), + I_3x3)) ); GTSAM_PRINT(clgc); } -TEST_UNSAFE(HybridFactorGraph, eliminate) { +TEST_DISABLED(HybridFactorGraph, eliminate) { HybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -67,7 +74,7 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) { EXPECT_LONGS_EQUAL(result.first->size(), 1); } -TEST(HybridFactorGraph, eliminateMultifrontal) { +TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { HybridFactorGraph hfg; DiscreteKey x(X(1), 2); From 0aeb59695e542bc1513ba63f900afd601dd86525 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 13 Mar 2022 23:15:20 -0400 Subject: [PATCH 10/70] add more comments --- gtsam/hybrid/HybridConditional.cpp | 21 +++++++++++-- gtsam/hybrid/HybridFactorGraph.cpp | 7 ++++- gtsam/hybrid/tests/testHybridConditional.cpp | 31 +++++++++++++++++++- 3 files changed, 55 insertions(+), 4 deletions(-) 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; From fe5dde7e27e080f5a9abcf4692bdf16dc3164fce Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 14 Mar 2022 19:22:20 -0400 Subject: [PATCH 11/70] 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 #include #include @@ -12,6 +13,7 @@ #include +#include #include 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 #include #include @@ -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. + */ } /* ************************************************************************* */ From f237438bf30da2dab660761eb2523570c90121b5 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 15 Mar 2022 12:50:31 -0400 Subject: [PATCH 12/70] Address comments --- gtsam/hybrid/CGMixtureFactor.cpp | 22 ++++- gtsam/hybrid/CGMixtureFactor.h | 16 ++-- gtsam/hybrid/CLGaussianConditional.cpp | 40 +++++---- gtsam/hybrid/CLGaussianConditional.h | 18 ++-- gtsam/hybrid/HybridBayesTree.h | 15 ++-- gtsam/hybrid/HybridConditional.cpp | 33 ++++++-- gtsam/hybrid/HybridConditional.h | 36 ++++---- gtsam/hybrid/HybridDiscreteFactor.cpp | 29 +++++-- gtsam/hybrid/HybridDiscreteFactor.h | 10 ++- gtsam/hybrid/HybridEliminationTree.h | 45 +++++----- gtsam/hybrid/HybridFactor.cpp | 32 ++++--- gtsam/hybrid/HybridFactor.h | 61 +++++++------- gtsam/hybrid/HybridFactorGraph.cpp | 72 +++++++++------- gtsam/hybrid/HybridFactorGraph.h | 54 ++++++------ gtsam/hybrid/HybridGaussianFactor.cpp | 35 +++++--- gtsam/hybrid/HybridGaussianFactor.h | 10 ++- gtsam/hybrid/HybridJunctionTree.cpp | 10 +-- gtsam/hybrid/tests/testHybridConditional.cpp | 87 +++++++++----------- 18 files changed, 346 insertions(+), 279 deletions(-) diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp index 16ead783e..705160273 100644 --- a/gtsam/hybrid/CGMixtureFactor.cpp +++ b/gtsam/hybrid/CGMixtureFactor.cpp @@ -1,6 +1,22 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * 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.cpp + * @brief A set of Gaussian factors indexed by a set of discrete keys. + * @author Fan Jiang + * @author Varun Agrawal + * @author Frank Dellaert + * @date Mar 12, 2022 + */ #include diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h index 7ff53b7ed..0528d5401 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -18,15 +18,15 @@ * @date Mar 12, 2022 */ -#include -#include #include #include +#include +#include namespace gtsam { class CGMixtureFactor : public HybridFactor { -public: + public: using Base = HybridFactor; using This = CGMixtureFactor; using shared_ptr = boost::shared_ptr; @@ -38,13 +38,13 @@ public: CGMixtureFactor() = default; CGMixtureFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const Factors &factors); + const DiscreteKeys &discreteKeys, const Factors &factors); bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - void print(const std::string &s = "HybridFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print( + const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp index 37c82a0d5..30531c023 100644 --- a/gtsam/hybrid/CLGaussianConditional.cpp +++ b/gtsam/hybrid/CLGaussianConditional.cpp @@ -16,30 +16,28 @@ * @date Mar 12, 2022 */ -#include - #include - -#include #include +#include +#include namespace gtsam { -CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const CLGaussianConditional::Conditionals &conditionals) - : BaseFactor( - CollectKeys(continuousFrontals, continuousParents), discreteParents), - BaseConditional(continuousFrontals.size()), conditionals_(conditionals) { - -} +CLGaussianConditional::CLGaussianConditional( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const CLGaussianConditional::Conditionals &conditionals) + : BaseFactor(CollectKeys(continuousFrontals, continuousParents), + discreteParents), + BaseConditional(continuousFrontals.size()), + conditionals_(conditionals) {} bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const { return false; } -void CLGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const { +void CLGaussianConditional::print(const std::string &s, + const KeyFormatter &formatter) const { std::cout << s << ": "; if (isContinuous_) std::cout << "Cont. "; if (isDiscrete_) std::cout << "Disc. "; @@ -51,14 +49,14 @@ void CLGaussianConditional::print(const std::string &s, const KeyFormatter &form } std::cout << "\n"; conditionals_.print( - "", - [&](Key k) { - return formatter(k); - }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { + "", [&](Key k) { return formatter(k); }, + [&](const GaussianConditional::shared_ptr &gf) -> std::string { RedirectCout rd; - if (!gf->empty()) gf->print("", formatter); - else return {"nullptr"}; + if (!gf->empty()) + gf->print("", formatter); + else + return {"nullptr"}; return rd.str(); }); } -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h index 0319c60a7..0429f4835 100644 --- a/gtsam/hybrid/CLGaussianConditional.h +++ b/gtsam/hybrid/CLGaussianConditional.h @@ -16,15 +16,16 @@ * @date Mar 12, 2022 */ -#include -#include - -#include #include +#include +#include +#include namespace gtsam { -class CLGaussianConditional : public HybridFactor, public Conditional { -public: +class CLGaussianConditional + : public HybridFactor, + public Conditional { + public: using This = CLGaussianConditional; using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; @@ -34,8 +35,7 @@ public: Conditionals conditionals_; -public: - + public: CLGaussianConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, @@ -47,4 +47,4 @@ public: const std::string &s = "CLGaussianConditional\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 06df66112..74b967fc8 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -11,8 +11,7 @@ /** * @file HybridBayesTree.h - * @brief Hybrid Bayes Tree, the result of eliminating a - * HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ @@ -22,8 +21,8 @@ #include #include #include -#include #include +#include #include @@ -39,22 +38,18 @@ class GTSAM_EXPORT HybridBayesTreeClique : public BayesTreeCliqueBase { public: typedef HybridBayesTreeClique This; - typedef BayesTreeCliqueBase - Base; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; HybridBayesTreeClique() {} virtual ~HybridBayesTreeClique() {} - HybridBayesTreeClique( - const boost::shared_ptr& conditional) + HybridBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} - }; /* ************************************************************************* */ /** A Bayes tree representing a Hybrid density */ -class GTSAM_EXPORT HybridBayesTree - : public BayesTree { +class GTSAM_EXPORT HybridBayesTree : public BayesTree { private: typedef BayesTree Base; diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index b3009d5b9..5cb98d921 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -1,6 +1,19 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * 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 HybridConditional.cpp + * @date Mar 11, 2022 + * @author Fan Jiang + */ #include #include @@ -14,7 +27,10 @@ void HybridConditional::print(const std::string &s, const size_t contN = N - discreteKeys_.size(); while (index < N) { if (index > 0) { - if (index == nrFrontals_) std::cout << " | "; else std::cout << ", "; + if (index == nrFrontals_) + std::cout << " | "; + else + std::cout << ", "; } if (index < contN) { std::cout << formatter(keys()[index]); @@ -28,13 +44,12 @@ void HybridConditional::print(const std::string &s, if (inner) inner->print("", formatter); } -bool HybridConditional::equals(const HybridFactor &other, - double tol) const { +bool HybridConditional::equals(const HybridFactor &other, double tol) const { return false; } -HybridConditional HybridConditional::operator*(const HybridConditional &other) const { +HybridConditional HybridConditional::operator*( + const HybridConditional &other) const { return {}; } -} - +} // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 92856cae2..89071092f 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -17,11 +17,10 @@ #pragma once -#include #include +#include #include - #include #include #include @@ -38,33 +37,30 @@ namespace gtsam { * - GaussianConditional */ class GTSAM_EXPORT HybridConditional -: public HybridFactor, -public Conditional { -public: + : public HybridFactor, + public Conditional { + public: // typedefs needed to play nice with gtsam - typedef HybridConditional This; ///< Typedef to this class + typedef HybridConditional This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class typedef HybridFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional BaseConditional; ///< Typedef to our conditional base class -private: + private: // Type-erased pointer to the inner type std::unique_ptr inner; -public: -/// @name Standard Constructors -/// @{ + public: + /// @name Standard Constructors + /// @{ /// Default constructor needed for serialization. HybridConditional() = default; - HybridConditional(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - size_t nFrontals) - : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) { - - } + HybridConditional(const KeyVector& continuousKeys, + const DiscreteKeys& discreteKeys, size_t nFrontals) + : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {} /** * @brief Combine two conditionals, yielding a new conditional with the union @@ -83,9 +79,9 @@ public: */ HybridConditional operator*(const HybridConditional& other) const; -/// @} -/// @name Testable -/// @{ + /// @} + /// @name Testable + /// @{ /// GTSAM-style print void print( @@ -95,7 +91,7 @@ public: /// GTSAM-style equals bool equals(const HybridFactor& other, double tol = 1e-9) const override; -/// @} + /// @} }; // DiscreteConditional diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 1758e9025..96d3842b8 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -1,6 +1,20 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * 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 HybridDiscreteFactor.cpp + * @brief Wrapper for a discrete factor + * @date Mar 11, 2022 + * @author Fan Jiang + */ #include @@ -15,17 +29,16 @@ HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : Base(dtf.discreteKeys()), - inner(boost::make_shared(std::move(dtf))) { - -} + inner(boost::make_shared(std::move(dtf))) {} bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { return false; } -void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const { +void HybridDiscreteFactor::print(const std::string &s, + const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); inner->print("inner: ", formatter); }; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 9d574b736..09da9cf70 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -15,8 +15,8 @@ * @author Fan Jiang */ -#include #include +#include #include namespace gtsam { @@ -36,8 +36,10 @@ class HybridDiscreteFactor : public HybridFactor { HybridDiscreteFactor(DecisionTreeFactor &&dtf); public: - virtual bool equals(const HybridFactor& lf, double tol) const override; + virtual bool equals(const HybridFactor &lf, double tol) const override; - void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print( + const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index e218ce9f6..b72bbcad9 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -23,40 +23,39 @@ namespace gtsam { -class GTSAM_EXPORT HybridEliminationTree : - public EliminationTree -{ +class GTSAM_EXPORT HybridEliminationTree + : public EliminationTree { public: - typedef EliminationTree Base; ///< Base class - typedef HybridEliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef EliminationTree + Base; ///< Base class + typedef HybridEliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** - * Build the elimination tree of a factor graph using pre-computed column structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ + * Build the elimination tree of a factor graph using pre-computed column + * structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is + * not precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ HybridEliminationTree(const HybridFactorGraph& factorGraph, - const VariableIndex& structure, const Ordering& order); + const VariableIndex& structure, const Ordering& order); - /** Build the elimination tree of a factor graph. Note that this has to compute the column - * structure as a VariableIndex, so if you already have this precomputed, use the other - * constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ + /** Build the elimination tree of a factor graph. Note that this has to + * compute the column structure as a VariableIndex, so if you already have + * this precomputed, use the other constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ HybridEliminationTree(const HybridFactorGraph& factorGraph, - const Ordering& order); + const Ordering& order); /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; private: - friend class ::EliminationTreeTester; - }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 171ea790a..f16092eff 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -19,11 +19,12 @@ namespace gtsam { -KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) { +KeyVector CollectKeys(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys) { KeyVector allKeys; - std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys)); - std::transform(discreteKeys.begin(), - discreteKeys.end(), + std::copy(continuousKeys.begin(), continuousKeys.end(), + std::back_inserter(allKeys)); + std::transform(discreteKeys.begin(), discreteKeys.end(), std::back_inserter(allKeys), [](const DiscreteKey &k) { return k.first; }); return allKeys; @@ -36,7 +37,8 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { return allKeys; } -DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) { +DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, + const DiscreteKeys &key2) { DiscreteKeys allKeys; std::copy(key1.begin(), key1.end(), std::back_inserter(allKeys)); std::copy(key2.begin(), key2.end(), std::back_inserter(allKeys)); @@ -45,16 +47,20 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &k HybridFactor::HybridFactor() = default; -HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {} +HybridFactor::HybridFactor(const KeyVector &keys) + : Base(keys), isContinuous_(true) {} -HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) - : Base( - CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true), discreteKeys_(discreteKeys) {} +HybridFactor::HybridFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys) + : Base(CollectKeys(continuousKeys, discreteKeys)), + isHybrid_(true), + discreteKeys_(discreteKeys) {} -HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), - isDiscrete_(true), - discreteKeys_(discreteKeys) {} +HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) + : Base(CollectKeys({}, discreteKeys)), + isDiscrete_(true), + discreteKeys_(discreteKeys) {} HybridFactor::~HybridFactor() = default; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index dd925fee2..cd562869e 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -17,29 +17,30 @@ #pragma once -#include -#include -#include #include +#include +#include +#include #include namespace gtsam { -KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); +KeyVector CollectKeys(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys); KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2); -DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2); +DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, + const DiscreteKeys &key2); /** * Base class for hybrid probabilistic factors */ class GTSAM_EXPORT HybridFactor : public Factor { - -public: - + public: // typedefs needed to play nice with gtsam - typedef HybridFactor This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - typedef Factor Base; ///< Our base class + typedef HybridFactor This; ///< This class + typedef boost::shared_ptr + shared_ptr; ///< shared_ptr to this class + typedef Factor Base; ///< Our base class bool isDiscrete_ = false; bool isContinuous_ = false; @@ -47,31 +48,33 @@ public: DiscreteKeys discreteKeys_; -public: - -/// @name Standard Constructors -/// @{ + public: + /// @name Standard Constructors + /// @{ /** Default constructor creates empty factor */ HybridFactor(); - /** Construct from container of keys. This constructor is used internally from derived factor - * constructors, either from a container of keys or from a boost::assign::list_of. */ -// template -// HybridFactor(const CONTAINER &keys) : Base(keys) {} + /** Construct from container of keys. This constructor is used internally + * from derived factor + * constructors, either from a container of keys or from a + * boost::assign::list_of. */ + // template + // HybridFactor(const CONTAINER &keys) : Base(keys) {} explicit HybridFactor(const KeyVector &keys); - HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); + HybridFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys); explicit HybridFactor(const DiscreteKeys &discreteKeys); /// Virtual destructor virtual ~HybridFactor(); -/// @} -/// @name Testable -/// @{ + /// @} + /// @name Testable + /// @{ /// equals virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0; @@ -87,16 +90,16 @@ public: this->printKeys("", formatter); } -/// @} -/// @name Standard Interface -/// @{ + /// @} + /// @name Standard Interface + /// @{ -/// @} + /// @} }; // HybridFactor // traits -template<> +template <> struct traits : public Testable {}; -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 03610166e..7d4daaceb 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -1,25 +1,37 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- -#include "gtsam/inference/Key.h" -#include -#include -#include -#include + * 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 HybridFactorGraph.cpp + * @brief Hybrid factor graph that uses type erasure + * @author Fan Jiang + * @date Mar 11, 2022 + */ -#include #include - +#include +#include +#include +#include +#include #include #include #include +#include "gtsam/inference/Key.h" + namespace gtsam { -template -class EliminateableFactorGraph; +template class EliminateableFactorGraph; static std::string BLACK_BOLD = "\033[1;30m"; static std::string RED_BOLD = "\033[1;31m"; @@ -29,8 +41,7 @@ static std::string RESET = "\033[0m"; /* ************************************************************************ */ 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 // a few cases: // continuous variable, we make a GM if there are hybrid factors; @@ -38,9 +49,10 @@ EliminateHybrid(const HybridFactorGraph &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) + // 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? @@ -102,7 +114,8 @@ EliminateHybrid(const HybridFactorGraph &factors, 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; + std::cout << boost::format(" (%1%,%2%),") % + DefaultKeyFormatter(key.first) % key.second; } else { std::cout << " " << DefaultKeyFormatter(f) << ","; } @@ -115,7 +128,8 @@ EliminateHybrid(const HybridFactorGraph &factors, 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; + std::cout << boost::format(" (%1%,%2%),") % + DefaultKeyFormatter(key.first) % key.second; } else { std::cout << DefaultKeyFormatter(f) << ","; } @@ -124,9 +138,10 @@ EliminateHybrid(const HybridFactorGraph &factors, // PRODUCT: multiply all factors gttic(product); - HybridConditional sum(KeyVector(continuousSeparator.begin(), continuousSeparator.end()), - DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()), - separatorKeys.size()); + HybridConditional sum( + KeyVector(continuousSeparator.begin(), continuousSeparator.end()), + DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()), + separatorKeys.size()); // HybridDiscreteFactor product(DiscreteConditional()); // for (auto&& factor : factors) product = (*factor) * product; @@ -139,10 +154,8 @@ EliminateHybrid(const HybridFactorGraph &factors, // 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()); + 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); @@ -163,12 +176,11 @@ EliminateHybrid(const HybridFactorGraph &factors, std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; // return std::make_pair(conditional, sum); - return std::make_pair( - conditional, - boost::make_shared(std::move(sum))); + return std::make_pair(conditional, + boost::make_shared(std::move(sum))); } void HybridFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 9a57d36e6..92f98c8f2 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -36,43 +36,50 @@ class HybridJunctionTree; class JacobianFactor; /** Main elimination function for HybridFactorGraph */ -GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); +GTSAM_EXPORT + std::pair, HybridFactor::shared_ptr> + EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ -template<> struct EliminationTraits -{ - typedef HybridFactor FactorType; ///< Type of factors in factor graph - typedef HybridFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) - typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination - typedef HybridBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination - typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree - typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree - typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree +template <> +struct EliminationTraits { + typedef HybridFactor FactorType; ///< Type of factors in factor graph + typedef HybridFactorGraph + FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) + typedef HybridConditional + ConditionalType; ///< Type of conditionals from elimination + typedef HybridBayesNet + BayesNetType; ///< Type of Bayes net from sequential elimination + typedef HybridEliminationTree + EliminationTreeType; ///< Type of elimination tree + typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree + typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function - static std::pair, boost::shared_ptr > + static std::pair, + boost::shared_ptr > DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { - return EliminateHybrid(factors, keys); } + return EliminateHybrid(factors, keys); + } }; - -class HybridFactorGraph : public FactorGraph, public EliminateableFactorGraph { +class HybridFactorGraph : public FactorGraph, + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = HybridFactorGraph; ///< this class + using This = HybridFactorGraph; ///< this class using BaseEliminateable = - EliminateableFactorGraph; ///< for elimination + EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility - using Indices = KeyVector; ///> map from keys to values + using Indices = KeyVector; ///> map from keys to values public: HybridFactorGraph() = default; -// /** Construct from container of factors (shared_ptr or plain objects) */ -// template -// explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} + // /** Construct from container of factors (shared_ptr or plain objects) */ + // template + // explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} /** Implicit copy/downcast constructor to override explicit template container * constructor. In BayesTree this is used for: @@ -84,8 +91,7 @@ class HybridFactorGraph : public FactorGraph, public Eliminateable using FactorGraph::add; /// Add a factor directly using a shared_ptr. - void add(JacobianFactor &&factor); + void add(JacobianFactor&& factor); }; -} - +} // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index faa4ba998..d83f90024 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -1,6 +1,19 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * 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 HybridGaussianFactor.cpp + * @date Mar 11, 2022 + * @author Fan Jiang + */ #include @@ -8,20 +21,22 @@ namespace gtsam { -HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) { +HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) + : Base(other->keys()) { inner = other; } -HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner(boost::make_shared(std::move(jf))) { +HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) + : Base(jf.keys()), + inner(boost::make_shared(std::move(jf))) {} -} - -bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { +bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { return false; } -void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { +void HybridGaussianFactor::print(const std::string &s, + const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); inner->print("inner: ", formatter); }; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 8562075b4..23a3c0110 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -15,9 +15,9 @@ * @author Fan Jiang */ +#include #include #include -#include namespace gtsam { @@ -36,8 +36,10 @@ class HybridGaussianFactor : public HybridFactor { explicit HybridGaussianFactor(JacobianFactor &&jf); public: - virtual bool equals(const HybridFactor& lf, double tol) const override; + virtual bool equals(const HybridFactor &lf, double tol) const override; - void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print( + const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 0e3d2ea00..9da1bfed3 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -15,9 +15,9 @@ * @author Fan Jiang */ -#include -#include #include +#include +#include namespace gtsam { @@ -27,7 +27,7 @@ template class JunctionTree; /* ************************************************************************* */ HybridJunctionTree::HybridJunctionTree( - const HybridEliminationTree& eliminationTree) : - Base(eliminationTree) {} + const HybridEliminationTree& eliminationTree) + : Base(eliminationTree) {} -} +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index c682f5216..f945d0702 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -15,32 +15,30 @@ * @author Fan Jiang */ -#include "Test.h" -#include -#include -#include -#include -#include -#include +#include +#include #include +#include #include #include - -#include +#include +#include +#include +#include +#include #include - #include - -#include +#include #include + using namespace boost::assign; using namespace std; using namespace gtsam; -using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::C; +using gtsam::symbol_shorthand::X; /* ************************************************************************* */ TEST(HybridFactorGraph, creation) { @@ -51,17 +49,13 @@ TEST(HybridFactorGraph, creation) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); CLGaussianConditional clgc( - {X(0)}, {X(1)}, - DiscreteKeys(DiscreteKey{C(0), 2}), + {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), CLGaussianConditional::Conditionals( C(0), - boost::make_shared( - X(0), Z_3x1, I_3x3, X(1), I_3x3), - boost::make_shared( - X(0), Vector3::Ones(), - I_3x3, X(1), - I_3x3)) - ); + boost::make_shared(X(0), Z_3x1, I_3x3, X(1), + I_3x3), + boost::make_shared(X(0), Vector3::Ones(), I_3x3, + X(1), I_3x3))); GTSAM_PRINT(clgc); } @@ -90,7 +84,6 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { } TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { - std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -100,29 +93,27 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { 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())); + 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)}, {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"))); + 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)})); + auto result = hfg.eliminateMultifrontal( + Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); GTSAM_PRINT(*result); GTSAM_PRINT(*result->marginalFactor(C(1))); } TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { - std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -132,29 +123,28 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { 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())); + 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)}, {c}, dt)); hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); -// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); + // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 + // 2 3 4"))); - auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)})); + auto result = + hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)})); GTSAM_PRINT(*result); // 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. + 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. */ } @@ -164,4 +154,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From 36ee4ce7cbc8dc5ffdf5ca71b107fd7a2dc3beb0 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 15 Mar 2022 12:53:58 -0400 Subject: [PATCH 13/70] Missing pragma onces --- gtsam/CMakeLists.txt | 2 +- gtsam/hybrid/CGMixtureFactor.h | 2 ++ gtsam/hybrid/CLGaussianConditional.h | 2 ++ gtsam/hybrid/HybridDiscreteFactor.h | 2 ++ gtsam/hybrid/HybridGaussianFactor.h | 2 ++ 5 files changed, 9 insertions(+), 1 deletion(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 845ac7cd0..09f1ea806 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -10,7 +10,7 @@ set (gtsam_subdirs inference symbolic discrete - hybrid + hybrid linear nonlinear sam diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h index 0528d5401..556c53cc5 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -18,6 +18,8 @@ * @date Mar 12, 2022 */ +#pragma once + #include #include #include diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h index 0429f4835..d80cb804f 100644 --- a/gtsam/hybrid/CLGaussianConditional.h +++ b/gtsam/hybrid/CLGaussianConditional.h @@ -16,6 +16,8 @@ * @date Mar 12, 2022 */ +#pragma once + #include #include #include diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 09da9cf70..f182f90a9 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -15,6 +15,8 @@ * @author Fan Jiang */ +#pragma once + #include #include #include diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 23a3c0110..03c49564e 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -15,6 +15,8 @@ * @author Fan Jiang */ +#pragma once + #include #include #include From 7ad2031b2fa81710fac83d96b021d4830d296ba7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 22 Mar 2022 15:37:28 -0400 Subject: [PATCH 14/70] Now we have real multifrontal! --- ...ianConditional.cpp => GaussianMixture.cpp} | 12 +- ...aussianConditional.h => GaussianMixture.h} | 16 +- gtsam/hybrid/HybridConditional.cpp | 24 ++- gtsam/hybrid/HybridConditional.h | 11 +- gtsam/hybrid/HybridFactor.cpp | 14 +- gtsam/hybrid/HybridFactor.h | 8 +- gtsam/hybrid/HybridFactorGraph.cpp | 92 ++++++++--- gtsam/hybrid/HybridJunctionTree.cpp | 152 +++++++++++++++++- gtsam/hybrid/tests/testHybridConditional.cpp | 83 ++++++++-- gtsam/inference/JunctionTree.h | 2 +- 10 files changed, 360 insertions(+), 54 deletions(-) rename gtsam/hybrid/{CLGaussianConditional.cpp => GaussianMixture.cpp} (83%) rename gtsam/hybrid/{CLGaussianConditional.h => GaussianMixture.h} (74%) diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/GaussianMixture.cpp similarity index 83% rename from gtsam/hybrid/CLGaussianConditional.cpp rename to gtsam/hybrid/GaussianMixture.cpp index 30531c023..6af5f7192 100644 --- a/gtsam/hybrid/CLGaussianConditional.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file CLGaussianConditional.cpp + * @file GaussianMixture.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @date Mar 12, 2022 @@ -18,25 +18,25 @@ #include #include -#include +#include #include namespace gtsam { -CLGaussianConditional::CLGaussianConditional( +GaussianMixture::GaussianMixture( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const CLGaussianConditional::Conditionals &conditionals) + const GaussianMixture::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), discreteParents), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} -bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const { +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { return false; } -void CLGaussianConditional::print(const std::string &s, +void GaussianMixture::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s << ": "; if (isContinuous_) std::cout << "Cont. "; diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/GaussianMixture.h similarity index 74% rename from gtsam/hybrid/CLGaussianConditional.h rename to gtsam/hybrid/GaussianMixture.h index d80cb804f..541a2b8a6 100644 --- a/gtsam/hybrid/CLGaussianConditional.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file CLGaussianConditional.h + * @file GaussianMixture.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @date Mar 12, 2022 @@ -24,21 +24,21 @@ #include namespace gtsam { -class CLGaussianConditional +class GaussianMixture : public HybridFactor, - public Conditional { + public Conditional { public: - using This = CLGaussianConditional; - using shared_ptr = boost::shared_ptr; + using This = GaussianMixture; + using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using BaseConditional = Conditional; using Conditionals = DecisionTree; Conditionals conditionals_; public: - CLGaussianConditional(const KeyVector &continuousFrontals, + GaussianMixture(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const Conditionals &conditionals); @@ -46,7 +46,7 @@ class CLGaussianConditional bool equals(const HybridFactor &lf, double tol = 1e-9) const override; void print( - const std::string &s = "CLGaussianConditional\n", + const std::string &s = "GaussianMixture\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; } // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 5cb98d921..e212f4534 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -19,9 +19,31 @@ #include namespace gtsam { + +HybridConditional::HybridConditional(const KeyVector &continuousFrontals, + const DiscreteKeys &discreteFrontals, + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents) + : HybridConditional( + CollectKeys({continuousFrontals.begin(), continuousFrontals.end()}, + {continuousParents.begin(), continuousParents.end()}), + CollectDiscreteKeys( + {discreteFrontals.begin(), discreteFrontals.end()}, + {discreteParents.begin(), discreteParents.end()}), + continuousFrontals.size() + discreteFrontals.size()) {} + +HybridConditional::HybridConditional( + boost::shared_ptr continuousConditional) + : BaseFactor(continuousConditional->keys()), + BaseConditional(continuousConditional->nrFrontals()) {} + void HybridConditional::print(const std::string &s, const KeyFormatter &formatter) const { - std::cout << s << "P("; + std::cout << s; + if (isContinuous_) std::cout << "Cont. "; + if (isDiscrete_) std::cout << "Disc. "; + if (isHybrid_) std::cout << "Hybr. "; + std::cout << "P("; int index = 0; const size_t N = keys().size(); const size_t contN = N - discreteKeys_.size(); diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 89071092f..d851cd68e 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -25,6 +25,8 @@ #include #include #include +#include "gtsam/inference/Key.h" +#include "gtsam/linear/GaussianConditional.h" namespace gtsam { @@ -33,8 +35,8 @@ namespace gtsam { * * As a type-erased variant of: * - DiscreteConditional - * - CLGaussianConditional * - GaussianConditional + * - GaussianMixture */ class GTSAM_EXPORT HybridConditional : public HybridFactor, @@ -62,6 +64,13 @@ class GTSAM_EXPORT HybridConditional const DiscreteKeys& discreteKeys, size_t nFrontals) : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {} + HybridConditional(const KeyVector& continuousFrontals, + const DiscreteKeys& discreteFrontals, + const KeyVector& continuousParents, + const DiscreteKeys& discreteParents); + + HybridConditional(boost::shared_ptr continuousConditional); + /** * @brief Combine two conditionals, yielding a new conditional with the union * of the frontal keys, ordered by gtsam::Key. diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index f16092eff..48a9dc468 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -53,7 +53,9 @@ HybridFactor::HybridFactor(const KeyVector &keys) HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) : Base(CollectKeys(continuousKeys, discreteKeys)), - isHybrid_(true), + isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)), + isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), + isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), discreteKeys_(discreteKeys) {} HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) @@ -61,6 +63,16 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) isDiscrete_(true), discreteKeys_(discreteKeys) {} +void HybridFactor::print( + const std::string &s, + const KeyFormatter &formatter) const { + std::cout << s; + if (isContinuous_) std::cout << "Cont. "; + if (isDiscrete_) std::cout << "Disc. "; + if (isHybrid_) std::cout << "Hybr. "; + this->printKeys("", formatter); +} + HybridFactor::~HybridFactor() = default; } // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index cd562869e..84d9b71bc 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -82,13 +82,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { /// print void print( const std::string &s = "HybridFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override { - std::cout << s; - if (isContinuous_) std::cout << "Cont. "; - if (isDiscrete_) std::cout << "Disc. "; - if (isHybrid_) std::cout << "Hybr. "; - this->printKeys("", formatter); - } + const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} /// @name Standard Interface diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 7d4daaceb..367f9f9ab 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -26,8 +26,11 @@ #include #include +#include #include "gtsam/inference/Key.h" +#include "gtsam/linear/GaussianFactorGraph.h" +#include "gtsam/linear/HessianFactor.h" namespace gtsam { @@ -60,6 +63,28 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // 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! + // Because of all these reasons, we need to think very carefully about how to + // implement the hybrid factors so that we do not get poor performance. + // + // The first thing is how to represent the GaussianMixture. A very possible + // scenario is that the incoming factors will have different levels of discrete + // keys. For example, imagine we are going to eliminate the fragment: + // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will need + // to know how to retrieve the corresponding continuous densities for the assi- + // -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). And we + // also need to consider when there is pruning. Two mixture factors could have + // different pruning patterns-one could have (c1=0,c2=1) pruned, and another + // could have (c2=0,c3=1) pruned, and this creates a big problem in how to + // identify the intersection of non-pruned branches. + + // One possible approach is first building the collection of all discrete keys. + // After that we enumerate the space of all key combinations *lazily* so that + // the exploration branch terminates whenever an assignment yields NULL in any + // of the hybrid factors. + + // When the number of assignments is large we may encounter stack overflows. + // However this is also the case with iSAM2, so no pressure :) + // PREPROCESS: Identify the nature of the current elimination std::unordered_map discreteCardinalities; std::set discreteSeparator; @@ -110,31 +135,62 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } } - 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 << "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) << ","; + } } + + 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; } - if (separatorKeys.size() > 0) { - std::cout << " | "; + // NOTE: We should really defer the product here because of pruning + + // Case 1: we are only dealing with continuous + if (discreteCardinalities.empty()) { + GaussianFactorGraph gfg; + for (auto &fp : factors) { + gfg.push_back(boost::static_pointer_cast(fp)->inner); + } + + auto result = EliminatePreferCholesky(gfg, frontalKeys); + return std::make_pair( + boost::make_shared(result.first), + boost::make_shared(result.second)); } - 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) << ","; + // Case 2: we are only dealing with discrete + if (discreteCardinalities.empty()) { + GaussianFactorGraph gfg; + for (auto &fp : factors) { + gfg.push_back(boost::static_pointer_cast(fp)->inner); } + + auto result = EliminatePreferCholesky(gfg, frontalKeys); + return std::make_pair( + boost::make_shared(result.first), + boost::make_shared(result.second)); } - std::cout << "\n" << RESET; + // PRODUCT: multiply all factors gttic(product); diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 9da1bfed3..9445e26b8 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -19,15 +19,163 @@ #include #include +#include + +#include "gtsam/hybrid/HybridFactorGraph.h" +#include "gtsam/inference/Key.h" + namespace gtsam { // Instantiate base classes template class EliminatableClusterTree; template class JunctionTree; +struct HybridConstructorTraversalData { + typedef typename JunctionTree::Node Node; + typedef typename JunctionTree::sharedNode + sharedNode; + + HybridConstructorTraversalData* const parentData; + sharedNode myJTNode; + FastVector childSymbolicConditionals; + FastVector childSymbolicFactors; + KeySet discreteKeys; + + // Small inner class to store symbolic factors + class SymbolicFactors : public FactorGraph {}; + + HybridConstructorTraversalData(HybridConstructorTraversalData* _parentData) + : parentData(_parentData) {} + + // Pre-order visitor function + static HybridConstructorTraversalData ConstructorTraversalVisitorPre( + const boost::shared_ptr& node, + HybridConstructorTraversalData& parentData) { + // On the pre-order pass, before children have been visited, we just set up + // a traversal data structure with its own JT node, and create a child + // pointer in its parent. + HybridConstructorTraversalData myData = + HybridConstructorTraversalData(&parentData); + myData.myJTNode = boost::make_shared(node->key, node->factors); + parentData.myJTNode->addChild(myData.myJTNode); + + std::cout << "Getting discrete info: "; + for (HybridFactor::shared_ptr& f : node->factors) { + for (auto& k : f->discreteKeys_) { + std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n"; + myData.discreteKeys.insert(k.first); + } + } + + return myData; + } + + // Post-order visitor function + static void ConstructorTraversalVisitorPostAlg2( + const boost::shared_ptr& ETreeNode, + const HybridConstructorTraversalData& myData) { + // In this post-order visitor, we combine the symbolic elimination results + // from the elimination tree children and symbolically eliminate the current + // elimination tree node. We then check whether each of our elimination + // tree child nodes should be merged with us. The check for this is that + // our number of symbolic elimination parents is exactly 1 less than + // our child's symbolic elimination parents - this condition indicates that + // eliminating the current node did not introduce any parents beyond those + // already in the child-> + + // Do symbolic elimination for this node + SymbolicFactors symbolicFactors; + symbolicFactors.reserve(ETreeNode->factors.size() + + myData.childSymbolicFactors.size()); + // Add ETree node factors + symbolicFactors += ETreeNode->factors; + // Add symbolic factors passed up from children + symbolicFactors += myData.childSymbolicFactors; + + Ordering keyAsOrdering; + keyAsOrdering.push_back(ETreeNode->key); + SymbolicConditional::shared_ptr myConditional; + SymbolicFactor::shared_ptr mySeparatorFactor; + boost::tie(myConditional, mySeparatorFactor) = + internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); + + std::cout << "Symbolic: "; + myConditional->print(); + + // Store symbolic elimination results in the parent + myData.parentData->childSymbolicConditionals.push_back(myConditional); + myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); + myData.parentData->discreteKeys.merge(myData.discreteKeys); + + sharedNode node = myData.myJTNode; + const FastVector& childConditionals = + myData.childSymbolicConditionals; + node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size()); + + // Merge our children if they are in our clique - if our conditional has + // exactly one fewer parent than our child's conditional. + const size_t myNrParents = myConditional->nrParents(); + const size_t nrChildren = node->nrChildren(); + assert(childConditionals.size() == nrChildren); + + // decide which children to merge, as index into children + std::vector nrFrontals = node->nrFrontalsOfChildren(); + std::vector merge(nrChildren, false); + size_t myNrFrontals = 1; + for (size_t i = 0; i < nrChildren; i++) { + // Check if we should merge the i^th child + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + const bool myType = + myData.discreteKeys.exists(myConditional->frontals()[0]); + const bool theirType = + myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); + std::cout << "Type: " + << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs " + << DefaultKeyFormatter(childConditionals[i]->frontals()[0]) + << "\n"; + if (myType == theirType) { + // Increment number of frontal variables + myNrFrontals += nrFrontals[i]; + std::cout << "Merging "; + childConditionals[i]->print(); + merge[i] = true; + } + } + } + + // now really merge + node->mergeChildren(merge); + } +}; + /* ************************************************************************* */ HybridJunctionTree::HybridJunctionTree( - const HybridEliminationTree& eliminationTree) - : Base(eliminationTree) {} + const HybridEliminationTree& eliminationTree) { + gttic(JunctionTree_FromEliminationTree); + // Here we rely on the BayesNet having been produced by this elimination tree, + // such that the conditionals are arranged in DFS post-order. We traverse the + // elimination tree, and inspect the symbolic conditional corresponding to + // each node. The elimination tree node is added to the same clique with its + // parent if it has exactly one more Bayes net conditional parent than + // does its elimination tree parent. + + // Traverse the elimination tree, doing symbolic elimination and merging nodes + // as we go. Gather the created junction tree roots in a dummy Node. + typedef typename HybridEliminationTree::Node ETreeNode; + typedef HybridConstructorTraversalData Data; + Data rootData(0); + rootData.myJTNode = + boost::make_shared(); // Make a dummy node to gather + // the junction tree roots + treeTraversal::DepthFirstForest(eliminationTree, rootData, + Data::ConstructorTraversalVisitorPre, + Data::ConstructorTraversalVisitorPostAlg2); + + // Assign roots from the dummy node + this->addChildrenAsRoots(rootData.myJTNode); + + // Transfer remaining factors from elimination tree + Base::remainingFactors_ = eliminationTree.remainingFactors(); +} } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index f945d0702..d16715300 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -31,6 +31,7 @@ #include #include +#include "gtsam/base/Testable.h" using namespace boost::assign; @@ -48,9 +49,9 @@ TEST(HybridFactorGraph, creation) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - CLGaussianConditional clgc( + GaussianMixture clgc( {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - CLGaussianConditional::Conditionals( + GaussianMixture::Conditionals( C(0), boost::make_shared(X(0), Z_3x1, I_3x3, X(1), I_3x3), @@ -69,7 +70,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) { EXPECT_LONGS_EQUAL(result.first->size(), 1); } -TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { +TEST(HybridFactorGraph, eliminateMultifrontal) { HybridFactorGraph hfg; DiscreteKey x(X(1), 2); @@ -132,13 +133,77 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 // 2 3 4"))); - auto result = - hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)})); + auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)}); - GTSAM_PRINT(*result); + HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full); - // We immediately need to escape the CLG domain if we do this!!! - GTSAM_PRINT(*result->marginalFactor(X(1))); + GTSAM_PRINT(*hbt); + /* + 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. + */ +} + +/** + * This test is about how to assemble the Bayes Tree roots after we do partial elimination +*/ +TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { + std::cout << ">>>>>>>>>>>>>>\n"; + + HybridFactorGraph hfg; + + hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); + hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); + + { + DecisionTree dt( + C(0), boost::make_shared(X(0), I_3x3, Z_3x1), + boost::make_shared(X(0), I_3x3, Vector3::Ones())); + + hfg.add(CGMixtureFactor({X(0)}, {{C(0), 2}}, dt)); + + DecisionTree dt1( + C(1), boost::make_shared(X(2), I_3x3, Z_3x1), + boost::make_shared(X(2), I_3x3, Vector3::Ones())); + + hfg.add(CGMixtureFactor({X(2)}, {{C(1), 2}}, dt1)); + } + + // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); + hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); + + hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); + hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); + + { + DecisionTree dt( + C(3), boost::make_shared(X(3), I_3x3, Z_3x1), + boost::make_shared(X(3), I_3x3, Vector3::Ones())); + + hfg.add(CGMixtureFactor({X(3)}, {{C(3), 2}}, dt)); + + DecisionTree dt1( + C(2), boost::make_shared(X(5), I_3x3, Z_3x1), + boost::make_shared(X(5), I_3x3, Vector3::Ones())); + + hfg.add(CGMixtureFactor({X(5)}, {{C(2), 2}}, dt1)); + } + + auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)}); + + GTSAM_PRINT(ordering_full); + + HybridBayesTree::shared_ptr hbt; + HybridFactorGraph::shared_ptr remaining; + std::tie(hbt, remaining) = + hfg.eliminatePartialMultifrontal(Ordering(ordering_full.begin(), ordering_full.end())); + + GTSAM_PRINT(*hbt); + + GTSAM_PRINT(*remaining); /* 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 diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index e01f3721a..e914c325e 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -70,7 +70,7 @@ namespace gtsam { /// @} - private: + protected: // Private default constructor (used in static construction methods) JunctionTree() {} From d42fc214176fb8ced3436edc4c9662dc2f1ef65a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:16:05 -0400 Subject: [PATCH 15/70] Fully working Multifrontal --- gtsam/hybrid/CGMixtureFactor.cpp | 27 ++ gtsam/hybrid/CGMixtureFactor.h | 12 + gtsam/hybrid/GaussianMixture.cpp | 27 ++ gtsam/hybrid/GaussianMixture.h | 10 + gtsam/hybrid/HybridConditional.cpp | 29 +- gtsam/hybrid/HybridConditional.h | 43 +-- gtsam/hybrid/HybridDiscreteFactor.cpp | 5 +- gtsam/hybrid/HybridFactor.cpp | 3 +- gtsam/hybrid/HybridFactor.h | 3 + gtsam/hybrid/HybridFactorGraph.cpp | 345 +++++++++++++++---- gtsam/hybrid/tests/testHybridConditional.cpp | 56 +-- 11 files changed, 454 insertions(+), 106 deletions(-) diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp index 705160273..08ae8b6e9 100644 --- a/gtsam/hybrid/CGMixtureFactor.cpp +++ b/gtsam/hybrid/CGMixtureFactor.cpp @@ -20,7 +20,9 @@ #include +#include #include +#include #include namespace gtsam { @@ -47,4 +49,29 @@ void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) }); } +const CGMixtureFactor::Factors& CGMixtureFactor::factors() { + return factors_; +} + +/* *******************************************************************************/ +CGMixtureFactor::Sum CGMixtureFactor::addTo(const CGMixtureFactor::Sum &sum) const { + using Y = GaussianFactorGraph; + auto add = [](const Y &graph1, const Y &graph2) { + auto result = graph1; + result.push_back(graph2); + return result; + }; + const Sum wrapped = wrappedFactors(); + return sum.empty() ? wrapped : sum.apply(wrapped, add); +} + +/* *******************************************************************************/ +CGMixtureFactor::Sum CGMixtureFactor::wrappedFactors() const { + auto wrap = [](const GaussianFactor::shared_ptr &factor) { + GaussianFactorGraph result; + result.push_back(factor); + return result; + }; + return {factors_, wrap}; +} } \ No newline at end of file diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h index 556c53cc5..fd81cdd91 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -27,6 +27,8 @@ namespace gtsam { +class GaussianFactorGraph; + class CGMixtureFactor : public HybridFactor { public: using Base = HybridFactor; @@ -42,6 +44,16 @@ class CGMixtureFactor : public HybridFactor { CGMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors); + using Sum = DecisionTree; + + const Factors& factors(); + + /* *******************************************************************************/ + Sum addTo(const Sum &sum) const; + + /* *******************************************************************************/ + Sum wrappedFactors() const; + bool equals(const HybridFactor &lf, double tol = 1e-9) const override; void print( diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 6af5f7192..8135b2d2d 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace gtsam { @@ -32,6 +33,32 @@ GaussianMixture::GaussianMixture( BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} +const GaussianMixture::Conditionals& GaussianMixture::conditionals() { + return conditionals_; +} + +/* *******************************************************************************/ +GaussianMixture::Sum GaussianMixture::addTo(const GaussianMixture::Sum &sum) const { + using Y = GaussianFactorGraph; + auto add = [](const Y &graph1, const Y &graph2) { + auto result = graph1; + result.push_back(graph2); + return result; + }; + const Sum wrapped = wrappedConditionals(); + return sum.empty() ? wrapped : sum.apply(wrapped, add); +} + +/* *******************************************************************************/ +GaussianMixture::Sum GaussianMixture::wrappedConditionals() const { + auto wrap = [](const GaussianFactor::shared_ptr &factor) { + GaussianFactorGraph result; + result.push_back(factor); + return result; + }; + return {conditionals_, wrap}; +} + bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { return false; } diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 541a2b8a6..13171ae5d 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -43,6 +43,16 @@ class GaussianMixture const DiscreteKeys &discreteParents, const Conditionals &conditionals); + using Sum = DecisionTree; + + const Conditionals& conditionals(); + + /* *******************************************************************************/ + Sum addTo(const Sum &sum) const; + + /* *******************************************************************************/ + Sum wrappedConditionals() const; + bool equals(const HybridFactor &lf, double tol = 1e-9) const override; void print( diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index e212f4534..7ada061b9 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -17,6 +17,8 @@ #include #include +#include "gtsam/hybrid/HybridFactor.h" +#include "gtsam/inference/Key.h" namespace gtsam { @@ -34,8 +36,27 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals, HybridConditional::HybridConditional( boost::shared_ptr continuousConditional) - : BaseFactor(continuousConditional->keys()), - BaseConditional(continuousConditional->nrFrontals()) {} + : HybridConditional(continuousConditional->keys(), {}, + continuousConditional->nrFrontals()) { + inner = continuousConditional; +} + +HybridConditional::HybridConditional( + boost::shared_ptr discreteConditional) + : HybridConditional({}, discreteConditional->discreteKeys(), + discreteConditional->nrFrontals()) { + inner = discreteConditional; +} + +HybridConditional::HybridConditional( + boost::shared_ptr gaussianMixture) + : BaseFactor(KeyVector(gaussianMixture->keys().begin(), + gaussianMixture->keys().begin() + + gaussianMixture->nrContinuous), + gaussianMixture->discreteKeys_), + BaseConditional(gaussianMixture->nrFrontals()) { + inner = gaussianMixture; +} void HybridConditional::print(const std::string &s, const KeyFormatter &formatter) const { @@ -70,8 +91,4 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const { return false; } -HybridConditional HybridConditional::operator*( - const HybridConditional &other) const { - return {}; -} } // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index d851cd68e..5fdf5064a 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -23,13 +23,20 @@ #include #include +#include #include +#include #include -#include "gtsam/inference/Key.h" -#include "gtsam/linear/GaussianConditional.h" +#include "gtsam/hybrid/GaussianMixture.h" + +#include +#include +#include namespace gtsam { +class HybridFactorGraph; + /** * Hybrid Conditional Density * @@ -49,9 +56,9 @@ class GTSAM_EXPORT HybridConditional typedef Conditional BaseConditional; ///< Typedef to our conditional base class - private: + protected: // Type-erased pointer to the inner type - std::unique_ptr inner; + boost::shared_ptr inner; public: /// @name Standard Constructors @@ -70,23 +77,15 @@ class GTSAM_EXPORT HybridConditional const DiscreteKeys& discreteParents); HybridConditional(boost::shared_ptr continuousConditional); + + HybridConditional(boost::shared_ptr discreteConditional); - /** - * @brief Combine two conditionals, yielding a new conditional with the union - * of the frontal keys, ordered by gtsam::Key. - * - * The two conditionals must make a valid Bayes net fragment, i.e., - * the frontal variables cannot overlap, and must be acyclic: - * Example of correct use: - * P(A,B) = P(A|B) * P(B) - * P(A,B|C) = P(A|B) * P(B|C) - * P(A,B,C) = P(A,B|C) * P(C) - * Example of incorrect use: - * P(A|B) * P(A|C) = ? - * P(A|B) * P(B|A) = ? - * We check for overlapping frontals, but do *not* check for cyclic. - */ - HybridConditional operator*(const HybridConditional& other) const; + HybridConditional(boost::shared_ptr gaussianMixture); + + GaussianMixture::shared_ptr asMixture() { + if (!isHybrid_) throw std::invalid_argument("Not a mixture"); + return boost::static_pointer_cast(inner); + } /// @} /// @name Testable @@ -101,6 +100,10 @@ class GTSAM_EXPORT HybridConditional bool equals(const HybridFactor& other, double tol = 1e-9) const override; /// @} + + friend std::pair // + EliminateHybrid(const HybridFactorGraph& factors, + const Ordering& frontalKeys); }; // DiscreteConditional diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 96d3842b8..be5659f04 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -19,12 +19,15 @@ #include #include +#include "gtsam/discrete/DecisionTreeFactor.h" namespace gtsam { +// TODO(fan): THIS IS VERY VERY DIRTY! We need to get DiscreteFactor right! HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) - : Base(other->keys()) { + : Base(boost::dynamic_pointer_cast(other)->discreteKeys()) { inner = other; + } HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 48a9dc468..a5ce8bd4e 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -48,7 +48,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, HybridFactor::HybridFactor() = default; HybridFactor::HybridFactor(const KeyVector &keys) - : Base(keys), isContinuous_(true) {} + : Base(keys), isContinuous_(true), nrContinuous(keys.size()) {} HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) @@ -56,6 +56,7 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys, isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)), isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), + nrContinuous(continuousKeys.size()), discreteKeys_(discreteKeys) {} HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 84d9b71bc..653d4270a 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -22,6 +22,7 @@ #include #include +#include #include namespace gtsam { @@ -46,6 +47,8 @@ class GTSAM_EXPORT HybridFactor : public Factor { bool isContinuous_ = false; bool isHybrid_ = false; + size_t nrContinuous = 0; + DiscreteKeys discreteKeys_; public: diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 367f9f9ab..735b8cfa9 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -13,9 +13,17 @@ * @file HybridFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang + * @author Varun Agrawal + * @author Frank Dellaert * @date Mar 11, 2022 */ +#include +#include +#include +#include +#include +#include #include #include #include @@ -23,14 +31,20 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include #include #include - -#include "gtsam/inference/Key.h" -#include "gtsam/linear/GaussianFactorGraph.h" -#include "gtsam/linear/HessianFactor.h" +#include namespace gtsam { @@ -42,6 +56,26 @@ static std::string GREEN = "\033[0;32m"; static std::string GREEN_BOLD = "\033[1;32m"; static std::string RESET = "\033[0m"; +static CGMixtureFactor::Sum &addGaussian( + CGMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { + using Y = GaussianFactorGraph; + // If the decision tree is not intiialized, then intialize it. + if (sum.empty()) { + GaussianFactorGraph result; + result.push_back(factor); + sum = CGMixtureFactor::Sum(result); + + } else { + auto add = [&factor](const Y &graph) { + auto result = graph; + result.push_back(factor); + return result; + }; + sum = sum.apply(add); + } + return sum; +} + /* ************************************************************************ */ std::pair // EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { @@ -65,29 +99,29 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Because of all these reasons, we need to think very carefully about how to // implement the hybrid factors so that we do not get poor performance. - // + // // The first thing is how to represent the GaussianMixture. A very possible - // scenario is that the incoming factors will have different levels of discrete - // keys. For example, imagine we are going to eliminate the fragment: - // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will need - // to know how to retrieve the corresponding continuous densities for the assi- - // -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). And we - // also need to consider when there is pruning. Two mixture factors could have - // different pruning patterns-one could have (c1=0,c2=1) pruned, and another - // could have (c2=0,c3=1) pruned, and this creates a big problem in how to - // identify the intersection of non-pruned branches. + // scenario is that the incoming factors will have different levels of + // discrete keys. For example, imagine we are going to eliminate the fragment: + // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will + // need to know how to retrieve the corresponding continuous densities for the + // assi- -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). + // And we also need to consider when there is pruning. Two mixture factors + // could have different pruning patterns-one could have (c1=0,c2=1) pruned, + // and another could have (c2=0,c3=1) pruned, and this creates a big problem + // in how to identify the intersection of non-pruned branches. - // One possible approach is first building the collection of all discrete keys. - // After that we enumerate the space of all key combinations *lazily* so that - // the exploration branch terminates whenever an assignment yields NULL in any - // of the hybrid factors. + // One possible approach is first building the collection of all discrete + // keys. After that we enumerate the space of all key combinations *lazily* so + // that the exploration branch terminates whenever an assignment yields NULL + // in any of the hybrid factors. // When the number of assignments is large we may encounter stack overflows. // However this is also the case with iSAM2, so no pressure :) // PREPROCESS: Identify the nature of the current elimination std::unordered_map discreteCardinalities; - std::set discreteSeparator; + std::set discreteSeparatorSet; std::set discreteFrontals; KeySet separatorKeys; @@ -123,15 +157,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { discreteFrontals.insert(discreteCardinalities.at(k)); } else { continuousFrontals.insert(k); + allContinuousKeys.insert(k); } } // Fill in discrete frontals and continuous frontals for the end result for (auto &k : separatorKeys) { if (discreteCardinalities.find(k) != discreteCardinalities.end()) { - discreteSeparator.insert(discreteCardinalities.at(k)); + discreteSeparatorSet.insert(discreteCardinalities.at(k)); } else { continuousSeparator.insert(k); + allContinuousKeys.insert(k); } } @@ -164,12 +200,18 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } // NOTE: We should really defer the product here because of pruning - + // Case 1: we are only dealing with continuous - if (discreteCardinalities.empty()) { + if (discreteCardinalities.empty() && !allContinuousKeys.empty()) { + std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n"; GaussianFactorGraph gfg; for (auto &fp : factors) { - gfg.push_back(boost::static_pointer_cast(fp)->inner); + auto ptr = boost::dynamic_pointer_cast(fp); + if (ptr) + gfg.push_back(ptr->inner); + else + gfg.push_back(boost::static_pointer_cast( + boost::static_pointer_cast(fp)->inner)); } auto result = EliminatePreferCholesky(gfg, frontalKeys); @@ -179,61 +221,248 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } // Case 2: we are only dealing with discrete - if (discreteCardinalities.empty()) { - GaussianFactorGraph gfg; + if (allContinuousKeys.empty()) { + std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n"; + DiscreteFactorGraph dfg; for (auto &fp : factors) { - gfg.push_back(boost::static_pointer_cast(fp)->inner); + auto ptr = boost::dynamic_pointer_cast(fp); + if (ptr) + dfg.push_back(ptr->inner); + else + dfg.push_back(boost::static_pointer_cast( + boost::static_pointer_cast(fp)->inner)); } - auto result = EliminatePreferCholesky(gfg, frontalKeys); + auto result = EliminateDiscrete(dfg, frontalKeys); + return std::make_pair( boost::make_shared(result.first), - boost::make_shared(result.second)); + boost::make_shared(result.second)); } + // Case 3: We are now in the hybrid land! + // NOTE: since we use the special JunctionTree, only possiblity is cont. + // conditioned on disc. + DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), + discreteSeparatorSet.end()); + + // We will need to know a mapping on when will a factor be fully determined by + // discrete keys std::vector> + // availableFactors; + + // { + // std::vector> keysForFactor; + // keysForFactor.reserve(factors.size()); + // std::transform( + // factors.begin(), factors.end(), std::back_inserter(keysForFactor), + // [](HybridFactor::shared_ptr factor) + // -> std::pair { + // return {KeySet(factor->keys().begin() + factor->nrContinuous, + // factor->keys().end()), + // factor}; + // }); + + // KeySet currentAllKeys; + // const auto N = discreteSeparator.size(); + // for (size_t k = 0; k < N; k++) { + // currentAllKeys.insert(discreteSeparator.at(k).first); + // std::vector shouldRemove(N, false); + // for (size_t i = 0; i < keysForFactor.size(); i++) { + // availableFactors.emplace_back(); + + // if (std::includes(currentAllKeys.begin(), currentAllKeys.end(), + // keysForFactor[i].first.begin(), + // keysForFactor[i].first.end())) { + // // mark for delete + // shouldRemove[i] = true; + // availableFactors.back().push_back(keysForFactor[i].second); + // } + // keysForFactor.erase( + // std::remove_if(keysForFactor.begin(), keysForFactor.end(), + // [&shouldRemove, &keysForFactor](std::pair const &i) { + // return shouldRemove.at(&i - + // keysForFactor.data()); + // }), + // keysForFactor.end()); + // } + // } + // } + + // std::function>( + // (Assignment, GaussianFactorGraph, int))> + // visitor = [&](Assignment history, GaussianFactorGraph gf, int pos) + // -> boost::shared_ptr> { + // const auto currentKey = discreteSeparator[pos].first; + // const auto currentCard = discreteSeparator[pos].second; + + // std::vector>> + // children(currentCard, nullptr); + // for (size_t choice = 0; choice < currentCard; choice++) { + // Assignment new_assignment = history; + // GaussianFactorGraph new_gf(gf); + // // we try to get all currently available factors + // for (auto &factor : availableFactors[pos]) { + // if (!factor) { + // continue; + // } + + // auto ptr_mf = boost::dynamic_pointer_cast(factor); + // if (ptr_mf) gf.push_back(ptr_mf->factors_(new_assignment)); + + // auto ptr_gm = boost::dynamic_pointer_cast(factor); + // if (ptr_gm) gf.push_back(ptr_gm->conditionals_(new_assignment)); + + // children[choice] = visitor(new_assignment, new_gf, pos + 1); + // } + // } + // }; + // PRODUCT: multiply all factors - gttic(product); - - HybridConditional sum( - KeyVector(continuousSeparator.begin(), continuousSeparator.end()), - DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()), - separatorKeys.size()); - - // HybridDiscreteFactor product(DiscreteConditional()); - // for (auto&& factor : factors) product = (*factor) * product; - gttoc(product); + // HybridConditional sum_factor( + // KeyVector(continuousSeparator.begin(), continuousSeparator.end()), + // DiscreteKeys(discreteSeparatorSet.begin(), discreteSeparatorSet.end()), + // separatorKeys.size()); // sum out frontals, this is the factor on the separator gttic(sum); - // HybridFactor::shared_ptr sum = product.sum(frontalKeys); + + std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n"; + + CGMixtureFactor::Sum sum; + + std::vector deferredFactors; + + for (auto &f : factors) { + if (f->isHybrid_) { + auto cgmf = boost::dynamic_pointer_cast(f); + if (cgmf) { + sum = cgmf->addTo(sum); + } + + auto gm = boost::dynamic_pointer_cast(f); + if (gm) { + sum = gm->asMixture()->addTo(sum); + } + + } else if (f->isContinuous_) { + deferredFactors.push_back( + boost::dynamic_pointer_cast(f)->inner); + } else { + throw std::invalid_argument( + "factor is discrete in continuous elimination"); + } + } + + for (auto &f : deferredFactors) { + std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n"; + sum = addGaussian(sum, f); + } + + std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET; + sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) { + RedirectCout rd; + gfg.print(""); + return rd.str(); + }); + 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()); + using EliminationPair = GaussianFactorGraph::EliminationResult; - // now divide product/sum to get conditional - gttic(divide); - // auto conditional = - // boost::make_shared(product, *sum, orderedKeys); - gttoc(divide); + KeyVector keysOfEliminated; // Not the ordering + KeyVector keysOfSeparator; // TODO(frank): Is this just (keys - ordering)? + + auto eliminate = [&](const GaussianFactorGraph &graph) + -> GaussianFactorGraph::EliminationResult { + if (graph.empty()) { + return {nullptr, nullptr}; + } + auto result = EliminatePreferCholesky(graph, frontalKeys); + if (keysOfEliminated.empty()) { + keysOfEliminated = + result.first->keys(); // Initialize the keysOfEliminated to be the + } + // keysOfEliminated of the GaussianConditional + if (keysOfSeparator.empty()) { + keysOfSeparator = result.second->keys(); + } + return result; + }; + + DecisionTree eliminationResults(sum, eliminate); + + auto pair = unzip(eliminationResults); + + const GaussianMixture::Conditionals &conditionals = pair.first; + const CGMixtureFactor::Factors &separatorFactors = pair.second; + + // Create the GaussianMixture from the conditionals + auto conditional = boost::make_shared( + frontalKeys, keysOfSeparator, discreteSeparator, conditionals); - auto conditional = boost::make_shared( - CollectKeys({continuousFrontals.begin(), continuousFrontals.end()}, - {continuousSeparator.begin(), continuousSeparator.end()}), - CollectDiscreteKeys({discreteFrontals.begin(), discreteFrontals.end()}, - {discreteSeparator.begin(), discreteSeparator.end()}), - continuousFrontals.size() + discreteFrontals.size()); std::cout << GREEN_BOLD << "[Conditional]\n" << RESET; conditional->print(); std::cout << GREEN_BOLD << "[Separator]\n" << RESET; - sum.print(); + separatorFactors.print("", DefaultKeyFormatter, + [](GaussianFactor::shared_ptr gc) { + RedirectCout rd; + gc->print(""); + return rd.str(); + }); std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; - // return std::make_pair(conditional, sum); - return std::make_pair(conditional, - boost::make_shared(std::move(sum))); + // If there are no more continuous parents, then we should create here a + // DiscreteFactor, with the error for each discrete choice. + if (keysOfSeparator.empty()) { + VectorValues empty_values; + auto factorError = [&](const GaussianFactor::shared_ptr &factor) { + if (!factor) return 0.0; // TODO(fan): does this make sense? + return exp(-factor->error(empty_values)); + }; + DecisionTree fdt(separatorFactors, factorError); + auto discreteFactor = + boost::make_shared(discreteSeparator, fdt); + + return {boost::make_shared(conditional), + boost::make_shared(discreteFactor)}; + + } else { + // Create a resulting DCGaussianMixture on the separator. + auto factor = boost::make_shared( + frontalKeys, discreteSeparator, separatorFactors); + return {boost::make_shared(conditional), factor}; + } + + // 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_factor.keys().begin(), + // sum_factor.keys().end()); + + // // now divide product/sum to get conditional + // gttic(divide); + // // auto conditional = + // // boost::make_shared(product, *sum, orderedKeys); + // gttoc(divide); + + // auto conditional = boost::make_shared( + // CollectKeys({continuousFrontals.begin(), continuousFrontals.end()}, + // {continuousSeparator.begin(), continuousSeparator.end()}), + // CollectDiscreteKeys( + // {discreteFrontals.begin(), discreteFrontals.end()}, + // {discreteSeparatorSet.begin(), discreteSeparatorSet.end()}), + // continuousFrontals.size() + discreteFrontals.size()); + // std::cout << GREEN_BOLD << "[Conditional]\n" << RESET; + // conditional->print(); + // std::cout << GREEN_BOLD << "[Separator]\n" << RESET; + // sum_factor.print(); + // std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; + + // // return std::make_pair(conditional, sum); + // return std::make_pair(conditional, boost::make_shared( + // std::move(sum_factor))); } void HybridFactorGraph::add(JacobianFactor &&factor) { diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index d16715300..6672a1870 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -31,7 +31,6 @@ #include #include -#include "gtsam/base/Testable.h" using namespace boost::assign; @@ -41,22 +40,33 @@ using namespace gtsam; using gtsam::symbol_shorthand::C; using gtsam::symbol_shorthand::X; +#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED + +#include // ::signal, ::raise + +#include + +void my_signal_handler(int signum) { + ::signal(signum, SIG_DFL); + std::cout << boost::stacktrace::stacktrace(); + ::raise(SIGABRT); +} + /* ************************************************************************* */ -TEST(HybridFactorGraph, creation) { +TEST_DISABLED(HybridFactorGraph, creation) { HybridConditional test; HybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GaussianMixture clgc( - {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - GaussianMixture::Conditionals( - C(0), - boost::make_shared(X(0), Z_3x1, I_3x3, X(1), - I_3x3), - boost::make_shared(X(0), Vector3::Ones(), I_3x3, - X(1), I_3x3))); + GaussianMixture clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), + GaussianMixture::Conditionals( + C(0), + boost::make_shared( + X(0), Z_3x1, I_3x3, X(1), I_3x3), + boost::make_shared( + X(0), Vector3::Ones(), I_3x3, X(1), I_3x3))); GTSAM_PRINT(clgc); } @@ -70,7 +80,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) { EXPECT_LONGS_EQUAL(result.first->size(), 1); } -TEST(HybridFactorGraph, eliminateMultifrontal) { +TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { HybridFactorGraph hfg; DiscreteKey x(X(1), 2); @@ -84,7 +94,7 @@ TEST(HybridFactorGraph, eliminateMultifrontal) { EXPECT_LONGS_EQUAL(result.second->size(), 1); } -TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { +TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -99,7 +109,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { boost::make_shared(X(1), I_3x3, Vector3::Ones())); hfg.add(CGMixtureFactor({X(1)}, {c1}, dt)); - hfg.add(CGMixtureFactor({X(0)}, {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"))); @@ -114,7 +124,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { GTSAM_PRINT(*result->marginalFactor(C(1))); } -TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { +TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -148,8 +158,9 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { } /** - * This test is about how to assemble the Bayes Tree roots after we do partial elimination -*/ + * This test is about how to assemble the Bayes Tree roots after we do partial + * elimination + */ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { std::cout << ">>>>>>>>>>>>>>\n"; @@ -173,7 +184,8 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { } // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); - hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); + hfg.add(HybridDiscreteFactor( + DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); @@ -192,14 +204,15 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(CGMixtureFactor({X(5)}, {{C(2), 2}}, dt1)); } - auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)}); + auto ordering_full = + Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)}); GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; HybridFactorGraph::shared_ptr remaining; - std::tie(hbt, remaining) = - hfg.eliminatePartialMultifrontal(Ordering(ordering_full.begin(), ordering_full.end())); + std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal( + Ordering(ordering_full.begin(), ordering_full.end())); GTSAM_PRINT(*hbt); @@ -215,6 +228,9 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ int main() { + ::signal(SIGSEGV, &my_signal_handler); + ::signal(SIGBUS, &my_signal_handler); + TestResult tr; return TestRegistry::runAllTests(tr); } From 1e8aae3f06b5cb704de3a1a7578b15216760159a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:18:02 -0400 Subject: [PATCH 16/70] Add a test for EliminateSequential --- gtsam/hybrid/tests/testHybridConditional.cpp | 31 +++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 6672a1870..32cb05325 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -94,6 +94,35 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { EXPECT_LONGS_EQUAL(result.second->size(), 1); } +TEST(HybridFactorGraph, eliminateFullSequentialSimple) { + std::cout << ">>>>>>>>>>>>>>\n"; + + HybridFactorGraph hfg; + + 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)); + + 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)}, {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.eliminateSequential( + Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); + + GTSAM_PRINT(*result); +} + TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { std::cout << ">>>>>>>>>>>>>>\n"; @@ -121,7 +150,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); GTSAM_PRINT(*result); - GTSAM_PRINT(*result->marginalFactor(C(1))); + GTSAM_PRINT(*result->marginalFactor(C(2))); } TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) { From b4f8eea23138266f795b994048f559861939b180 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:36:18 -0400 Subject: [PATCH 17/70] Address comments --- ...reFactor.cpp => GaussianMixtureFactor.cpp} | 18 ++++++++--------- ...ixtureFactor.h => GaussianMixtureFactor.h} | 10 +++++----- gtsam/hybrid/HybridBayesNet.h | 6 +++--- gtsam/hybrid/HybridBayesTree.h | 4 +++- gtsam/hybrid/HybridDiscreteFactor.h | 4 ++++ gtsam/hybrid/HybridFactor.h | 5 +++++ gtsam/hybrid/HybridFactorGraph.cpp | 18 ++++++++--------- gtsam/hybrid/HybridGaussianFactor.h | 4 ++++ gtsam/hybrid/tests/CMakeLists.txt | 2 +- gtsam/hybrid/tests/testHybridConditional.cpp | 20 +++++++++---------- 10 files changed, 53 insertions(+), 38 deletions(-) rename gtsam/hybrid/{CGMixtureFactor.cpp => GaussianMixtureFactor.cpp} (77%) rename gtsam/hybrid/{CGMixtureFactor.h => GaussianMixtureFactor.h} (87%) diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp similarity index 77% rename from gtsam/hybrid/CGMixtureFactor.cpp rename to gtsam/hybrid/GaussianMixtureFactor.cpp index 08ae8b6e9..61cfb1d70 100644 --- a/gtsam/hybrid/CGMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file CGMixtureFactor.cpp + * @file GaussianMixtureFactor.cpp * @brief A set of Gaussian factors indexed by a set of discrete keys. * @author Fan Jiang * @author Varun Agrawal @@ -18,7 +18,7 @@ * @date Mar 12, 2022 */ -#include +#include #include #include @@ -27,15 +27,15 @@ namespace gtsam { -CGMixtureFactor::CGMixtureFactor(const KeyVector &continuousKeys, +GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) : Base(continuousKeys, discreteKeys), factors_(factors) {} -bool CGMixtureFactor::equals(const HybridFactor &lf, double tol) const { +bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { return false; } -void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { +void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); factors_.print( "mixture = ", @@ -49,12 +49,12 @@ void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) }); } -const CGMixtureFactor::Factors& CGMixtureFactor::factors() { +const GaussianMixtureFactor::Factors& GaussianMixtureFactor::factors() { return factors_; } /* *******************************************************************************/ -CGMixtureFactor::Sum CGMixtureFactor::addTo(const CGMixtureFactor::Sum &sum) const { +GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo(const GaussianMixtureFactor::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -66,7 +66,7 @@ CGMixtureFactor::Sum CGMixtureFactor::addTo(const CGMixtureFactor::Sum &sum) con } /* *******************************************************************************/ -CGMixtureFactor::Sum CGMixtureFactor::wrappedFactors() const { +GaussianMixtureFactor::Sum GaussianMixtureFactor::wrappedFactors() const { auto wrap = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -74,4 +74,4 @@ CGMixtureFactor::Sum CGMixtureFactor::wrappedFactors() const { }; return {factors_, wrap}; } -} \ No newline at end of file +} diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h similarity index 87% rename from gtsam/hybrid/CGMixtureFactor.h rename to gtsam/hybrid/GaussianMixtureFactor.h index fd81cdd91..4cd5e71de 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file CGMixtureFactor.h + * @file GaussianMixtureFactor.h * @brief A set of Gaussian factors indexed by a set of discrete keys. * @author Fan Jiang * @author Varun Agrawal @@ -29,19 +29,19 @@ namespace gtsam { class GaussianFactorGraph; -class CGMixtureFactor : public HybridFactor { +class GaussianMixtureFactor : public HybridFactor { public: using Base = HybridFactor; - using This = CGMixtureFactor; + using This = GaussianMixtureFactor; using shared_ptr = boost::shared_ptr; using Factors = DecisionTree; Factors factors_; - CGMixtureFactor() = default; + GaussianMixtureFactor() = default; - CGMixtureFactor(const KeyVector &continuousKeys, + GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors); using Sum = DecisionTree; diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 53d10518f..d7e2f33af 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -25,8 +25,8 @@ namespace gtsam { /** - * A hybrid Bayes net can have discrete conditionals, Gaussian mixtures, - * or pure Gaussian conditionals. + * A hybrid Bayes net is a collection of HybridConditionals, which can have + * discrete conditionals, Gaussian mixtures, or pure Gaussian conditionals. */ class GTSAM_EXPORT HybridBayesNet : public BayesNet { public: @@ -40,4 +40,4 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { HybridBayesNet() : Base() {} }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 74b967fc8..ee51bdd6c 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -33,7 +33,9 @@ class HybridConditional; class VectorValues; /* ************************************************************************* */ -/** A clique in a HybridBayesTree */ +/** A clique in a HybridBayesTree + * which is a HybridConditional internally. + */ class GTSAM_EXPORT HybridBayesTreeClique : public BayesTreeCliqueBase { public: diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index f182f90a9..c55c5ecf0 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -23,6 +23,10 @@ namespace gtsam { +/** + * A HybridDiscreteFactor is a wrapper for DiscreteFactor, so we hide the + * implementation of DiscreteFactor, and thus avoiding diamond inheritance. + */ class HybridDiscreteFactor : public HybridFactor { public: using Base = HybridFactor; diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 653d4270a..3d5bd7b21 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -34,6 +34,11 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, /** * Base class for hybrid probabilistic factors + * Examples: + * - HybridGaussianFactor + * - HybridDiscreteFactor + * - GaussianMixtureFactor + * - GaussianMixture */ class GTSAM_EXPORT HybridFactor : public Factor { public: diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 735b8cfa9..574c3b781 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,14 +56,14 @@ static std::string GREEN = "\033[0;32m"; static std::string GREEN_BOLD = "\033[1;32m"; static std::string RESET = "\033[0m"; -static CGMixtureFactor::Sum &addGaussian( - CGMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { +static GaussianMixtureFactor::Sum &addGaussian( + GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { using Y = GaussianFactorGraph; // If the decision tree is not intiialized, then intialize it. if (sum.empty()) { GaussianFactorGraph result; result.push_back(factor); - sum = CGMixtureFactor::Sum(result); + sum = GaussianMixtureFactor::Sum(result); } else { auto add = [&factor](const Y &graph) { @@ -307,7 +307,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // continue; // } - // auto ptr_mf = boost::dynamic_pointer_cast(factor); + // auto ptr_mf = boost::dynamic_pointer_cast(factor); // if (ptr_mf) gf.push_back(ptr_mf->factors_(new_assignment)); // auto ptr_gm = boost::dynamic_pointer_cast(factor); @@ -329,13 +329,13 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n"; - CGMixtureFactor::Sum sum; + GaussianMixtureFactor::Sum sum; std::vector deferredFactors; for (auto &f : factors) { if (f->isHybrid_) { - auto cgmf = boost::dynamic_pointer_cast(f); + auto cgmf = boost::dynamic_pointer_cast(f); if (cgmf) { sum = cgmf->addTo(sum); } @@ -395,7 +395,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { auto pair = unzip(eliminationResults); const GaussianMixture::Conditionals &conditionals = pair.first; - const CGMixtureFactor::Factors &separatorFactors = pair.second; + const GaussianMixtureFactor::Factors &separatorFactors = pair.second; // Create the GaussianMixture from the conditionals auto conditional = boost::make_shared( @@ -429,7 +429,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } else { // Create a resulting DCGaussianMixture on the separator. - auto factor = boost::make_shared( + auto factor = boost::make_shared( frontalKeys, discreteSeparator, separatorFactors); return {boost::make_shared(conditional), factor}; } diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 03c49564e..86c87a0ec 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -23,6 +23,10 @@ namespace gtsam { +/** + * A HybridGaussianFactor is a wrapper for GaussianFactor so that we do not have + * a diamond inheritance. + */ class HybridGaussianFactor : public HybridFactor { public: using Base = HybridFactor; diff --git a/gtsam/hybrid/tests/CMakeLists.txt b/gtsam/hybrid/tests/CMakeLists.txt index b52e18586..06ad2c505 100644 --- a/gtsam/hybrid/tests/CMakeLists.txt +++ b/gtsam/hybrid/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam") \ No newline at end of file +gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam") diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 32cb05325..876105510 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -108,8 +108,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) { 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)}, {c1}, dt)); - // hfg.add(CGMixtureFactor({X(0)}, {c1}, dt)); + hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); + // hfg.add(GaussianMixtureFactor({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"))); @@ -137,8 +137,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { 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)}, {c1}, dt)); - // hfg.add(CGMixtureFactor({X(0)}, {c1}, dt)); + hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); + // hfg.add(GaussianMixtureFactor({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"))); @@ -167,7 +167,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) { 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)}, {c}, dt)); + hfg.add(GaussianMixtureFactor({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"))); @@ -203,13 +203,13 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { C(0), boost::make_shared(X(0), I_3x3, Z_3x1), boost::make_shared(X(0), I_3x3, Vector3::Ones())); - hfg.add(CGMixtureFactor({X(0)}, {{C(0), 2}}, dt)); + hfg.add(GaussianMixtureFactor({X(0)}, {{C(0), 2}}, dt)); DecisionTree dt1( C(1), boost::make_shared(X(2), I_3x3, Z_3x1), boost::make_shared(X(2), I_3x3, Vector3::Ones())); - hfg.add(CGMixtureFactor({X(2)}, {{C(1), 2}}, dt1)); + hfg.add(GaussianMixtureFactor({X(2)}, {{C(1), 2}}, dt1)); } // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); @@ -224,13 +224,13 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { C(3), boost::make_shared(X(3), I_3x3, Z_3x1), boost::make_shared(X(3), I_3x3, Vector3::Ones())); - hfg.add(CGMixtureFactor({X(3)}, {{C(3), 2}}, dt)); + hfg.add(GaussianMixtureFactor({X(3)}, {{C(3), 2}}, dt)); DecisionTree dt1( C(2), boost::make_shared(X(5), I_3x3, Z_3x1), boost::make_shared(X(5), I_3x3, Vector3::Ones())); - hfg.add(CGMixtureFactor({X(5)}, {{C(2), 2}}, dt1)); + hfg.add(GaussianMixtureFactor({X(5)}, {{C(2), 2}}, dt1)); } auto ordering_full = From 4f8dfeb5b254ab75f34fca053c56bede74e3f5d6 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:36:56 -0400 Subject: [PATCH 18/70] Remove warning --- gtsam/hybrid/HybridJunctionTree.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 9445e26b8..7c48934fc 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -161,7 +161,6 @@ HybridJunctionTree::HybridJunctionTree( // Traverse the elimination tree, doing symbolic elimination and merging nodes // as we go. Gather the created junction tree roots in a dummy Node. - typedef typename HybridEliminationTree::Node ETreeNode; typedef HybridConstructorTraversalData Data; Data rootData(0); rootData.myJTNode = From 293ef614acc01156bdbb767761a7f5dba42b998b Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:42:26 -0400 Subject: [PATCH 19/70] Address comments --- gtsam/hybrid/HybridEliminationTree.h | 3 +++ gtsam/hybrid/HybridFactorGraph.h | 10 ++++++++-- gtsam/hybrid/HybridGaussianFactor.cpp | 2 +- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index b72bbcad9..902beb279 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -23,6 +23,9 @@ namespace gtsam { +/** + * Elimination Tree type for Hybrid + */ class GTSAM_EXPORT HybridEliminationTree : public EliminationTree { public: diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 92f98c8f2..21332d80a 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -37,8 +37,8 @@ class JacobianFactor; /** Main elimination function for HybridFactorGraph */ GTSAM_EXPORT - std::pair, HybridFactor::shared_ptr> - EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); +std::pair, HybridFactor::shared_ptr> +EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ template <> @@ -62,6 +62,12 @@ struct EliminationTraits { } }; +/** + * Hybrid Factor Graph + * ----------------------- + * This is the linear version of a hybrid factor graph. Everything inside needs + * to be hybrid factor or hybrid conditional. + */ class HybridFactorGraph : public FactorGraph, public EliminateableFactorGraph { public: diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index d83f90024..5fa4b555a 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -39,4 +39,4 @@ void HybridGaussianFactor::print(const std::string &s, inner->print("inner: ", formatter); }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From a36c86a4f168a03f316d3c13f8bb78b13dfc3a71 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:50:36 -0400 Subject: [PATCH 20/70] Display debug messages only when DEBUG = true --- gtsam/hybrid/HybridFactorGraph.cpp | 186 ++++++++--------------------- 1 file changed, 49 insertions(+), 137 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 574c3b781..9e7eebc57 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -21,8 +21,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -56,6 +56,8 @@ static std::string GREEN = "\033[0;32m"; static std::string GREEN_BOLD = "\033[1;32m"; static std::string RESET = "\033[0m"; +static bool DEBUG = false; + static GaussianMixtureFactor::Sum &addGaussian( GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { using Y = GaussianFactorGraph; @@ -129,15 +131,18 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { KeySet continuousFrontals; KeySet continuousSeparator; - // TODO: we do a mock by just doing the correct key thing - std::cout << RED_BOLD << "Begin Eliminate: " << RESET; - frontalKeys.print(); + if (DEBUG) { + std::cout << RED_BOLD << "Begin Eliminate: " << RESET; + frontalKeys.print(); + } // This initializes separatorKeys and discreteCardinalities for (auto &&factor : factors) { - std::cout << ">>> Adding factor: " << GREEN; - factor->print(); - std::cout << RESET; + if (DEBUG) { + std::cout << ">>> Adding factor: " << GREEN; + factor->print(); + std::cout << RESET; + } separatorKeys.insert(factor->begin(), factor->end()); if (!factor->isContinuous_) { for (auto &k : factor->discreteKeys_) { @@ -171,7 +176,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } } - { + // Only for printing + if (DEBUG) { std::cout << RED_BOLD << "Keys: " << RESET; for (auto &f : frontalKeys) { if (discreteCardinalities.find(f) != discreteCardinalities.end()) { @@ -203,7 +209,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Case 1: we are only dealing with continuous if (discreteCardinalities.empty() && !allContinuousKeys.empty()) { - std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n"; + if (DEBUG) { + std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n"; + } + GaussianFactorGraph gfg; for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); @@ -222,7 +231,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Case 2: we are only dealing with discrete if (allContinuousKeys.empty()) { - std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n"; + if (DEBUG) { + std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n"; + } + DiscreteFactorGraph dfg; for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); @@ -246,88 +258,12 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), discreteSeparatorSet.end()); - // We will need to know a mapping on when will a factor be fully determined by - // discrete keys std::vector> - // availableFactors; - - // { - // std::vector> keysForFactor; - // keysForFactor.reserve(factors.size()); - // std::transform( - // factors.begin(), factors.end(), std::back_inserter(keysForFactor), - // [](HybridFactor::shared_ptr factor) - // -> std::pair { - // return {KeySet(factor->keys().begin() + factor->nrContinuous, - // factor->keys().end()), - // factor}; - // }); - - // KeySet currentAllKeys; - // const auto N = discreteSeparator.size(); - // for (size_t k = 0; k < N; k++) { - // currentAllKeys.insert(discreteSeparator.at(k).first); - // std::vector shouldRemove(N, false); - // for (size_t i = 0; i < keysForFactor.size(); i++) { - // availableFactors.emplace_back(); - - // if (std::includes(currentAllKeys.begin(), currentAllKeys.end(), - // keysForFactor[i].first.begin(), - // keysForFactor[i].first.end())) { - // // mark for delete - // shouldRemove[i] = true; - // availableFactors.back().push_back(keysForFactor[i].second); - // } - // keysForFactor.erase( - // std::remove_if(keysForFactor.begin(), keysForFactor.end(), - // [&shouldRemove, &keysForFactor](std::pair const &i) { - // return shouldRemove.at(&i - - // keysForFactor.data()); - // }), - // keysForFactor.end()); - // } - // } - // } - - // std::function>( - // (Assignment, GaussianFactorGraph, int))> - // visitor = [&](Assignment history, GaussianFactorGraph gf, int pos) - // -> boost::shared_ptr> { - // const auto currentKey = discreteSeparator[pos].first; - // const auto currentCard = discreteSeparator[pos].second; - - // std::vector>> - // children(currentCard, nullptr); - // for (size_t choice = 0; choice < currentCard; choice++) { - // Assignment new_assignment = history; - // GaussianFactorGraph new_gf(gf); - // // we try to get all currently available factors - // for (auto &factor : availableFactors[pos]) { - // if (!factor) { - // continue; - // } - - // auto ptr_mf = boost::dynamic_pointer_cast(factor); - // if (ptr_mf) gf.push_back(ptr_mf->factors_(new_assignment)); - - // auto ptr_gm = boost::dynamic_pointer_cast(factor); - // if (ptr_gm) gf.push_back(ptr_gm->conditionals_(new_assignment)); - - // children[choice] = visitor(new_assignment, new_gf, pos + 1); - // } - // } - // }; - - // PRODUCT: multiply all factors - // HybridConditional sum_factor( - // KeyVector(continuousSeparator.begin(), continuousSeparator.end()), - // DiscreteKeys(discreteSeparatorSet.begin(), discreteSeparatorSet.end()), - // separatorKeys.size()); - // sum out frontals, this is the factor on the separator gttic(sum); - std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n"; + if (DEBUG) { + std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n"; + } GaussianMixtureFactor::Sum sum; @@ -355,16 +291,20 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } for (auto &f : deferredFactors) { - std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n"; + if (DEBUG) { + std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n"; + } sum = addGaussian(sum, f); } - std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET; - sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) { - RedirectCout rd; - gfg.print(""); - return rd.str(); - }); + if (DEBUG) { + std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET; + sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) { + RedirectCout rd; + gfg.print(""); + return rd.str(); + }); + } gttoc(sum); @@ -401,23 +341,25 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { auto conditional = boost::make_shared( frontalKeys, keysOfSeparator, discreteSeparator, conditionals); - std::cout << GREEN_BOLD << "[Conditional]\n" << RESET; - conditional->print(); - std::cout << GREEN_BOLD << "[Separator]\n" << RESET; - separatorFactors.print("", DefaultKeyFormatter, - [](GaussianFactor::shared_ptr gc) { - RedirectCout rd; - gc->print(""); - return rd.str(); - }); - std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; + if (DEBUG) { + std::cout << GREEN_BOLD << "[Conditional]\n" << RESET; + conditional->print(); + std::cout << GREEN_BOLD << "[Separator]\n" << RESET; + separatorFactors.print("", DefaultKeyFormatter, + [](GaussianFactor::shared_ptr gc) { + RedirectCout rd; + gc->print(""); + return rd.str(); + }); + std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; + } // If there are no more continuous parents, then we should create here a // DiscreteFactor, with the error for each discrete choice. if (keysOfSeparator.empty()) { VectorValues empty_values; auto factorError = [&](const GaussianFactor::shared_ptr &factor) { - if (!factor) return 0.0; // TODO(fan): does this make sense? + if (!factor) return 0.0; // TODO(fan): does this make sense? return exp(-factor->error(empty_values)); }; DecisionTree fdt(separatorFactors, factorError); @@ -433,36 +375,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { frontalKeys, discreteSeparator, separatorFactors); return {boost::make_shared(conditional), factor}; } - - // 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_factor.keys().begin(), - // sum_factor.keys().end()); - - // // now divide product/sum to get conditional - // gttic(divide); - // // auto conditional = - // // boost::make_shared(product, *sum, orderedKeys); - // gttoc(divide); - - // auto conditional = boost::make_shared( - // CollectKeys({continuousFrontals.begin(), continuousFrontals.end()}, - // {continuousSeparator.begin(), continuousSeparator.end()}), - // CollectDiscreteKeys( - // {discreteFrontals.begin(), discreteFrontals.end()}, - // {discreteSeparatorSet.begin(), discreteSeparatorSet.end()}), - // continuousFrontals.size() + discreteFrontals.size()); - // std::cout << GREEN_BOLD << "[Conditional]\n" << RESET; - // conditional->print(); - // std::cout << GREEN_BOLD << "[Separator]\n" << RESET; - // sum_factor.print(); - // std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; - - // // return std::make_pair(conditional, sum); - // return std::make_pair(conditional, boost::make_shared( - // std::move(sum_factor))); } void HybridFactorGraph::add(JacobianFactor &&factor) { From 8b4283b281a691c7c797ba943abfe1355ca8d162 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:54:53 -0400 Subject: [PATCH 21/70] Add doxygen for GMM --- gtsam/hybrid/GaussianMixture.h | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 13171ae5d..9879e80bd 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -24,9 +24,8 @@ #include namespace gtsam { -class GaussianMixture - : public HybridFactor, - public Conditional { +class GaussianMixture : public HybridFactor, + public Conditional { public: using This = GaussianMixture; using shared_ptr = boost::shared_ptr; @@ -38,14 +37,22 @@ class GaussianMixture Conditionals conditionals_; public: + /** + * @brief Construct a new Gaussian Mixture object + * + * @param continuousFrontals the continuous frontals. + * @param continuousParents the continuous parents. + * @param discreteParents the discrete parents. Will be placed last. + * @param conditionals a decision tree of GaussianConditionals. + */ GaussianMixture(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const Conditionals &conditionals); + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const Conditionals &conditionals); using Sum = DecisionTree; - const Conditionals& conditionals(); + const Conditionals &conditionals(); /* *******************************************************************************/ Sum addTo(const Sum &sum) const; From 5f03e0f68b27aa62d6fe519ea65beecdb838ef8d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:59:31 -0400 Subject: [PATCH 22/70] Address compilation error on GCC --- gtsam/hybrid/HybridConditional.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7ada061b9..ed4b01608 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -28,7 +28,7 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals, const DiscreteKeys &discreteParents) : HybridConditional( CollectKeys({continuousFrontals.begin(), continuousFrontals.end()}, - {continuousParents.begin(), continuousParents.end()}), + KeyVector {continuousParents.begin(), continuousParents.end()}), CollectDiscreteKeys( {discreteFrontals.begin(), discreteFrontals.end()}, {discreteParents.begin(), discreteParents.end()}), From 8d888606a36fe48fee634c4ef0a8bcbffb365e44 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 21:31:22 -0400 Subject: [PATCH 23/70] Fix GCC error --- gtsam/hybrid/GaussianMixture.cpp | 2 ++ gtsam/hybrid/tests/testHybridConditional.cpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 8135b2d2d..26668da59 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -13,6 +13,8 @@ * @file GaussianMixture.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang + * @author Varun Agrawal + * @author Frank Dellaert * @date Mar 12, 2022 */ diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 876105510..95703b0af 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -17,8 +17,8 @@ #include #include -#include #include +#include #include #include #include @@ -75,7 +75,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - auto result = hfg.eliminatePartialSequential({0}); + auto result = hfg.eliminatePartialSequential(KeyVector{0}); EXPECT_LONGS_EQUAL(result.first->size(), 1); } From d2dc620b1e3031fc35c5ebe9d5030235b54ebf68 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 25 Mar 2022 17:14:00 -0600 Subject: [PATCH 24/70] Add Python bindings --- gtsam/hybrid/GaussianMixture.cpp | 19 ++- gtsam/hybrid/GaussianMixture.h | 5 + gtsam/hybrid/GaussianMixtureFactor.cpp | 45 ++++--- gtsam/hybrid/GaussianMixtureFactor.h | 11 +- gtsam/hybrid/HybridConditional.cpp | 9 +- gtsam/hybrid/HybridDiscreteFactor.h | 4 +- gtsam/hybrid/HybridFactorGraph.cpp | 14 ++ gtsam/hybrid/HybridFactorGraph.h | 7 + gtsam/hybrid/hybrid.i | 134 +++++++++++++++++++ gtsam/hybrid/tests/testHybridConditional.cpp | 29 ++-- gtsam/inference/inference.i | 13 +- python/CMakeLists.txt | 1 + python/gtsam/preamble/hybrid.h | 14 ++ python/gtsam/specializations/hybrid.h | 4 + python/gtsam/tests/test_HybridFactorGraph.py | 49 +++++++ 15 files changed, 311 insertions(+), 47 deletions(-) create mode 100644 gtsam/hybrid/hybrid.i create mode 100644 python/gtsam/preamble/hybrid.h create mode 100644 python/gtsam/specializations/hybrid.h create mode 100644 python/gtsam/tests/test_HybridFactorGraph.py diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 26668da59..bc674966c 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -35,12 +35,23 @@ GaussianMixture::GaussianMixture( BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} -const GaussianMixture::Conditionals& GaussianMixture::conditionals() { +const GaussianMixture::Conditionals &GaussianMixture::conditionals() { return conditionals_; } +GaussianMixture GaussianMixture::FromConditionalList( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const std::vector &conditionalsList) { + Conditionals dt(discreteParents, conditionalsList); + + return GaussianMixture(continuousFrontals, continuousParents, discreteParents, + dt); +} + /* *******************************************************************************/ -GaussianMixture::Sum GaussianMixture::addTo(const GaussianMixture::Sum &sum) const { +GaussianMixture::Sum GaussianMixture::addTo( + const GaussianMixture::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -66,7 +77,7 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { } void GaussianMixture::print(const std::string &s, - const KeyFormatter &formatter) const { + const KeyFormatter &formatter) const { std::cout << s << ": "; if (isContinuous_) std::cout << "Cont. "; if (isDiscrete_) std::cout << "Disc. "; @@ -88,4 +99,4 @@ void GaussianMixture::print(const std::string &s, return rd.str(); }); } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 9879e80bd..4412e741c 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -60,6 +60,11 @@ class GaussianMixture : public HybridFactor, /* *******************************************************************************/ Sum wrappedConditionals() const; + static This FromConditionalList( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const std::vector &conditionals); + bool equals(const HybridFactor &lf, double tol = 1e-9) const override; void print( diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 61cfb1d70..3963e675e 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -18,43 +18,52 @@ * @date Mar 12, 2022 */ -#include - -#include -#include -#include #include +#include +#include +#include +#include namespace gtsam { GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const Factors &factors) : Base(continuousKeys, discreteKeys), - factors_(factors) {} + const DiscreteKeys &discreteKeys, + const Factors &factors) + : Base(continuousKeys, discreteKeys), factors_(factors) {} bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { return false; } -void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { +GaussianMixtureFactor GaussianMixtureFactor::FromFactorList( + const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, + const std::vector &factorsList) { + Factors dt(discreteKeys, factorsList); + + return GaussianMixtureFactor(continuousKeys, discreteKeys, dt); +} + +void GaussianMixtureFactor::print(const std::string &s, + const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); factors_.print( - "mixture = ", - [&](Key k) { - return formatter(k); - }, [&](const GaussianFactor::shared_ptr &gf) -> std::string { + "mixture = ", [&](Key k) { return formatter(k); }, + [&](const GaussianFactor::shared_ptr &gf) -> std::string { RedirectCout rd; - if (!gf->empty()) gf->print("", formatter); - else return {"nullptr"}; + if (!gf->empty()) + gf->print("", formatter); + else + return {"nullptr"}; return rd.str(); }); } -const GaussianMixtureFactor::Factors& GaussianMixtureFactor::factors() { +const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() { return factors_; } /* *******************************************************************************/ -GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo(const GaussianMixtureFactor::Sum &sum) const { +GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo( + const GaussianMixtureFactor::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -74,4 +83,4 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::wrappedFactors() const { }; return {factors_, wrap}; } -} +} // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 4cd5e71de..57a0cca03 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -29,6 +29,8 @@ namespace gtsam { class GaussianFactorGraph; +typedef std::vector GaussianFactorVector; + class GaussianMixtureFactor : public HybridFactor { public: using Base = HybridFactor; @@ -42,11 +44,16 @@ class GaussianMixtureFactor : public HybridFactor { GaussianMixtureFactor() = default; GaussianMixtureFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, const Factors &factors); + const DiscreteKeys &discreteKeys, + const Factors &factors); using Sum = DecisionTree; - const Factors& factors(); + const Factors &factors(); + + static This FromFactorList( + const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, + const std::vector &factors); /* *******************************************************************************/ Sum addTo(const Sum &sum) const; diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index ed4b01608..48bee192c 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -16,9 +16,9 @@ */ #include +#include #include -#include "gtsam/hybrid/HybridFactor.h" -#include "gtsam/inference/Key.h" +#include namespace gtsam { @@ -27,8 +27,9 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents) : HybridConditional( - CollectKeys({continuousFrontals.begin(), continuousFrontals.end()}, - KeyVector {continuousParents.begin(), continuousParents.end()}), + CollectKeys( + {continuousFrontals.begin(), continuousFrontals.end()}, + KeyVector{continuousParents.begin(), continuousParents.end()}), CollectDiscreteKeys( {discreteFrontals.begin(), discreteFrontals.end()}, {discreteParents.begin(), discreteParents.end()}), diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index c55c5ecf0..809510eac 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -35,10 +35,10 @@ class HybridDiscreteFactor : public HybridFactor { DiscreteFactor::shared_ptr inner; - // Implicit conversion from a shared ptr of GF + // Implicit conversion from a shared ptr of DF HybridDiscreteFactor(DiscreteFactor::shared_ptr other); - // Forwarding constructor from concrete JacobianFactor + // Forwarding constructor from concrete DecisionTreeFactor HybridDiscreteFactor(DecisionTreeFactor &&dtf); public: diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 9e7eebc57..6c7599a12 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -380,4 +381,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { void HybridFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } + +void HybridFactorGraph::add(DecisionTreeFactor &&factor) { + FactorGraph::add(boost::make_shared(std::move(factor))); +} + +void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { + FactorGraph::add(boost::make_shared(factor)); +} + +void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) { + FactorGraph::add(boost::make_shared(factor)); +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 21332d80a..bfd6a8690 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -32,6 +32,7 @@ class HybridBayesNet; class HybridEliminationTree; class HybridBayesTree; class HybridJunctionTree; +class DecisionTreeFactor; class JacobianFactor; @@ -98,6 +99,12 @@ class HybridFactorGraph : public FactorGraph, /// Add a factor directly using a shared_ptr. void add(JacobianFactor&& factor); + + void add(DecisionTreeFactor&& factor); + + void add(boost::shared_ptr factor); + + void add(boost::shared_ptr factor); }; } // namespace gtsam diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i new file mode 100644 index 000000000..c84cd82c5 --- /dev/null +++ b/gtsam/hybrid/hybrid.i @@ -0,0 +1,134 @@ +//************************************************************************* +// hybrid +//************************************************************************* + +namespace gtsam { + +#include +virtual class HybridFactor { + void print(string s = "HybridFactor\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const; + bool empty() const; + size_t size() const; + gtsam::KeyVector keys() const; +}; + +#include +class GaussianMixtureFactor : gtsam::HybridFactor { + static GaussianMixtureFactor FromFactorList( + const gtsam::KeyVector& continuousKeys, + const gtsam::DiscreteKeys& discreteKeys, + const std::vector& factorsList); + + void print(string s = "GaussianMixtureFactor\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + +#include +class GaussianMixture : gtsam::HybridFactor { + static GaussianMixture FromConditionalList( + const gtsam::KeyVector& continuousFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKeys& discreteParents, + const std::vector& + conditionalsList); + + void print(string s = "GaussianMixture\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + +#include +class HybridBayesTreeClique { + HybridBayesTreeClique(); + HybridBayesTreeClique(const gtsam::HybridConditional* conditional); + const gtsam::HybridConditional* conditional() const; + bool isRoot() const; + // double evaluate(const gtsam::HybridValues& values) const; +}; + +#include +class HybridBayesTree { + HybridBayesTree(); + void print(string s = "HybridBayesTree\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::HybridBayesTree& other, double tol = 1e-9) const; + + size_t size() const; + bool empty() const; + const HybridBayesTreeClique* operator[](size_t j) const; + + string dot(const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + +class HybridBayesNet { + HybridBayesNet(); + void add(const gtsam::HybridConditional& s); + bool empty() const; + size_t size() const; + gtsam::KeySet keys() const; + const gtsam::HybridConditional* at(size_t i) const; + void print(string s = "HybridBayesNet\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::HybridBayesNet& other, double tol = 1e-9) const; + + string dot( + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, + const gtsam::DotWriter& writer = gtsam::DotWriter()) const; + void saveGraph( + string s, + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, + const gtsam::DotWriter& writer = gtsam::DotWriter()) const; +}; + +#include +class HybridFactorGraph { + HybridFactorGraph(); + HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); + + // Building the graph + void push_back(const gtsam::HybridFactor* factor); + void push_back(const gtsam::HybridConditional* conditional); + void push_back(const gtsam::HybridFactorGraph& graph); + void push_back(const gtsam::HybridBayesNet& bayesNet); + void push_back(const gtsam::HybridBayesTree& bayesTree); + void push_back(const gtsam::GaussianMixtureFactor* gmm); + + void add(gtsam::DecisionTreeFactor* factor); + void add(gtsam::JacobianFactor* factor); + + bool empty() const; + size_t size() const; + gtsam::KeySet keys() const; + const gtsam::HybridFactor* at(size_t i) const; + + void print(string s = "") const; + bool equals(const gtsam::HybridFactorGraph& fg, double tol = 1e-9) const; + + gtsam::HybridBayesNet* eliminateSequential(); + gtsam::HybridBayesNet* eliminateSequential( + gtsam::Ordering::OrderingType type); + gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + pair + eliminatePartialSequential(const gtsam::Ordering& ordering); + + gtsam::HybridBayesTree* eliminateMultifrontal(); + gtsam::HybridBayesTree* eliminateMultifrontal( + gtsam::Ordering::OrderingType type); + gtsam::HybridBayesTree* eliminateMultifrontal( + const gtsam::Ordering& ordering); + pair + eliminatePartialMultifrontal(const gtsam::Ordering& ordering); + + string dot( + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, + const gtsam::DotWriter& writer = gtsam::DotWriter()) const; +}; + +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 95703b0af..97685b87d 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -133,15 +133,19 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { 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())); + // DecisionTree dt( + // C(1), boost::make_shared(X(1), I_3x3, Z_3x1), + // boost::make_shared(X(1), I_3x3, Vector3::Ones())); + + // hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); + hfg.add(GaussianMixtureFactor::FromFactorList( + {X(1)}, {{C(1), 2}}, + {boost::make_shared(X(1), I_3x3, Z_3x1), + boost::make_shared(X(1), I_3x3, Vector3::Ones())})); - hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); // hfg.add(GaussianMixtureFactor({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(DecisionTreeFactor(c1, {2, 8})); + hfg.add(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"))); @@ -199,11 +203,14 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); { - DecisionTree dt( - C(0), boost::make_shared(X(0), I_3x3, Z_3x1), - boost::make_shared(X(0), I_3x3, Vector3::Ones())); + // DecisionTree dt( + // C(0), boost::make_shared(X(0), I_3x3, Z_3x1), + // boost::make_shared(X(0), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor({X(0)}, {{C(0), 2}}, dt)); + hfg.add(GaussianMixtureFactor::FromFactorList( + {X(0)}, {{C(0), 2}}, + {boost::make_shared(X(0), I_3x3, Z_3x1), + boost::make_shared(X(0), I_3x3, Vector3::Ones())})); DecisionTree dt1( C(1), boost::make_shared(X(2), I_3x3, Z_3x1), diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index e7b074ec4..bbf1b2daa 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -9,6 +9,7 @@ namespace gtsam { #include #include #include +#include #include @@ -106,36 +107,36 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> static gtsam::Ordering ColamdConstrainedLast( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> static gtsam::Ordering ColamdConstrainedFirst( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, const FACTOR_GRAPH& graph); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 85ddc7b6d..5059ac95d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -65,6 +65,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i + ${PROJECT_SOURCE_DIR}/gtsam/hybrid/hybrid.i ) set(GTSAM_PYTHON_TARGET gtsam_py) diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h new file mode 100644 index 000000000..56a07cfdd --- /dev/null +++ b/python/gtsam/preamble/hybrid.h @@ -0,0 +1,14 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +#include diff --git a/python/gtsam/specializations/hybrid.h b/python/gtsam/specializations/hybrid.h new file mode 100644 index 000000000..bede6d86c --- /dev/null +++ b/python/gtsam/specializations/hybrid.h @@ -0,0 +1,4 @@ + +py::bind_vector >(m_, "GaussianFactorVector"); + +py::implicitly_convertible >(); diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py new file mode 100644 index 000000000..d8b0f1f33 --- /dev/null +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -0,0 +1,49 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Hybrid Factor Graphs. +Author: Fan Jiang +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +import numpy as np +from gtsam.symbol_shorthand import X, C +from gtsam.utils.test_case import GtsamTestCase + + +class TestHybridFactorGraph(GtsamTestCase): + def test_create(self): + noiseModel = gtsam.noiseModel.Unit.Create(3) + dk = gtsam.DiscreteKeys() + dk.push_back((C(0), 2)) + + # print(dk.at(0)) + jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), + noiseModel) + jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), + noiseModel) + + gmf = gtsam.GaussianMixtureFactor.FromFactorList([X(0)], dk, + [jf1, jf2]) + + hfg = gtsam.HybridFactorGraph() + hfg.add(jf1) + hfg.add(jf2) + hfg.push_back(gmf) + + hfg.eliminateSequential( + gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph( + hfg, [C(0)])).print() + + +if __name__ == "__main__": + unittest.main() From 7f2fa61fb53422aa195cd5815d51f3b5c4e47814 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 25 Mar 2022 23:28:40 -0600 Subject: [PATCH 25/70] Added more Python examples --- gtsam/hybrid/HybridConditional.h | 34 +++++++++++++++----- gtsam/hybrid/HybridFactorGraph.cpp | 26 +++++++-------- gtsam/hybrid/HybridJunctionTree.cpp | 11 +++++++ gtsam/hybrid/hybrid.i | 11 +++++++ python/gtsam/tests/test_HybridFactorGraph.py | 12 +++++-- 5 files changed, 71 insertions(+), 23 deletions(-) diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 5fdf5064a..dea8fa9f4 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -19,7 +19,10 @@ #include #include +#include #include +#include +#include #include #include @@ -27,11 +30,8 @@ #include #include #include -#include "gtsam/hybrid/GaussianMixture.h" -#include -#include -#include +#include "gtsam/hybrid/GaussianMixture.h" namespace gtsam { @@ -44,6 +44,19 @@ class HybridFactorGraph; * - DiscreteConditional * - GaussianConditional * - GaussianMixture + * + * The reason why this is important is that `Conditional` is a CRTP class. + * CRTP is static polymorphism such that all CRTP classes, while bearing the + * same name, are different classes not sharing a vtable. This prevents them + * from being contained in any container, and thus it is impossible to + * dynamically cast between them. A better option, as illustrated here, is + * treating them as an implementation detail - such that the hybrid mechanism + * does not know what is inside the HybridConditional. This prevents us from + * having diamond inheritances, and neutralized the need to change other + * components of GTSAM to make hybrid elimination work. + * + * A great reference to the type-erasure pattern is Edurado Madrid's CppCon + * talk. */ class GTSAM_EXPORT HybridConditional : public HybridFactor, @@ -76,15 +89,20 @@ class GTSAM_EXPORT HybridConditional const KeyVector& continuousParents, const DiscreteKeys& discreteParents); - HybridConditional(boost::shared_ptr continuousConditional); + HybridConditional( + boost::shared_ptr continuousConditional); HybridConditional(boost::shared_ptr discreteConditional); - + HybridConditional(boost::shared_ptr gaussianMixture); GaussianMixture::shared_ptr asMixture() { - if (!isHybrid_) throw std::invalid_argument("Not a mixture"); - return boost::static_pointer_cast(inner); + if (!isHybrid_) throw std::invalid_argument("Not a mixture"); + return boost::static_pointer_cast(inner); + } + + boost::shared_ptr getInner() { + return inner; } /// @} diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 6c7599a12..7a26dfadb 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -57,7 +57,7 @@ static std::string GREEN = "\033[0;32m"; static std::string GREEN_BOLD = "\033[1;32m"; static std::string RESET = "\033[0m"; -static bool DEBUG = false; +constexpr bool DEBUG = false; static GaussianMixtureFactor::Sum &addGaussian( GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { @@ -123,7 +123,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // However this is also the case with iSAM2, so no pressure :) // PREPROCESS: Identify the nature of the current elimination - std::unordered_map discreteCardinalities; + std::unordered_map mapFromKeyToDiscreteKey; std::set discreteSeparatorSet; std::set discreteFrontals; @@ -137,7 +137,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { frontalKeys.print(); } - // This initializes separatorKeys and discreteCardinalities + // This initializes separatorKeys and mapFromKeyToDiscreteKey for (auto &&factor : factors) { if (DEBUG) { std::cout << ">>> Adding factor: " << GREEN; @@ -147,7 +147,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { separatorKeys.insert(factor->begin(), factor->end()); if (!factor->isContinuous_) { for (auto &k : factor->discreteKeys_) { - discreteCardinalities[k.first] = k; + mapFromKeyToDiscreteKey[k.first] = k; } } } @@ -159,8 +159,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Fill in discrete frontals and continuous frontals for the end result for (auto &k : frontalKeys) { - if (discreteCardinalities.find(k) != discreteCardinalities.end()) { - discreteFrontals.insert(discreteCardinalities.at(k)); + if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) { + discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k)); } else { continuousFrontals.insert(k); allContinuousKeys.insert(k); @@ -169,8 +169,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Fill in discrete frontals and continuous frontals for the end result for (auto &k : separatorKeys) { - if (discreteCardinalities.find(k) != discreteCardinalities.end()) { - discreteSeparatorSet.insert(discreteCardinalities.at(k)); + if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) { + discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k)); } else { continuousSeparator.insert(k); allContinuousKeys.insert(k); @@ -181,8 +181,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { if (DEBUG) { std::cout << RED_BOLD << "Keys: " << RESET; for (auto &f : frontalKeys) { - if (discreteCardinalities.find(f) != discreteCardinalities.end()) { - auto &key = discreteCardinalities.at(f); + if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) { + auto &key = mapFromKeyToDiscreteKey.at(f); std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second; } else { @@ -195,8 +195,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } for (auto &f : separatorKeys) { - if (discreteCardinalities.find(f) != discreteCardinalities.end()) { - auto &key = discreteCardinalities.at(f); + if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) { + auto &key = mapFromKeyToDiscreteKey.at(f); std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second; } else { @@ -209,7 +209,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // NOTE: We should really defer the product here because of pruning // Case 1: we are only dealing with continuous - if (discreteCardinalities.empty() && !allContinuousKeys.empty()) { + if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) { if (DEBUG) { std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n"; } diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 7c48934fc..22b40ad05 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -59,10 +59,15 @@ struct HybridConstructorTraversalData { myData.myJTNode = boost::make_shared(node->key, node->factors); parentData.myJTNode->addChild(myData.myJTNode); +#ifndef NDEBUG std::cout << "Getting discrete info: "; +#endif for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys_) { +#ifndef NDEBUG std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n"; +#endif + myData.discreteKeys.insert(k.first); } } @@ -99,8 +104,10 @@ struct HybridConstructorTraversalData { boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); +#ifndef NDEBUG std::cout << "Symbolic: "; myConditional->print(); +#endif // Store symbolic elimination results in the parent myData.parentData->childSymbolicConditionals.push_back(myConditional); @@ -129,15 +136,19 @@ struct HybridConstructorTraversalData { myData.discreteKeys.exists(myConditional->frontals()[0]); const bool theirType = myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); +#ifndef NDEBUG std::cout << "Type: " << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs " << DefaultKeyFormatter(childConditionals[i]->frontals()[0]) << "\n"; +#endif if (myType == theirType) { // Increment number of frontal variables myNrFrontals += nrFrontals[i]; +#ifndef NDEBUG std::cout << "Merging "; childConditionals[i]->print(); +#endif merge[i] = true; } } diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index c84cd82c5..052575011 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -15,6 +15,17 @@ virtual class HybridFactor { gtsam::KeyVector keys() const; }; +#include +virtual class HybridConditional { + void print(string s = "Hybrid Conditional\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::HybridConditional& other, double tol = 1e-9) const; + size_t nrFrontals() const; + size_t nrParents() const; + Factor* getInner(); +}; + #include class GaussianMixtureFactor : gtsam::HybridFactor { static GaussianMixtureFactor FromFactorList( diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index d8b0f1f33..48187b7a2 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -40,9 +40,17 @@ class TestHybridFactorGraph(GtsamTestCase): hfg.add(jf2) hfg.push_back(gmf) - hfg.eliminateSequential( + hbn = hfg.eliminateSequential( gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph( - hfg, [C(0)])).print() + hfg, [C(0)])) + + print("hbn = ", hbn) + + mixture = hbn.at(0).getInner() + print(mixture) + + discrete_conditional = hbn.at(hbn.size()-1).getInner() + print(discrete_conditional) if __name__ == "__main__": From 0f69b4c93f09cda7fe802c7fdec27f73134cb257 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 26 Mar 2022 23:13:57 -0600 Subject: [PATCH 26/70] Added plotting for nested dissection --- gtsam/hybrid/HybridConditional.h | 2 +- gtsam/hybrid/tests/testHybridConditional.cpp | 87 ++++++++++++++++++-- 2 files changed, 82 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index dea8fa9f4..b91d2b91a 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -31,7 +31,7 @@ #include #include -#include "gtsam/hybrid/GaussianMixture.h" +#include namespace gtsam { diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 97685b87d..703ec136e 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -31,6 +31,11 @@ #include #include +#include + +#include "gtsam/inference/DotWriter.h" +#include "gtsam/inference/Key.h" +#include "gtsam/inference/Ordering.h" using namespace boost::assign; @@ -83,10 +88,10 @@ TEST_DISABLED(HybridFactorGraph, eliminate) { TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { HybridFactorGraph hfg; - DiscreteKey x(X(1), 2); + DiscreteKey c(C(1), 2); hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); - hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8}))); + hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); auto result = hfg.eliminatePartialMultifrontal({X(0)}); @@ -94,7 +99,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { EXPECT_LONGS_EQUAL(result.second->size(), 1); } -TEST(HybridFactorGraph, eliminateFullSequentialSimple) { +TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -123,7 +128,7 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) { GTSAM_PRINT(*result); } -TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { +TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -247,12 +252,82 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { HybridBayesTree::shared_ptr hbt; HybridFactorGraph::shared_ptr remaining; - std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal( - Ordering(ordering_full.begin(), ordering_full.end())); + std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); GTSAM_PRINT(*remaining); + + hbt->dot(std::cout); + /* + 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. + */ +} + +HybridFactorGraph::shared_ptr makeSwitchingChain(size_t n) { + HybridFactorGraph hfg; + + hfg.add(JacobianFactor(X(1), I_3x3, Z_3x1)); + + // X(1) to X(n+1) + for (size_t t = 1; t < n; t++) { + hfg.add(GaussianMixtureFactor::FromFactorList( + {X(t), X(t + 1)}, {{C(t), 2}}, + {boost::make_shared(X(t), I_3x3, X(t + 1), I_3x3, + Z_3x1), + boost::make_shared(X(t), I_3x3, X(t + 1), I_3x3, + Vector3::Ones())})); + } + + return boost::make_shared(std::move(hfg)); +} + +// TODO(fan): make a graph like Varun's paper one +TEST(HybridFactorGraph, Switching) { + auto hfg = makeSwitchingChain(9); + + // X(5) will be the center, X(1-4), X(6-9) + // X(3), X(7) + // X(2), X(8) + // X(1), X(4), X(6), X(9) + // C(5) will be the center, C(1-4), C(6-8) + // C(3), C(7) + // C(1), C(4), C(2), C(6), C(8) + auto ordering_full = + Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), X(5), + C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)}); + + GTSAM_PRINT(*hfg); + GTSAM_PRINT(ordering_full); + + HybridBayesTree::shared_ptr hbt; + HybridFactorGraph::shared_ptr remaining; + std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); + + GTSAM_PRINT(*hbt); + + GTSAM_PRINT(*remaining); + + { + DotWriter dw; + dw.positionHints['c'] = 2; + dw.positionHints['x'] = 1; + std::cout << hfg->dot(DefaultKeyFormatter, dw); + std::cout << "\n"; + hbt->dot(std::cout); + } + + { + DotWriter dw; + dw.positionHints['x'] = 1; + std::cout << "\n"; + std::cout << hfg->eliminateSequential(ordering_full) + ->dot(DefaultKeyFormatter, dw); + } /* 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 From a9c1d43dc530092f6fb4cbf35047a70069eb21de Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 1 Apr 2022 00:20:22 -0400 Subject: [PATCH 27/70] Working iSAM for Hybrid --- gtsam/hybrid/HybridBayesTree.h | 29 ++ gtsam/hybrid/HybridConditional.h | 10 +- gtsam/hybrid/HybridFactorGraph.cpp | 45 ++- gtsam/hybrid/HybridISAM.cpp | 99 ++++++ gtsam/hybrid/HybridISAM.h | 59 ++++ gtsam/hybrid/HybridJunctionTree.cpp | 6 +- gtsam/hybrid/tests/Switching.h | 68 ++++ gtsam/hybrid/tests/testHybridConditional.cpp | 313 +++++++++++++++++-- gtsam/inference/BayesTree.h | 24 +- 9 files changed, 595 insertions(+), 58 deletions(-) create mode 100644 gtsam/hybrid/HybridISAM.cpp create mode 100644 gtsam/hybrid/HybridISAM.h create mode 100644 gtsam/hybrid/tests/Switching.h diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index ee51bdd6c..dd8b7f022 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -70,4 +70,33 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /// @} }; +/* This does special stuff for the hybrid case */ +template +class BayesTreeOrphanWrapper< + CLIQUE, + boost::enable_if_t::value> > + : public CLIQUE::ConditionalType { + public: + typedef CLIQUE CliqueType; + typedef typename CLIQUE::ConditionalType Base; + + boost::shared_ptr clique; + + BayesTreeOrphanWrapper(const boost::shared_ptr& clique) + : clique(clique) { + // Store parent keys in our base type factor so that eliminating those + // parent keys will pull this subtree into the elimination. + this->keys_.assign(clique->conditional()->beginParents(), + clique->conditional()->endParents()); + this->discreteKeys_.assign(clique->conditional()->discreteKeys_.begin(), + clique->conditional()->discreteKeys_.end()); + } + + void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + clique->print(s + "stored clique", formatter); + } +}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index b91d2b91a..5d7ee2351 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include #include @@ -31,8 +32,6 @@ #include #include -#include - namespace gtsam { class HybridFactorGraph; @@ -101,10 +100,13 @@ class GTSAM_EXPORT HybridConditional return boost::static_pointer_cast(inner); } - boost::shared_ptr getInner() { - return inner; + DiscreteConditional::shared_ptr asDiscreteConditional() { + if (!isDiscrete_) throw std::invalid_argument("Not a discrete conditional"); + return boost::static_pointer_cast(inner); } + boost::shared_ptr getInner() { return inner; } + /// @} /// @name Testable /// @{ diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 7a26dfadb..ee6a5dd82 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -217,11 +217,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) + if (ptr) { gfg.push_back(ptr->inner); - else - gfg.push_back(boost::static_pointer_cast( - boost::static_pointer_cast(fp)->inner)); + } else { + auto p = boost::static_pointer_cast(fp)->inner; + if (p) { + gfg.push_back(boost::static_pointer_cast(p)); + } else { + // It is an orphan wrapper + if (DEBUG) std::cout << "Got an orphan wrapper conditional\n"; + } + } } auto result = EliminatePreferCholesky(gfg, frontalKeys); @@ -239,11 +245,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) + if (ptr) { dfg.push_back(ptr->inner); - else - dfg.push_back(boost::static_pointer_cast( - boost::static_pointer_cast(fp)->inner)); + } else { + auto p = boost::static_pointer_cast(fp)->inner; + if (p) { + dfg.push_back(boost::static_pointer_cast(p)); + } else { + // It is an orphan wrapper + if (DEBUG) std::cout << "Got an orphan wrapper conditional\n"; + } + } } auto result = EliminateDiscrete(dfg, frontalKeys); @@ -286,8 +298,18 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { deferredFactors.push_back( boost::dynamic_pointer_cast(f)->inner); } else { - throw std::invalid_argument( - "factor is discrete in continuous elimination"); + // We need to handle the case where the object is actually an + // BayesTreeOrphanWrapper! + auto orphan = boost::dynamic_pointer_cast< + BayesTreeOrphanWrapper>(f); + if (orphan) { + if (DEBUG) std::cout << "Got an orphan wrapper conditional\n"; + } else { + auto &fr = *f; + throw std::invalid_argument( + std::string("factor is discrete in continuous elimination") + + typeid(fr).name()); + } } } @@ -373,7 +395,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } else { // Create a resulting DCGaussianMixture on the separator. auto factor = boost::make_shared( - frontalKeys, discreteSeparator, separatorFactors); + KeyVector(continuousSeparator.begin(), continuousSeparator.end()), + discreteSeparator, separatorFactors); return {boost::make_shared(conditional), factor}; } } diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp new file mode 100644 index 000000000..0db30f1f3 --- /dev/null +++ b/gtsam/hybrid/HybridISAM.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridISAM.h + * @date March 31, 2022 + * @author Fan Jiang + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +// Instantiate base class +// template class ISAM; + +/* ************************************************************************* */ +HybridISAM::HybridISAM() {} + +/* ************************************************************************* */ +HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} + +void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, + const HybridBayesTree::Eliminate& function) { + // Remove the contaminated part of the Bayes tree + BayesNetType bn; + const KeySet newFactorKeys = newFactors.keys(); + if (!this->empty()) { + KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end()); + this->removeTop(keyVector, &bn, orphans); + } + + // Add the removed top and the new factors + FactorGraphType factors; + factors += bn; + factors += newFactors; + + // Add the orphaned subtrees + for (const sharedClique& orphan : *orphans) + factors += boost::make_shared >(orphan); + + KeySet allDiscrete; + for (auto& factor : factors) { + for (auto& k : factor->discreteKeys_) { + allDiscrete.insert(k.first); + } + } + KeyVector newKeysDiscreteLast; + for (auto& k : newFactorKeys) { + if (!allDiscrete.exists(k)) { + newKeysDiscreteLast.push_back(k); + } + } + std::copy(allDiscrete.begin(), allDiscrete.end(), + std::back_inserter(newKeysDiscreteLast)); + + // KeyVector new + + // Get an ordering where the new keys are eliminated last + const VariableIndex index(factors); + const Ordering ordering = Ordering::ColamdConstrainedLast( + index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), + true); + + ordering.print("ORD"); + + // eliminate all factors (top, added, orphans) into a new Bayes tree + auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); + + // Re-add into Bayes tree data structures + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); +} + +void HybridISAM::update(const HybridFactorGraph& newFactors, + const HybridBayesTree::Eliminate& function) { + Cliques orphans; + this->updateInternal(newFactors, &orphans, function); +} + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h new file mode 100644 index 000000000..ac9e17e83 --- /dev/null +++ b/gtsam/hybrid/HybridISAM.h @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------------------- + + * 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 HybridISAM.h + * @date March 31, 2022 + * @author Fan Jiang + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +class GTSAM_EXPORT HybridISAM : public ISAM { + public: + typedef ISAM Base; + typedef HybridISAM This; + typedef boost::shared_ptr shared_ptr; + + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + HybridISAM(); + + /** Copy constructor */ + HybridISAM(const HybridBayesTree& bayesTree); + + void updateInternal( + const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, + const HybridBayesTree::Eliminate& function = + HybridBayesTree::EliminationTraitsType::DefaultEliminate); + + void update(const HybridFactorGraph& newFactors, + const HybridBayesTree::Eliminate& function = + HybridBayesTree::EliminationTraitsType::DefaultEliminate); + /// @} +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 22b40ad05..77e623a41 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -16,14 +16,14 @@ */ #include +#include #include #include +#include #include -#include "gtsam/hybrid/HybridFactorGraph.h" -#include "gtsam/inference/Key.h" - +// #undef NDEBUG namespace gtsam { // Instantiate base classes diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h new file mode 100644 index 000000000..0816db8df --- /dev/null +++ b/gtsam/hybrid/tests/Switching.h @@ -0,0 +1,68 @@ +#include +#include +#include +#include +#include +#include + +#pragma once + +using gtsam::symbol_shorthand::C; +using gtsam::symbol_shorthand::X; + +namespace gtsam { +inline HybridFactorGraph::shared_ptr makeSwitchingChain( + size_t n, std::function keyFunc = X, + std::function dKeyFunc = C) { + HybridFactorGraph hfg; + + hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); + + // keyFunc(1) to keyFunc(n+1) + for (size_t t = 1; t < n; t++) { + hfg.add(GaussianMixtureFactor::FromFactorList( + {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, + {boost::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), + I_3x3, Z_3x1), + boost::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), + I_3x3, Vector3::Ones())})); + + if (t > 1) { + hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, + "0 1 1 3")); + } + } + + return boost::make_shared(std::move(hfg)); +} + +inline std::pair, std::vector> makeBinaryOrdering( + std::vector &input) { + std::vector new_order; + std::vector levels(input.size()); + std::function::iterator, std::vector::iterator, + int)> + bsg = [&bsg, &new_order, &levels, &input]( + std::vector::iterator begin, + std::vector::iterator end, int lvl) { + if (std::distance(begin, end) > 1) { + std::vector::iterator pivot = + begin + std::distance(begin, end) / 2; + + new_order.push_back(*pivot); + levels[std::distance(input.begin(), pivot)] = lvl; + bsg(begin, pivot, lvl + 1); + bsg(pivot + 1, end, lvl + 1); + } else if (std::distance(begin, end) == 1) { + new_order.push_back(*begin); + levels[std::distance(input.begin(), begin)] = lvl; + } + }; + + bsg(input.begin(), input.end(), 0); + std::reverse(new_order.begin(), new_order.end()); + // std::reverse(levels.begin(), levels.end()); + return {new_order, levels}; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 703ec136e..f3664e619 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -17,6 +17,9 @@ #include #include +#include +#include +#include #include #include #include @@ -26,16 +29,23 @@ #include #include #include +#include #include +#include +#include +#include #include #include +#include #include #include +#include +#include +#include +#include -#include "gtsam/inference/DotWriter.h" -#include "gtsam/inference/Key.h" -#include "gtsam/inference/Ordering.h" +#include "Switching.h" using namespace boost::assign; @@ -43,7 +53,9 @@ using namespace std; using namespace gtsam; using gtsam::symbol_shorthand::C; +using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::X; +using gtsam::symbol_shorthand::Y; #define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED @@ -99,6 +111,29 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { EXPECT_LONGS_EQUAL(result.second->size(), 1); } +TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { + HybridFactorGraph hfg; + + 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)); + + DecisionTree dt( + C(1), boost::make_shared(X(1), I_3x3, Z_3x1), + boost::make_shared(X(1), I_3x3, Vector3::Ones())); + + hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); + + auto result = + hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)})); + + auto dc = result->at(2)->asDiscreteConditional(); + DiscreteValues dv; + dv[C(1)] = 0; + EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3); +} + TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) { std::cout << ">>>>>>>>>>>>>>\n"; @@ -268,27 +303,10 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { */ } -HybridFactorGraph::shared_ptr makeSwitchingChain(size_t n) { - HybridFactorGraph hfg; - - hfg.add(JacobianFactor(X(1), I_3x3, Z_3x1)); - - // X(1) to X(n+1) - for (size_t t = 1; t < n; t++) { - hfg.add(GaussianMixtureFactor::FromFactorList( - {X(t), X(t + 1)}, {{C(t), 2}}, - {boost::make_shared(X(t), I_3x3, X(t + 1), I_3x3, - Z_3x1), - boost::make_shared(X(t), I_3x3, X(t + 1), I_3x3, - Vector3::Ones())})); - } - - return boost::make_shared(std::move(hfg)); -} - // TODO(fan): make a graph like Varun's paper one TEST(HybridFactorGraph, Switching) { - auto hfg = makeSwitchingChain(9); + auto N = 12; + auto hfg = makeSwitchingChain(N); // X(5) will be the center, X(1-4), X(6-9) // X(3), X(7) @@ -297,20 +315,73 @@ TEST(HybridFactorGraph, Switching) { // C(5) will be the center, C(1-4), C(6-8) // C(3), C(7) // C(1), C(4), C(2), C(6), C(8) - auto ordering_full = - Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), X(5), - C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)}); + // auto ordering_full = + // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), + // X(5), + // C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)}); + KeyVector ordering; - GTSAM_PRINT(*hfg); + { + std::vector naturalX(N); + std::iota(naturalX.begin(), naturalX.end(), 1); + std::vector ordX; + std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX), + [](int x) { return X(x); }); + + KeyVector ndX; + std::vector lvls; + std::tie(ndX, lvls) = makeBinaryOrdering(ordX); + std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); + for (auto &l : lvls) { + l = -l; + } + std::copy(lvls.begin(), lvls.end(), + std::ostream_iterator(std::cout, ",")); + std::cout << "\n"; + } + { + std::vector naturalC(N - 1); + std::iota(naturalC.begin(), naturalC.end(), 1); + std::vector ordC; + std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), + [](int x) { return C(x); }); + KeyVector ndC; + std::vector lvls; + + // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); + std::tie(ndC, lvls) = makeBinaryOrdering(ordC); + std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); + std::copy(lvls.begin(), lvls.end(), + std::ostream_iterator(std::cout, ",")); + } + auto ordering_full = Ordering(ordering); + + // auto ordering_full = + // Ordering(); + + // for (int i = 1; i <= 9; i++) { + // ordering_full.push_back(X(i)); + // } + + // for (int i = 1; i < 9; i++) { + // ordering_full.push_back(C(i)); + // } + + // auto ordering_full = + // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), + // X(5), + // C(1), C(2), C(3), C(4), C(5), C(6), C(7), C(8)}); + + // GTSAM_PRINT(*hfg); GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; HybridFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); - GTSAM_PRINT(*hbt); + // GTSAM_PRINT(*hbt); - GTSAM_PRINT(*remaining); + // GTSAM_PRINT(*remaining); { DotWriter dw; @@ -323,7 +394,8 @@ TEST(HybridFactorGraph, Switching) { { DotWriter dw; - dw.positionHints['x'] = 1; + // dw.positionHints['c'] = 2; + // dw.positionHints['x'] = 1; std::cout << "\n"; std::cout << hfg->eliminateSequential(ordering_full) ->dot(DefaultKeyFormatter, dw); @@ -335,6 +407,189 @@ TEST(HybridFactorGraph, Switching) { is 1. expensive and 2. inexact. neverless it is doable. And I believe that we should do this. */ + hbt->marginalFactor(C(11))->print("HBT: "); +} + +// TODO(fan): make a graph like Varun's paper one +TEST(HybridFactorGraph, SwitchingISAM) { + auto N = 11; + auto hfg = makeSwitchingChain(N); + + // X(5) will be the center, X(1-4), X(6-9) + // X(3), X(7) + // X(2), X(8) + // X(1), X(4), X(6), X(9) + // C(5) will be the center, C(1-4), C(6-8) + // C(3), C(7) + // C(1), C(4), C(2), C(6), C(8) + // auto ordering_full = + // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), + // X(5), + // C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)}); + KeyVector ordering; + + { + std::vector naturalX(N); + std::iota(naturalX.begin(), naturalX.end(), 1); + std::vector ordX; + std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX), + [](int x) { return X(x); }); + + KeyVector ndX; + std::vector lvls; + std::tie(ndX, lvls) = makeBinaryOrdering(ordX); + std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); + for (auto &l : lvls) { + l = -l; + } + std::copy(lvls.begin(), lvls.end(), + std::ostream_iterator(std::cout, ",")); + std::cout << "\n"; + } + { + std::vector naturalC(N - 1); + std::iota(naturalC.begin(), naturalC.end(), 1); + std::vector ordC; + std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), + [](int x) { return C(x); }); + KeyVector ndC; + std::vector lvls; + + // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); + std::tie(ndC, lvls) = makeBinaryOrdering(ordC); + std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); + std::copy(lvls.begin(), lvls.end(), + std::ostream_iterator(std::cout, ",")); + } + auto ordering_full = Ordering(ordering); + + // GTSAM_PRINT(*hfg); + GTSAM_PRINT(ordering_full); + + HybridBayesTree::shared_ptr hbt; + HybridFactorGraph::shared_ptr remaining; + std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); + + // GTSAM_PRINT(*hbt); + + // GTSAM_PRINT(*remaining); + + { + DotWriter dw; + dw.positionHints['c'] = 2; + dw.positionHints['x'] = 1; + std::cout << hfg->dot(DefaultKeyFormatter, dw); + std::cout << "\n"; + hbt->dot(std::cout); + } + + { + DotWriter dw; + // dw.positionHints['c'] = 2; + // dw.positionHints['x'] = 1; + std::cout << "\n"; + std::cout << hfg->eliminateSequential(ordering_full) + ->dot(DefaultKeyFormatter, dw); + } + + auto new_fg = makeSwitchingChain(12); + auto isam = HybridISAM(*hbt); + + { + HybridFactorGraph factorGraph; + factorGraph.push_back(new_fg->at(new_fg->size() - 2)); + factorGraph.push_back(new_fg->at(new_fg->size() - 1)); + isam.update(factorGraph); + std::cout << isam.dot(); + isam.marginalFactor(C(11))->print(); + } +} + +TEST_DISABLED(HybridFactorGraph, SwitchingTwoVar) { + const int N = 7; + auto hfg = makeSwitchingChain(N, X); + hfg->push_back(*makeSwitchingChain(N, Y, D)); + + for (int t = 1; t <= N; t++) { + hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0))); + } + + KeyVector ordering; + + KeyVector naturalX(N); + std::iota(naturalX.begin(), naturalX.end(), 1); + KeyVector ordX; + for (size_t i = 1; i <= N; i++) { + ordX.emplace_back(X(i)); + ordX.emplace_back(Y(i)); + } + + // { + // KeyVector ndX; + // std::vector lvls; + // std::tie(ndX, lvls) = makeBinaryOrdering(naturalX); + // std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); + // std::copy(lvls.begin(), lvls.end(), + // std::ostream_iterator(std::cout, ",")); + // std::cout << "\n"; + + // for (size_t i = 0; i < N; i++) { + // ordX.emplace_back(X(ndX[i])); + // ordX.emplace_back(Y(ndX[i])); + // } + // } + + for (size_t i = 1; i <= N - 1; i++) { + ordX.emplace_back(C(i)); + } + for (size_t i = 1; i <= N - 1; i++) { + ordX.emplace_back(D(i)); + } + + { + DotWriter dw; + dw.positionHints['x'] = 1; + dw.positionHints['c'] = 0; + dw.positionHints['d'] = 3; + dw.positionHints['y'] = 2; + std::cout << hfg->dot(DefaultKeyFormatter, dw); + std::cout << "\n"; + } + + { + DotWriter dw; + dw.positionHints['y'] = 9; + // dw.positionHints['c'] = 0; + // dw.positionHints['d'] = 3; + dw.positionHints['x'] = 1; + std::cout << "\n"; + // std::cout << hfg->eliminateSequential(Ordering(ordX)) + // ->dot(DefaultKeyFormatter, dw); + hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout); + } + + Ordering ordering_partial; + for (size_t i = 1; i <= N; i++) { + ordering_partial.emplace_back(X(i)); + ordering_partial.emplace_back(Y(i)); + } + { + HybridBayesNet::shared_ptr hbn; + HybridFactorGraph::shared_ptr remaining; + std::tie(hbn, remaining) = + hfg->eliminatePartialSequential(ordering_partial); + + // remaining->print(); + { + DotWriter dw; + dw.positionHints['x'] = 1; + dw.positionHints['c'] = 0; + dw.positionHints['d'] = 3; + dw.positionHints['y'] = 2; + std::cout << remaining->dot(DefaultKeyFormatter, dw); + std::cout << "\n"; + } + } } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index a32b3ce22..1abc0a682 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -33,6 +33,7 @@ namespace gtsam { // Forward declarations template class FactorGraph; template class EliminatableClusterTree; + class HybridBayesTreeClique; /* ************************************************************************* */ /** clique statistics */ @@ -272,24 +273,25 @@ namespace gtsam { }; // BayesTree /* ************************************************************************* */ - template - class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType - { - public: + template + class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType { + public: typedef CLIQUE CliqueType; typedef typename CLIQUE::ConditionalType Base; boost::shared_ptr clique; - BayesTreeOrphanWrapper(const boost::shared_ptr& clique) : - clique(clique) - { - // Store parent keys in our base type factor so that eliminating those parent keys will pull - // this subtree into the elimination. - this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); + BayesTreeOrphanWrapper(const boost::shared_ptr& clique) + : clique(clique) { + // Store parent keys in our base type factor so that eliminating those + // parent keys will pull this subtree into the elimination. + this->keys_.assign(clique->conditional()->beginParents(), + clique->conditional()->endParents()); } - void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const override { + void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { clique->print(s + "stored clique", formatter); } }; From 2de3169976e876578c736e844605495dac6c7f30 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 19 Apr 2022 21:50:15 -0400 Subject: [PATCH 28/70] Fix compile --- gtsam/hybrid/tests/Switching.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 0816db8df..f29b8d9d5 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -36,9 +36,9 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain( return boost::make_shared(std::move(hfg)); } -inline std::pair, std::vector> makeBinaryOrdering( +inline std::pair> makeBinaryOrdering( std::vector &input) { - std::vector new_order; + KeyVector new_order; std::vector levels(input.size()); std::function::iterator, std::vector::iterator, int)> From 4e481ea1e111cd08ff0222f86c2ad2447da0fbd7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 May 2022 17:45:50 -0700 Subject: [PATCH 29/70] Fix the enable_if_t absence --- gtsam/hybrid/HybridBayesTree.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index dd8b7f022..626f2b10c 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -74,7 +74,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { template class BayesTreeOrphanWrapper< CLIQUE, - boost::enable_if_t::value> > + typename std::enable_if::value> > : public CLIQUE::ConditionalType { public: typedef CLIQUE CliqueType; From d012bcd4dc1fa53ad805d6c1dc50ff6e3ea2e6d8 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 May 2022 19:22:50 -0700 Subject: [PATCH 30/70] Remove most debug messages --- gtsam/hybrid/HybridJunctionTree.cpp | 10 +++++----- gtsam/hybrid/tests/testHybridConditional.cpp | 5 ++++- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 77e623a41..bf088c5aa 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -59,12 +59,12 @@ struct HybridConstructorTraversalData { myData.myJTNode = boost::make_shared(node->key, node->factors); parentData.myJTNode->addChild(myData.myJTNode); -#ifndef NDEBUG +#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG std::cout << "Getting discrete info: "; #endif for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys_) { -#ifndef NDEBUG +#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n"; #endif @@ -104,7 +104,7 @@ struct HybridConstructorTraversalData { boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); -#ifndef NDEBUG +#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG std::cout << "Symbolic: "; myConditional->print(); #endif @@ -136,7 +136,7 @@ struct HybridConstructorTraversalData { myData.discreteKeys.exists(myConditional->frontals()[0]); const bool theirType = myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); -#ifndef NDEBUG +#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG std::cout << "Type: " << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs " << DefaultKeyFormatter(childConditionals[i]->frontals()[0]) @@ -145,7 +145,7 @@ struct HybridConstructorTraversalData { if (myType == theirType) { // Increment number of frontal variables myNrFrontals += nrFrontals[i]; -#ifndef NDEBUG +#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG std::cout << "Merging "; childConditionals[i]->print(); #endif diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index f3664e619..3d96ebd6a 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -57,6 +57,7 @@ using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::Y; +#ifdef HYBRID_DEBUG #define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED #include // ::signal, ::raise @@ -68,6 +69,7 @@ void my_signal_handler(int signum) { std::cout << boost::stacktrace::stacktrace(); ::raise(SIGABRT); } +#endif /* ************************************************************************* */ TEST_DISABLED(HybridFactorGraph, creation) { @@ -594,9 +596,10 @@ TEST_DISABLED(HybridFactorGraph, SwitchingTwoVar) { /* ************************************************************************* */ int main() { +#ifdef HYBRID_DEBUG ::signal(SIGSEGV, &my_signal_handler); ::signal(SIGBUS, &my_signal_handler); - +#endif TestResult tr; return TestRegistry::runAllTests(tr); } From 2c4990b613833a8fd38346a008965a60aac28dda Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 21 May 2022 14:23:05 -0700 Subject: [PATCH 31/70] Address Varun's comments --- gtsam/hybrid/GaussianMixture.cpp | 11 +++++----- gtsam/hybrid/GaussianMixture.h | 7 ++++--- gtsam/hybrid/GaussianMixtureFactor.cpp | 2 +- gtsam/hybrid/GaussianMixtureFactor.h | 2 +- gtsam/hybrid/HybridFactor.cpp | 2 +- gtsam/hybrid/HybridFactorGraph.cpp | 4 ++-- gtsam/hybrid/HybridGaussianFactor.h | 2 +- gtsam/hybrid/tests/Switching.h | 20 ++++++++++++++++++- ...ditional.cpp => testHybridFactorGraph.cpp} | 2 +- 9 files changed, 36 insertions(+), 16 deletions(-) rename gtsam/hybrid/tests/{testHybridConditional.cpp => testHybridFactorGraph.cpp} (99%) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index bc674966c..66971b69f 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -50,7 +50,7 @@ GaussianMixture GaussianMixture::FromConditionalList( } /* *******************************************************************************/ -GaussianMixture::Sum GaussianMixture::addTo( +GaussianMixture::Sum GaussianMixture::add( const GaussianMixture::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { @@ -58,20 +58,21 @@ GaussianMixture::Sum GaussianMixture::addTo( result.push_back(graph2); return result; }; - const Sum wrapped = wrappedConditionals(); + const Sum wrapped = asGraph(); return sum.empty() ? wrapped : sum.apply(wrapped, add); } /* *******************************************************************************/ -GaussianMixture::Sum GaussianMixture::wrappedConditionals() const { - auto wrap = [](const GaussianFactor::shared_ptr &factor) { +GaussianMixture::Sum GaussianMixture::asGraph() const { + auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); return result; }; - return {conditionals_, wrap}; + return {conditionals_, lambda}; } +/* TODO(fan): this (for Testable) is not implemented! */ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { return false; } diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 4412e741c..4379ea1ca 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -13,6 +13,7 @@ * @file GaussianMixture.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang + * @author Varun Agrawal * @date Mar 12, 2022 */ @@ -55,10 +56,10 @@ class GaussianMixture : public HybridFactor, const Conditionals &conditionals(); /* *******************************************************************************/ - Sum addTo(const Sum &sum) const; + Sum add(const Sum &sum) const; /* *******************************************************************************/ - Sum wrappedConditionals() const; + Sum asGraph() const; static This FromConditionalList( const KeyVector &continuousFrontals, const KeyVector &continuousParents, @@ -71,4 +72,4 @@ class GaussianMixture : public HybridFactor, const std::string &s = "GaussianMixture\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 3963e675e..c85383322 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -62,7 +62,7 @@ const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() { } /* *******************************************************************************/ -GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo( +GaussianMixtureFactor::Sum GaussianMixtureFactor::add( const GaussianMixtureFactor::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 57a0cca03..1a3c582ae 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -56,7 +56,7 @@ class GaussianMixtureFactor : public HybridFactor { const std::vector &factors); /* *******************************************************************************/ - Sum addTo(const Sum &sum) const; + Sum add(const Sum &sum) const; /* *******************************************************************************/ Sum wrappedFactors() const; diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index a5ce8bd4e..a0c213a17 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -76,4 +76,4 @@ void HybridFactor::print( HybridFactor::~HybridFactor() = default; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index ee6a5dd82..5ec562dc0 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -286,12 +286,12 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { if (f->isHybrid_) { auto cgmf = boost::dynamic_pointer_cast(f); if (cgmf) { - sum = cgmf->addTo(sum); + sum = cgmf->add(sum); } auto gm = boost::dynamic_pointer_cast(f); if (gm) { - sum = gm->asMixture()->addTo(sum); + sum = gm->asMixture()->add(sum); } } else if (f->isContinuous_) { diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 86c87a0ec..4a7939cd4 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -24,7 +24,7 @@ namespace gtsam { /** - * A HybridGaussianFactor is a wrapper for GaussianFactor so that we do not have + * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have * a diamond inheritance. */ class HybridGaussianFactor : public HybridFactor { diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index f29b8d9d5..74d682372 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -1,3 +1,21 @@ +/* ---------------------------------------------------------------------------- + + * 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 Switching.h + * @date Mar 11, 2022 + * @author Varun Agrawal + * @author Fan Jiang + */ + #include #include #include @@ -65,4 +83,4 @@ inline std::pair> makeBinaryOrdering( return {new_order, levels}; } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp similarity index 99% rename from gtsam/hybrid/tests/testHybridConditional.cpp rename to gtsam/hybrid/tests/testHybridFactorGraph.cpp index 3d96ebd6a..d8ff617b0 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testHybridConditional.cpp + * @file testHybridFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ From 04593ccb005d5ec0a0505fb511e33da929c8a29c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 21 May 2022 15:04:43 -0700 Subject: [PATCH 32/70] Fix compile error --- gtsam/hybrid/tests/testHybridFactorGraph.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index d8ff617b0..79c16d21a 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -72,7 +72,7 @@ void my_signal_handler(int signum) { #endif /* ************************************************************************* */ -TEST_DISABLED(HybridFactorGraph, creation) { +TEST(HybridFactorGraph, creation) { HybridConditional test; HybridFactorGraph hfg; @@ -89,7 +89,7 @@ TEST_DISABLED(HybridFactorGraph, creation) { GTSAM_PRINT(clgc); } -TEST_DISABLED(HybridFactorGraph, eliminate) { +TEST(HybridFactorGraph, eliminate) { HybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -99,7 +99,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) { EXPECT_LONGS_EQUAL(result.first->size(), 1); } -TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { +TEST(HybridFactorGraph, eliminateMultifrontal) { HybridFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -107,7 +107,9 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); - auto result = hfg.eliminatePartialMultifrontal({X(0)}); + Ordering ordering; + ordering.push_back(X(0)); + auto result = hfg.eliminatePartialMultifrontal(ordering); EXPECT_LONGS_EQUAL(result.first->size(), 1); EXPECT_LONGS_EQUAL(result.second->size(), 1); @@ -136,7 +138,7 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3); } -TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) { +TEST(HybridFactorGraph, eliminateFullSequentialSimple) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -165,7 +167,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) { GTSAM_PRINT(*result); } -TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { +TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -199,7 +201,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { GTSAM_PRINT(*result->marginalFactor(C(2))); } -TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) { +TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -236,7 +238,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) { * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ -TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { +TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -507,7 +509,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { } } -TEST_DISABLED(HybridFactorGraph, SwitchingTwoVar) { +TEST(HybridFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); From 2ae2cb6dc375a579d3c0754f45bf2c1060b22468 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 22 May 2022 14:16:13 -0700 Subject: [PATCH 33/70] Don't crash anymore --- python/gtsam/preamble/base.h | 4 +++- python/gtsam/preamble/discrete.h | 1 - python/gtsam/preamble/hybrid.h | 2 +- python/gtsam/preamble/inference.h | 2 -- python/gtsam/specializations/inference.h | 1 + 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index 626b47ae4..5cf633e65 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -11,6 +11,8 @@ * mutations on Python side will not be reflected on C++. */ -PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(gtsam::IndexPairVector); + +PYBIND11_MAKE_OPAQUE(gtsam::IndexPairSet); PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/discrete.h b/python/gtsam/preamble/discrete.h index 608508c32..320e0ac71 100644 --- a/python/gtsam/preamble/discrete.h +++ b/python/gtsam/preamble/discrete.h @@ -13,4 +13,3 @@ #include -PYBIND11_MAKE_OPAQUE(gtsam::DiscreteKeys); diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index 56a07cfdd..5e5a71e48 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -11,4 +11,4 @@ * mutations on Python side will not be reflected on C++. */ -#include +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h index 4106c794a..d07a75f6f 100644 --- a/python/gtsam/preamble/inference.h +++ b/python/gtsam/preamble/inference.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -#include \ No newline at end of file diff --git a/python/gtsam/specializations/inference.h b/python/gtsam/specializations/inference.h index 22fe3beff..9e23444ea 100644 --- a/python/gtsam/specializations/inference.h +++ b/python/gtsam/specializations/inference.h @@ -11,3 +11,4 @@ * and saves one copy operation. */ +py::bind_map>(m_, "__MapCharDouble"); From b8299d7ed62b4dc6c4be9a98c5e02d3bdefdcbfb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 22 May 2022 17:11:27 -0700 Subject: [PATCH 34/70] Don't use Python dict method since it is not --- python/gtsam/tests/test_DiscreteBayesNet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index 10c5db612..ff2ba99d1 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -139,7 +139,7 @@ class TestDiscreteBayesNet(GtsamTestCase): # Make sure we can *update* position hints writer = gtsam.DotWriter() ph: dict = writer.positionHints - ph.update({'a': 2}) # hint at symbol position + ph['a'] = 2 # hint at symbol position writer.positionHints = ph # Check the output of dot From 74af969f68ce661fd296a24106878864fcedca57 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 22 May 2022 21:11:40 -0700 Subject: [PATCH 35/70] Trying to make MSVC happy --- gtsam/hybrid/HybridFactorGraph.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 5ec562dc0..f44ad898b 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include From b215d3a3779e31ba04cd80b085e580445d3585ae Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 22 May 2022 21:29:12 -0700 Subject: [PATCH 36/70] Address PR comments --- ...ure.cpp => GaussianMixtureConditional.cpp} | 27 ++++++++------- ...Mixture.h => GaussianMixtureConditional.h} | 34 +++++++++++++------ gtsam/hybrid/GaussianMixtureFactor.cpp | 4 +++ gtsam/hybrid/GaussianMixtureFactor.h | 2 -- gtsam/hybrid/HybridBayesNet.h | 2 -- gtsam/hybrid/HybridConditional.cpp | 2 +- gtsam/hybrid/HybridConditional.h | 10 +++--- gtsam/hybrid/HybridFactorGraph.cpp | 10 +++--- gtsam/hybrid/hybrid.i | 8 ++--- gtsam/hybrid/tests/testHybridFactorGraph.cpp | 6 ++-- 10 files changed, 61 insertions(+), 44 deletions(-) rename gtsam/hybrid/{GaussianMixture.cpp => GaussianMixtureConditional.cpp} (72%) rename gtsam/hybrid/{GaussianMixture.h => GaussianMixtureConditional.h} (70%) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp similarity index 72% rename from gtsam/hybrid/GaussianMixture.cpp rename to gtsam/hybrid/GaussianMixtureConditional.cpp index 66971b69f..5fc3b4f83 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixtureConditional.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixture.cpp + * @file GaussianMixtureConditional.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -20,38 +20,40 @@ #include #include -#include +#include #include #include namespace gtsam { -GaussianMixture::GaussianMixture( +GaussianMixtureConditional::GaussianMixtureConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const GaussianMixture::Conditionals &conditionals) + const GaussianMixtureConditional::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), discreteParents), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} -const GaussianMixture::Conditionals &GaussianMixture::conditionals() { +/* *******************************************************************************/ +const GaussianMixtureConditional::Conditionals &GaussianMixtureConditional::conditionals() { return conditionals_; } -GaussianMixture GaussianMixture::FromConditionalList( +/* *******************************************************************************/ +GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixture(continuousFrontals, continuousParents, discreteParents, + return GaussianMixtureConditional(continuousFrontals, continuousParents, discreteParents, dt); } /* *******************************************************************************/ -GaussianMixture::Sum GaussianMixture::add( - const GaussianMixture::Sum &sum) const { +GaussianMixtureConditional::Sum GaussianMixtureConditional::add( + const GaussianMixtureConditional::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -63,7 +65,7 @@ GaussianMixture::Sum GaussianMixture::add( } /* *******************************************************************************/ -GaussianMixture::Sum GaussianMixture::asGraph() const { +GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -73,11 +75,12 @@ GaussianMixture::Sum GaussianMixture::asGraph() const { } /* TODO(fan): this (for Testable) is not implemented! */ -bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { +bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const { return false; } -void GaussianMixture::print(const std::string &s, +/* *******************************************************************************/ +void GaussianMixtureConditional::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s << ": "; if (isContinuous_) std::cout << "Cont. "; diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixtureConditional.h similarity index 70% rename from gtsam/hybrid/GaussianMixture.h rename to gtsam/hybrid/GaussianMixtureConditional.h index 4379ea1ca..e0cf7c050 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixture.h + * @file GaussianMixtureConditional.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -25,13 +25,13 @@ #include namespace gtsam { -class GaussianMixture : public HybridFactor, - public Conditional { +class GaussianMixtureConditional : public HybridFactor, + public Conditional { public: - using This = GaussianMixture; - using shared_ptr = boost::shared_ptr; + using This = GaussianMixtureConditional; + using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using BaseConditional = Conditional; using Conditionals = DecisionTree; @@ -46,7 +46,7 @@ class GaussianMixture : public HybridFactor, * @param discreteParents the discrete parents. Will be placed last. * @param conditionals a decision tree of GaussianConditionals. */ - GaussianMixture(const KeyVector &continuousFrontals, + GaussianMixtureConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const Conditionals &conditionals); @@ -55,21 +55,35 @@ class GaussianMixture : public HybridFactor, const Conditionals &conditionals(); - /* *******************************************************************************/ + /** + * @brief Combine Decision Trees + */ Sum add(const Sum &sum) const; - /* *******************************************************************************/ + /** + * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. + */ Sum asGraph() const; + /** + * @brief Make a Gaussian Mixture from a list of Gaussian conditionals + * + * @param continuousFrontals The continuous frontal variables + * @param continuousParents The continuous parent variables + * @param discreteParents Discrete parents variables + * @param conditionals List of conditionals + */ static This FromConditionalList( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionals); + /* TODO: this is only a stub */ bool equals(const HybridFactor &lf, double tol = 1e-9) const override; + /* print utility */ void print( - const std::string &s = "GaussianMixture\n", + const std::string &s = "GaussianMixtureConditional\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index c85383322..65c5c7001 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -34,6 +34,7 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { return false; } +/* *******************************************************************************/ GaussianMixtureFactor GaussianMixtureFactor::FromFactorList( const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector &factorsList) { @@ -42,6 +43,8 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactorList( return GaussianMixtureFactor(continuousKeys, discreteKeys, dt); } + +/* *******************************************************************************/ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); @@ -57,6 +60,7 @@ void GaussianMixtureFactor::print(const std::string &s, }); } +/* *******************************************************************************/ const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() { return factors_; } diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 1a3c582ae..f0f55911a 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -55,10 +55,8 @@ class GaussianMixtureFactor : public HybridFactor { const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector &factors); - /* *******************************************************************************/ Sum add(const Sum &sum) const; - /* *******************************************************************************/ Sum wrappedFactors() const; bool equals(const HybridFactor &lf, double tol = 1e-9) const override; diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index d7e2f33af..4e411b781 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -20,8 +20,6 @@ #include #include -#include // TODO! - namespace gtsam { /** diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 48bee192c..73e7747c6 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -50,7 +50,7 @@ HybridConditional::HybridConditional( } HybridConditional::HybridConditional( - boost::shared_ptr gaussianMixture) + boost::shared_ptr gaussianMixture) : BaseFactor(KeyVector(gaussianMixture->keys().begin(), gaussianMixture->keys().begin() + gaussianMixture->nrContinuous), diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 5d7ee2351..76d5b4833 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ class HybridFactorGraph; * As a type-erased variant of: * - DiscreteConditional * - GaussianConditional - * - GaussianMixture + * - GaussianMixtureConditional * * The reason why this is important is that `Conditional` is a CRTP class. * CRTP is static polymorphism such that all CRTP classes, while bearing the @@ -93,11 +93,11 @@ class GTSAM_EXPORT HybridConditional HybridConditional(boost::shared_ptr discreteConditional); - HybridConditional(boost::shared_ptr gaussianMixture); + HybridConditional(boost::shared_ptr gaussianMixture); - GaussianMixture::shared_ptr asMixture() { + GaussianMixtureConditional::shared_ptr asMixture() { if (!isHybrid_) throw std::invalid_argument("Not a mixture"); - return boost::static_pointer_cast(inner); + return boost::static_pointer_cast(inner); } DiscreteConditional::shared_ptr asDiscreteConditional() { diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index f44ad898b..699e6d2c6 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -104,7 +104,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Because of all these reasons, we need to think very carefully about how to // implement the hybrid factors so that we do not get poor performance. // - // The first thing is how to represent the GaussianMixture. A very possible + // The first thing is how to represent the GaussianMixtureConditional. A very possible // scenario is that the incoming factors will have different levels of // discrete keys. For example, imagine we are going to eliminate the fragment: // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will @@ -358,11 +358,11 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { auto pair = unzip(eliminationResults); - const GaussianMixture::Conditionals &conditionals = pair.first; + const GaussianMixtureConditional::Conditionals &conditionals = pair.first; const GaussianMixtureFactor::Factors &separatorFactors = pair.second; - // Create the GaussianMixture from the conditionals - auto conditional = boost::make_shared( + // Create the GaussianMixtureConditional from the conditionals + auto conditional = boost::make_shared( frontalKeys, keysOfSeparator, discreteSeparator, conditionals); if (DEBUG) { diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 052575011..5a76aaf48 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor { gtsam::DefaultKeyFormatter) const; }; -#include -class GaussianMixture : gtsam::HybridFactor { - static GaussianMixture FromConditionalList( +#include +class GaussianMixtureConditional : gtsam::HybridFactor { + static GaussianMixtureConditional FromConditionalList( const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const std::vector& conditionalsList); - void print(string s = "GaussianMixture\n", + void print(string s = "GaussianMixtureConditional\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index 79c16d21a..4986cc2a7 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -79,8 +79,8 @@ TEST(HybridFactorGraph, creation) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GaussianMixture clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - GaussianMixture::Conditionals( + GaussianMixtureConditional clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), + GaussianMixtureConditional::Conditionals( C(0), boost::make_shared( X(0), Z_3x1, I_3x3, X(1), I_3x3), From e36583e6d530080fc6062331a6a13629f21bce79 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 May 2022 20:37:57 -0400 Subject: [PATCH 37/70] include missing headers for msvc and fix warning --- gtsam/hybrid/HybridConditional.cpp | 2 +- gtsam/hybrid/HybridFactorGraph.cpp | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 73e7747c6..5b3c9d7b4 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -66,7 +66,7 @@ void HybridConditional::print(const std::string &s, if (isDiscrete_) std::cout << "Disc. "; if (isHybrid_) std::cout << "Hybr. "; std::cout << "P("; - int index = 0; + size_t index = 0; const size_t N = keys().size(); const size_t contN = N - discreteKeys_.size(); while (index < N) { diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 699e6d2c6..b72f50e8d 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -34,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -104,9 +106,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Because of all these reasons, we need to think very carefully about how to // implement the hybrid factors so that we do not get poor performance. // - // The first thing is how to represent the GaussianMixtureConditional. A very possible - // scenario is that the incoming factors will have different levels of - // discrete keys. For example, imagine we are going to eliminate the fragment: + // The first thing is how to represent the GaussianMixtureConditional. A very + // possible scenario is that the incoming factors will have different levels + // of discrete keys. For example, imagine we are going to eliminate the + // fragment: // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will // need to know how to retrieve the corresponding continuous densities for the // assi- -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). From e325cd1c4bce9f58322d13d8952f639396b62a3d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 24 May 2022 00:14:20 -0400 Subject: [PATCH 38/70] include GaussianJunctionTree --- gtsam/hybrid/HybridFactorGraph.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index b72f50e8d..cb961b807 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include From eb074e7424c8747727e7af04ff535e7c50e1a46e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 24 May 2022 11:36:12 -0400 Subject: [PATCH 39/70] run formatting and rename wrappedFactors to asGaussianFactorGraphTree --- gtsam/hybrid/GaussianMixtureConditional.cpp | 23 +++++---- gtsam/hybrid/GaussianMixtureConditional.h | 43 +++++++++-------- gtsam/hybrid/GaussianMixtureFactor.cpp | 11 +++-- gtsam/hybrid/GaussianMixtureFactor.h | 11 ++++- gtsam/hybrid/HybridBayesTree.cpp | 4 +- gtsam/hybrid/HybridBayesTree.h | 4 +- gtsam/hybrid/HybridConditional.h | 3 +- gtsam/hybrid/HybridDiscreteFactor.cpp | 5 +- gtsam/hybrid/HybridDiscreteFactor.h | 5 +- gtsam/hybrid/HybridEliminationTree.cpp | 15 +++--- gtsam/hybrid/HybridFactor.cpp | 5 +- gtsam/hybrid/HybridFactorGraph.cpp | 4 +- gtsam/hybrid/HybridJunctionTree.h | 52 +++++++++++---------- 13 files changed, 103 insertions(+), 82 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp index 5fc3b4f83..f0f3e8359 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixtureConditional.cpp @@ -36,7 +36,8 @@ GaussianMixtureConditional::GaussianMixtureConditional( conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixtureConditional::Conditionals &GaussianMixtureConditional::conditionals() { +const GaussianMixtureConditional::Conditionals & +GaussianMixtureConditional::conditionals() { return conditionals_; } @@ -47,8 +48,8 @@ GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList( const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixtureConditional(continuousFrontals, continuousParents, discreteParents, - dt); + return GaussianMixtureConditional(continuousFrontals, continuousParents, + discreteParents, dt); } /* *******************************************************************************/ @@ -60,12 +61,13 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add( result.push_back(graph2); return result; }; - const Sum wrapped = asGraph(); - return sum.empty() ? wrapped : sum.apply(wrapped, add); + const Sum tree = asGaussianFactorGraphTree(); + return sum.empty() ? tree : sum.apply(tree, add); } /* *******************************************************************************/ -GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const { +GaussianMixtureConditional::Sum +GaussianMixtureConditional::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -74,14 +76,15 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const { return {conditionals_, lambda}; } -/* TODO(fan): this (for Testable) is not implemented! */ -bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const { - return false; +/* *******************************************************************************/ +bool GaussianMixtureConditional::equals(const HybridFactor &lf, + double tol) const { + return BaseFactor::equals(lf, tol); } /* *******************************************************************************/ void GaussianMixtureConditional::print(const std::string &s, - const KeyFormatter &formatter) const { + const KeyFormatter &formatter) const { std::cout << s << ": "; if (isContinuous_) std::cout << "Cont. "; if (isDiscrete_) std::cout << "Disc. "; diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index e0cf7c050..3c74115f8 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -25,8 +25,9 @@ #include namespace gtsam { -class GaussianMixtureConditional : public HybridFactor, - public Conditional { +class GaussianMixtureConditional + : public HybridFactor, + public Conditional { public: using This = GaussianMixtureConditional; using shared_ptr = boost::shared_ptr; @@ -47,9 +48,9 @@ class GaussianMixtureConditional : public HybridFactor, * @param conditionals a decision tree of GaussianConditionals. */ GaussianMixtureConditional(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const Conditionals &conditionals); + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const Conditionals &conditionals); using Sum = DecisionTree; @@ -60,30 +61,32 @@ class GaussianMixtureConditional : public HybridFactor, */ Sum add(const Sum &sum) const; - /** - * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. - */ - Sum asGraph() const; - - /** - * @brief Make a Gaussian Mixture from a list of Gaussian conditionals - * - * @param continuousFrontals The continuous frontal variables - * @param continuousParents The continuous parent variables - * @param discreteParents Discrete parents variables - * @param conditionals List of conditionals - */ + /** + * @brief Make a Gaussian Mixture from a list of Gaussian conditionals + * + * @param continuousFrontals The continuous frontal variables + * @param continuousParents The continuous parent variables + * @param discreteParents Discrete parents variables + * @param conditionals List of conditionals + */ static This FromConditionalList( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionals); - /* TODO: this is only a stub */ + /// Test equality with base HybridFactor bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - /* print utility */ + /* print utility */ void print( const std::string &s = "GaussianMixtureConditional\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + + protected: + /** + * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. + */ + Sum asGaussianFactorGraphTree() const; }; + } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 65c5c7001..3df274db3 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -26,10 +26,13 @@ namespace gtsam { +/* *******************************************************************************/ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) : Base(continuousKeys, discreteKeys), factors_(factors) {} + +/* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { return false; } @@ -43,7 +46,6 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactorList( return GaussianMixtureFactor(continuousKeys, discreteKeys, dt); } - /* *******************************************************************************/ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { @@ -74,12 +76,13 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::add( result.push_back(graph2); return result; }; - const Sum wrapped = wrappedFactors(); - return sum.empty() ? wrapped : sum.apply(wrapped, add); + const Sum tree = asGaussianFactorGraphTree(); + return sum.empty() ? tree : sum.apply(tree, add); } /* *******************************************************************************/ -GaussianMixtureFactor::Sum GaussianMixtureFactor::wrappedFactors() const { +GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree() + const { auto wrap = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index f0f55911a..c6389c540 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -57,13 +57,20 @@ class GaussianMixtureFactor : public HybridFactor { Sum add(const Sum &sum) const; - Sum wrappedFactors() const; - bool equals(const HybridFactor &lf, double tol = 1e-9) const override; void print( const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + + protected: + /** + * @brief Helper function to return factors and functional to create a + * DecisionTree of Gaussian Factor Graphs. + * + * @return Sum (DecisionTree +#include +#include #include #include -#include -#include namespace gtsam { diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 626f2b10c..74bd234d8 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -73,8 +73,8 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /* This does special stuff for the hybrid case */ template class BayesTreeOrphanWrapper< - CLIQUE, - typename std::enable_if::value> > + CLIQUE, typename std::enable_if< + boost::is_same::value> > : public CLIQUE::ConditionalType { public: typedef CLIQUE CliqueType; diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 76d5b4833..3bc25414e 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -93,7 +93,8 @@ class GTSAM_EXPORT HybridConditional HybridConditional(boost::shared_ptr discreteConditional); - HybridConditional(boost::shared_ptr gaussianMixture); + HybridConditional( + boost::shared_ptr gaussianMixture); GaussianMixtureConditional::shared_ptr asMixture() { if (!isHybrid_) throw std::invalid_argument("Not a mixture"); diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index be5659f04..54b193196 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -19,15 +19,16 @@ #include #include + #include "gtsam/discrete/DecisionTreeFactor.h" namespace gtsam { // TODO(fan): THIS IS VERY VERY DIRTY! We need to get DiscreteFactor right! HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) - : Base(boost::dynamic_pointer_cast(other)->discreteKeys()) { + : Base(boost::dynamic_pointer_cast(other) + ->discreteKeys()) { inner = other; - } HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 809510eac..0f731f8b5 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -24,8 +24,9 @@ namespace gtsam { /** - * A HybridDiscreteFactor is a wrapper for DiscreteFactor, so we hide the - * implementation of DiscreteFactor, and thus avoiding diamond inheritance. + * A HybridDiscreteFactor is a thin container for DiscreteFactor, which allows + * us to hide the implementation of DiscreteFactor and thus avoid diamond + * inheritance. */ class HybridDiscreteFactor : public HybridFactor { public: diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index ff106095a..ecac96724 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -15,8 +15,8 @@ * @author Fan Jiang */ -#include #include +#include namespace gtsam { @@ -26,18 +26,17 @@ template class EliminationTree; /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( const HybridFactorGraph& factorGraph, const VariableIndex& structure, - const Ordering& order) : - Base(factorGraph, structure, order) {} + const Ordering& order) + : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const HybridFactorGraph& factorGraph, const Ordering& order) : - Base(factorGraph, order) {} + const HybridFactorGraph& factorGraph, const Ordering& order) + : Base(factorGraph, order) {} /* ************************************************************************* */ -bool HybridEliminationTree::equals(const This& other, double tol) const -{ +bool HybridEliminationTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index a0c213a17..1e9955c58 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -64,9 +64,8 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) isDiscrete_(true), discreteKeys_(discreteKeys) {} -void HybridFactor::print( - const std::string &s, - const KeyFormatter &formatter) const { +void HybridFactor::print(const std::string &s, + const KeyFormatter &formatter) const { std::cout << s; if (isContinuous_) std::cout << "Cont. "; if (isDiscrete_) std::cout << "Disc. "; diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index cb961b807..954ee57a6 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -229,8 +229,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { if (p) { gfg.push_back(boost::static_pointer_cast(p)); } else { - // It is an orphan wrapper - if (DEBUG) std::cout << "Got an orphan wrapper conditional\n"; + // It is an orphan wrapped conditional + if (DEBUG) std::cout << "Got an orphan conditional\n"; } } } diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index 1901e7007..824fa4f85 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -17,8 +17,8 @@ #pragma once -#include #include +#include #include namespace gtsam { @@ -27,41 +27,45 @@ namespace gtsam { class HybridEliminationTree; /** - * An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, - * with the additional property that it represents the clique tree associated with a Bayes net. + * An EliminatableClusterTree, i.e., a set of variable clusters with factors, + * arranged in a tree, with the additional property that it represents the + * clique tree associated with a Bayes net. * * In GTSAM a junction tree is an intermediate data structure in multifrontal * variable elimination. Each node is a cluster of factors, along with a - * clique of variables that are eliminated all at once. In detail, every node k represents - * a clique (maximal fully connected subset) of an associated chordal graph, such as a - * chordal Bayes net resulting from elimination. + * clique of variables that are eliminated all at once. In detail, every node k + * represents a clique (maximal fully connected subset) of an associated chordal + * graph, such as a chordal Bayes net resulting from elimination. * - * The difference with the BayesTree is that a JunctionTree stores factors, whereas a - * BayesTree stores conditionals, that are the product of eliminating the factors in the - * corresponding JunctionTree cliques. + * The difference with the BayesTree is that a JunctionTree stores factors, + * whereas a BayesTree stores conditionals, that are the product of eliminating + * the factors in the corresponding JunctionTree cliques. * - * The tree structure and elimination method are exactly analogous to the EliminationTree, - * except that in the JunctionTree, at each node multiple variables are eliminated at a time. + * The tree structure and elimination method are exactly analogous to the + * EliminationTree, except that in the JunctionTree, at each node multiple + * variables are eliminated at a time. * * \addtogroup Multifrontal * \nosubgrouping */ -class GTSAM_EXPORT HybridJunctionTree : - public JunctionTree { +class GTSAM_EXPORT HybridJunctionTree + : public JunctionTree { public: - typedef JunctionTree Base; ///< Base class - typedef HybridJunctionTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef JunctionTree + Base; ///< Base class + typedef HybridJunctionTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** - * Build the elimination tree of a factor graph using precomputed column structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ + * Build the elimination tree of a factor graph using precomputed column + * structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is + * not precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ HybridJunctionTree(const HybridEliminationTree& eliminationTree); }; -} +} // namespace gtsam From 3f239c28bef65e392a293bb9826878f4cf223186 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 24 May 2022 11:48:59 -0400 Subject: [PATCH 40/70] fix equality checks --- gtsam/hybrid/GaussianMixtureFactor.cpp | 2 +- gtsam/hybrid/HybridConditional.cpp | 2 +- gtsam/hybrid/HybridFactor.cpp | 4 ++++ gtsam/hybrid/HybridFactor.h | 2 +- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 3df274db3..edf94d040 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -34,7 +34,7 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { - return false; + return Base::equals(lf, tol); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 5b3c9d7b4..ea83c5f86 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -89,7 +89,7 @@ void HybridConditional::print(const std::string &s, } bool HybridConditional::equals(const HybridFactor &other, double tol) const { - return false; + return BaseFactor::equals(other, tol); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 1e9955c58..7a233bb1b 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -64,6 +64,10 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) isDiscrete_(true), discreteKeys_(discreteKeys) {} +bool HybridFactor::equals(const HybridFactor &lf, double tol) const { + return Base::equals(lf, tol); +} + void HybridFactor::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 3d5bd7b21..c6e4a5ffa 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -85,7 +85,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { /// @{ /// equals - virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0; + virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const; /// print void print( From c3a92a4705642f8708b610d4a5f9e81aba1ddcb9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 15:12:19 -0400 Subject: [PATCH 41/70] Hybrid and Gaussian Mixture conditional docs and some refactor --- gtsam/hybrid/GaussianMixtureConditional.cpp | 10 +-- gtsam/hybrid/GaussianMixtureConditional.h | 56 ++++++++++++----- gtsam/hybrid/HybridConditional.cpp | 24 ++++--- gtsam/hybrid/HybridConditional.h | 69 ++++++++++++++++----- 4 files changed, 116 insertions(+), 43 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp index f0f3e8359..68c3f505e 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixtureConditional.cpp @@ -42,7 +42,7 @@ GaussianMixtureConditional::conditionals() { } /* *******************************************************************************/ -GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList( +GaussianMixtureConditional GaussianMixtureConditional::FromConditionals( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionalsList) { @@ -86,12 +86,12 @@ bool GaussianMixtureConditional::equals(const HybridFactor &lf, void GaussianMixtureConditional::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s << ": "; - if (isContinuous_) std::cout << "Cont. "; - if (isDiscrete_) std::cout << "Disc. "; - if (isHybrid_) std::cout << "Hybr. "; + if (isContinuous()) std::cout << "Cont. "; + if (isDiscrete()) std::cout << "Disc. "; + if (isHybrid()) std::cout << "Hybr. "; BaseConditional::print("", formatter); std::cout << "Discrete Keys = "; - for (auto &dk : discreteKeys_) { + for (auto &dk : discreteKeys()) { std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } std::cout << "\n"; diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index 3c74115f8..d12fa09d7 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -25,6 +25,14 @@ #include namespace gtsam { + +/** + * @brief A conditional of gaussian mixtures indexed by discrete variables. + * + * Represents the conditional density P(X | M, Z) where X is a continuous random + * variable, M is the discrete variable and Z is the set of measurements. + * + */ class GaussianMixtureConditional : public HybridFactor, public Conditional { @@ -34,13 +42,28 @@ class GaussianMixtureConditional using BaseFactor = HybridFactor; using BaseConditional = Conditional; + /// Alias for DecisionTree of GaussianFactorGraphs + using Sum = DecisionTree; + + /// typedef for Decision Tree of Gaussian Conditionals using Conditionals = DecisionTree; + private: Conditionals conditionals_; - public: /** - * @brief Construct a new Gaussian Mixture object + * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. + */ + Sum asGaussianFactorGraphTree() const; + + public: + /// @name Constructors + /// @{ + + /// Defaut constructor, mainly for serialization. + GaussianMixtureConditional() = default; + /** + * @brief Construct a new GaussianMixtureConditional object * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. @@ -52,15 +75,6 @@ class GaussianMixtureConditional const DiscreteKeys &discreteParents, const Conditionals &conditionals); - using Sum = DecisionTree; - - const Conditionals &conditionals(); - - /** - * @brief Combine Decision Trees - */ - Sum add(const Sum &sum) const; - /** * @brief Make a Gaussian Mixture from a list of Gaussian conditionals * @@ -69,11 +83,15 @@ class GaussianMixtureConditional * @param discreteParents Discrete parents variables * @param conditionals List of conditionals */ - static This FromConditionalList( + static This FromConditionals( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionals); + /// @} + /// @name Testable + /// @{ + /// Test equality with base HybridFactor bool equals(const HybridFactor &lf, double tol = 1e-9) const override; @@ -82,11 +100,19 @@ class GaussianMixtureConditional const std::string &s = "GaussianMixtureConditional\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; - protected: + /// @} + + /// Getter for the underlying Conditionals DecisionTree + const Conditionals &conditionals(); + /** - * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. + * @brief Merge the Gaussian Factor Graphs in `this` and `sum` while + * maintaining the decision tree structure. + * + * @param sum Decision Tree of Gaussian Factor Graphs + * @return Sum */ - Sum asGaussianFactorGraphTree() const; + Sum add(const Sum &sum) const; }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index ea83c5f86..e70d100c3 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -22,6 +22,7 @@ namespace gtsam { +/* ************************************************************************ */ HybridConditional::HybridConditional(const KeyVector &continuousFrontals, const DiscreteKeys &discreteFrontals, const KeyVector &continuousParents, @@ -35,36 +36,40 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals, {discreteParents.begin(), discreteParents.end()}), continuousFrontals.size() + discreteFrontals.size()) {} +/* ************************************************************************ */ HybridConditional::HybridConditional( boost::shared_ptr continuousConditional) : HybridConditional(continuousConditional->keys(), {}, continuousConditional->nrFrontals()) { - inner = continuousConditional; + inner_ = continuousConditional; } +/* ************************************************************************ */ HybridConditional::HybridConditional( boost::shared_ptr discreteConditional) : HybridConditional({}, discreteConditional->discreteKeys(), discreteConditional->nrFrontals()) { - inner = discreteConditional; + inner_ = discreteConditional; } +/* ************************************************************************ */ HybridConditional::HybridConditional( boost::shared_ptr gaussianMixture) : BaseFactor(KeyVector(gaussianMixture->keys().begin(), gaussianMixture->keys().begin() + - gaussianMixture->nrContinuous), - gaussianMixture->discreteKeys_), + gaussianMixture->nrContinuous()), + gaussianMixture->discreteKeys()), BaseConditional(gaussianMixture->nrFrontals()) { - inner = gaussianMixture; + inner_ = gaussianMixture; } +/* ************************************************************************ */ void HybridConditional::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; - if (isContinuous_) std::cout << "Cont. "; - if (isDiscrete_) std::cout << "Disc. "; - if (isHybrid_) std::cout << "Hybr. "; + if (isContinuous()) std::cout << "Cont. "; + if (isDiscrete()) std::cout << "Disc. "; + if (isHybrid()) std::cout << "Hybr. "; std::cout << "P("; size_t index = 0; const size_t N = keys().size(); @@ -85,9 +90,10 @@ void HybridConditional::print(const std::string &s, index++; } std::cout << ")\n"; - if (inner) inner->print("", formatter); + if (inner_) inner_->print("", formatter); } +/* ************************************************************************ */ bool HybridConditional::equals(const HybridFactor &other, double tol) const { return BaseFactor::equals(other, tol); } diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 3bc25414e..b942773cb 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -54,8 +54,8 @@ class HybridFactorGraph; * having diamond inheritances, and neutralized the need to change other * components of GTSAM to make hybrid elimination work. * - * A great reference to the type-erasure pattern is Edurado Madrid's CppCon - * talk. + * A great reference to the type-erasure pattern is Eduaado Madrid's CppCon + * talk (https://www.youtube.com/watch?v=s082Qmd_nHs). */ class GTSAM_EXPORT HybridConditional : public HybridFactor, @@ -70,7 +70,7 @@ class GTSAM_EXPORT HybridConditional protected: // Type-erased pointer to the inner type - boost::shared_ptr inner; + boost::shared_ptr inner_; public: /// @name Standard Constructors @@ -79,35 +79,77 @@ class GTSAM_EXPORT HybridConditional /// Default constructor needed for serialization. HybridConditional() = default; + /** + * @brief Construct a new Hybrid Conditional object + * + * @param continuousKeys Vector of keys for continuous variables. + * @param discreteKeys Keys and cardinalities for discrete variables. + * @param nFrontals The number of frontal variables in the conditional. + */ HybridConditional(const KeyVector& continuousKeys, const DiscreteKeys& discreteKeys, size_t nFrontals) : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {} + /** + * @brief Construct a new Hybrid Conditional object + * + * @param continuousFrontals Vector of keys for continuous variables. + * @param discreteFrontals Keys and cardinalities for discrete variables. + * @param continuousParents Vector of keys for parent continuous variables. + * @param discreteParents Keys and cardinalities for parent discrete + * variables. + */ HybridConditional(const KeyVector& continuousFrontals, const DiscreteKeys& discreteFrontals, const KeyVector& continuousParents, const DiscreteKeys& discreteParents); + /** + * @brief Construct a new Hybrid Conditional object + * + * @param continuousConditional Conditional used to create the + * HybridConditional. + */ HybridConditional( boost::shared_ptr continuousConditional); + /** + * @brief Construct a new Hybrid Conditional object + * + * @param discreteConditional Conditional used to create the + * HybridConditional. + */ HybridConditional(boost::shared_ptr discreteConditional); + /** + * @brief Construct a new Hybrid Conditional object + * + * @param gaussianMixture Gaussian Mixture Conditional used to create the + * HybridConditional. + */ HybridConditional( boost::shared_ptr gaussianMixture); + /** + * @brief Return HybridConditional as a GaussianMixtureConditional + * + * @return GaussianMixtureConditional::shared_ptr + */ GaussianMixtureConditional::shared_ptr asMixture() { - if (!isHybrid_) throw std::invalid_argument("Not a mixture"); - return boost::static_pointer_cast(inner); + if (!isHybrid()) throw std::invalid_argument("Not a mixture"); + return boost::static_pointer_cast(inner_); } + /** + * @brief Return conditional as a DiscreteConditional + * + * @return DiscreteConditional::shared_ptr + */ DiscreteConditional::shared_ptr asDiscreteConditional() { - if (!isDiscrete_) throw std::invalid_argument("Not a discrete conditional"); - return boost::static_pointer_cast(inner); + if (!isDiscrete()) throw std::invalid_argument("Not a discrete conditional"); + return boost::static_pointer_cast(inner_); } - boost::shared_ptr getInner() { return inner; } - /// @} /// @name Testable /// @{ @@ -122,11 +164,10 @@ class GTSAM_EXPORT HybridConditional /// @} - friend std::pair // - EliminateHybrid(const HybridFactorGraph& factors, - const Ordering& frontalKeys); -}; -// DiscreteConditional + /// Get the type-erased pointer to the inner type + boost::shared_ptr inner() { return inner_; } + +}; // DiscreteConditional // traits template <> From b3cab1bd4ec44e416bfb2e58a900117fb6150ba8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 15:13:39 -0400 Subject: [PATCH 42/70] GaussianMixtureFactor docs --- gtsam/hybrid/GaussianMixtureFactor.cpp | 6 +-- gtsam/hybrid/GaussianMixtureFactor.h | 64 ++++++++++++++++++++------ 2 files changed, 54 insertions(+), 16 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index edf94d040..589e5c660 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -38,10 +38,10 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { } /* *******************************************************************************/ -GaussianMixtureFactor GaussianMixtureFactor::FromFactorList( +GaussianMixtureFactor GaussianMixtureFactor::FromFactors( const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const std::vector &factorsList) { - Factors dt(discreteKeys, factorsList); + const std::vector &factors) { + Factors dt(discreteKeys, factors); return GaussianMixtureFactor(continuousKeys, discreteKeys, dt); } diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index c6389c540..b2fbe4aef 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -11,7 +11,7 @@ /** * @file GaussianMixtureFactor.h - * @brief A set of Gaussian factors indexed by a set of discrete keys. + * @brief A factor that is a function of discrete and continuous variables. * @author Fan Jiang * @author Varun Agrawal * @author Frank Dellaert @@ -29,48 +29,86 @@ namespace gtsam { class GaussianFactorGraph; -typedef std::vector GaussianFactorVector; +using GaussianFactorVector = std::vector; +/** + * @brief A linear factor that is a function of both discrete and continuous + * variables, i.e. P(X, M | Z) where X is the set of continuous variables, M is + * the set of discrete variables and Z is the measurement set. + * + * Represents the underlying Gaussian Mixture as a Decision Tree, where the set + * of discrete variables indexes to the continuous gaussian distribution. + * + */ class GaussianMixtureFactor : public HybridFactor { public: using Base = HybridFactor; using This = GaussianMixtureFactor; using shared_ptr = boost::shared_ptr; + using Sum = DecisionTree; + + /// typedef for Decision Tree of Gaussian Factors using Factors = DecisionTree; + private: Factors factors_; + /** + * @brief Helper function to return factors and functional to create a + * DecisionTree of Gaussian Factor Graphs. + * + * @return Sum (DecisionTree) + */ + Sum asGaussianFactorGraphTree() const; + + public: + /// @name Constructors + /// @{ + + /// Default constructor, mainly for serialization. GaussianMixtureFactor() = default; + /** + * @brief Construct a new Gaussian Mixture Factor object. + * + * @param continuousKeys A vector of keys representing continuous variables. + * @param discreteKeys A vector of keys representing discrete variables and + * their cardinalities. + * @param factors The decision tree of Gaussian Factors stored as the mixture + * density. + */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors); - using Sum = DecisionTree; - - const Factors &factors(); - - static This FromFactorList( + static This FromFactors( const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector &factors); - Sum add(const Sum &sum) const; + /// @} + /// @name Testable + /// @{ bool equals(const HybridFactor &lf, double tol = 1e-9) const override; void print( const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + /// @} + + /// Getter for the underlying Gaussian Factor Decision Tree. + const Factors &factors(); - protected: /** - * @brief Helper function to return factors and functional to create a - * DecisionTree of Gaussian Factor Graphs. + * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while + * maintaining the original tree structure. * - * @return Sum (DecisionTree Date: Fri, 27 May 2022 15:15:34 -0400 Subject: [PATCH 43/70] Hybrid factor docs and minor refactor --- gtsam/hybrid/HybridFactor.cpp | 16 ++++++---- gtsam/hybrid/HybridFactor.h | 45 ++++++++++++++++----------- gtsam/hybrid/HybridGaussianFactor.cpp | 6 ++-- gtsam/hybrid/HybridGaussianFactor.h | 7 +++-- 4 files changed, 45 insertions(+), 29 deletions(-) diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 7a233bb1b..9358c473d 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -19,6 +19,7 @@ namespace gtsam { +/* ************************************************************************ */ KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) { KeyVector allKeys; @@ -30,6 +31,7 @@ KeyVector CollectKeys(const KeyVector &continuousKeys, return allKeys; } +/* ************************************************************************ */ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { KeyVector allKeys; std::copy(keys1.begin(), keys1.end(), std::back_inserter(allKeys)); @@ -37,6 +39,7 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { return allKeys; } +/* ************************************************************************ */ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) { DiscreteKeys allKeys; @@ -45,29 +48,32 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, return allKeys; } -HybridFactor::HybridFactor() = default; - +/* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &keys) - : Base(keys), isContinuous_(true), nrContinuous(keys.size()) {} + : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {} +/* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) : Base(CollectKeys(continuousKeys, discreteKeys)), isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)), isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), - nrContinuous(continuousKeys.size()), + nrContinuous_(continuousKeys.size()), discreteKeys_(discreteKeys) {} +/* ************************************************************************ */ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true), discreteKeys_(discreteKeys) {} +/* ************************************************************************ */ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { return Base::equals(lf, tol); } +/* ************************************************************************ */ void HybridFactor::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; @@ -77,6 +83,4 @@ void HybridFactor::print(const std::string &s, this->printKeys("", formatter); } -HybridFactor::~HybridFactor() = default; - } // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index c6e4a5ffa..da103da43 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -41,6 +41,16 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, * - GaussianMixture */ class GTSAM_EXPORT HybridFactor : public Factor { + private: + bool isDiscrete_ = false; + bool isContinuous_ = false; + bool isHybrid_ = false; + + size_t nrContinuous_ = 0; + + protected: + DiscreteKeys discreteKeys_; + public: // typedefs needed to play nice with gtsam typedef HybridFactor This; ///< This class @@ -48,27 +58,11 @@ class GTSAM_EXPORT HybridFactor : public Factor { shared_ptr; ///< shared_ptr to this class typedef Factor Base; ///< Our base class - bool isDiscrete_ = false; - bool isContinuous_ = false; - bool isHybrid_ = false; - - size_t nrContinuous = 0; - - DiscreteKeys discreteKeys_; - - public: /// @name Standard Constructors /// @{ /** Default constructor creates empty factor */ - HybridFactor(); - - /** Construct from container of keys. This constructor is used internally - * from derived factor - * constructors, either from a container of keys or from a - * boost::assign::list_of. */ - // template - // HybridFactor(const CONTAINER &keys) : Base(keys) {} + HybridFactor() = default; explicit HybridFactor(const KeyVector &keys); @@ -78,7 +72,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { explicit HybridFactor(const DiscreteKeys &discreteKeys); /// Virtual destructor - virtual ~HybridFactor(); + virtual ~HybridFactor() = default; /// @} /// @name Testable @@ -96,6 +90,21 @@ class GTSAM_EXPORT HybridFactor : public Factor { /// @name Standard Interface /// @{ + /// True if this is a factor of discrete variables only. + bool isDiscrete() const { return isDiscrete_; } + + /// True if this is a factor of continuous variables only. + bool isContinuous() const { return isContinuous_; } + + /// True is this is a Discrete-Continuous factor. + bool isHybrid() const { return isHybrid_; } + + /// Return the number of continuous variables in this factor. + size_t nrContinuous() const { return nrContinuous_; } + + /// Return vector of discrete keys. + DiscreteKeys discreteKeys() const { return discreteKeys_; } + /// @} }; // HybridFactor diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 5fa4b555a..721cb4cc7 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -23,12 +23,12 @@ namespace gtsam { HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) { - inner = other; + inner_ = other; } HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), - inner(boost::make_shared(std::move(jf))) {} + inner_(boost::make_shared(std::move(jf))) {} bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { return false; @@ -36,7 +36,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner->print("inner: ", formatter); + inner_->print("inner: ", formatter); }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 4a7939cd4..1749c8e41 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -28,13 +28,14 @@ namespace gtsam { * a diamond inheritance. */ class HybridGaussianFactor : public HybridFactor { + private: + GaussianFactor::shared_ptr inner_; + public: using Base = HybridFactor; using This = HybridGaussianFactor; using shared_ptr = boost::shared_ptr; - GaussianFactor::shared_ptr inner; - // Explicit conversion from a shared ptr of GF explicit HybridGaussianFactor(GaussianFactor::shared_ptr other); @@ -47,5 +48,7 @@ class HybridGaussianFactor : public HybridFactor { void print( const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + + GaussianFactor::shared_ptr inner() const { return inner_; } }; } // namespace gtsam From 573448f126cfe8ebe7fb2b3f149d1a7abe2b08e9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 15:16:28 -0400 Subject: [PATCH 44/70] make functional --- gtsam/hybrid/HybridBayesNet.h | 2 +- gtsam/hybrid/HybridBayesTree.cpp | 1 - gtsam/hybrid/HybridBayesTree.h | 18 +++++++++++++++--- gtsam/hybrid/HybridEliminationTree.h | 11 ++++++++--- gtsam/hybrid/HybridFactorGraph.cpp | 18 +++++++++--------- gtsam/hybrid/HybridISAM.cpp | 2 +- gtsam/hybrid/HybridJunctionTree.cpp | 2 +- 7 files changed, 35 insertions(+), 19 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 4e411b781..43eead280 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -35,7 +35,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using sharedConditional = boost::shared_ptr; /** Construct empty bayes net */ - HybridBayesNet() : Base() {} + HybridBayesNet() = default; }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 426202861..c8d70e67e 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -33,5 +33,4 @@ bool HybridBayesTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } -/* **************************************************************************/ } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 74bd234d8..f8a90d6b4 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -70,7 +70,13 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /// @} }; -/* This does special stuff for the hybrid case */ +/** + * @brief Class for Hybrid Bayes tree orphan subtrees. + * + * This does special stuff for the hybrid case + * + * @tparam CLIQUE + */ template class BayesTreeOrphanWrapper< CLIQUE, typename std::enable_if< @@ -82,16 +88,22 @@ class BayesTreeOrphanWrapper< boost::shared_ptr clique; + /** + * @brief Construct a new Bayes Tree Orphan Wrapper object. + * + * @param clique Bayes tree clique. + */ BayesTreeOrphanWrapper(const boost::shared_ptr& clique) : clique(clique) { // Store parent keys in our base type factor so that eliminating those // parent keys will pull this subtree into the elimination. this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); - this->discreteKeys_.assign(clique->conditional()->discreteKeys_.begin(), - clique->conditional()->discreteKeys_.end()); + this->discreteKeys_.assign(clique->conditional()->discreteKeys().begin(), + clique->conditional()->discreteKeys().end()); } + /// print utility void print( const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const override { diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 902beb279..27766724a 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -28,12 +28,18 @@ namespace gtsam { */ class GTSAM_EXPORT HybridEliminationTree : public EliminationTree { + private: + friend class ::EliminationTreeTester; + public: typedef EliminationTree Base; ///< Base class typedef HybridEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + /// @name Constructors + /// @{ + /** * Build the elimination tree of a factor graph using pre-computed column * structure. @@ -54,11 +60,10 @@ class GTSAM_EXPORT HybridEliminationTree HybridEliminationTree(const HybridFactorGraph& factorGraph, const Ordering& order); + /// @} + /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; - - private: - friend class ::EliminationTreeTester; }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 954ee57a6..450636ab3 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -150,8 +150,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { std::cout << RESET; } separatorKeys.insert(factor->begin(), factor->end()); - if (!factor->isContinuous_) { - for (auto &k : factor->discreteKeys_) { + if (!factor->isContinuous()) { + for (auto &k : factor->discreteKeys()) { mapFromKeyToDiscreteKey[k.first] = k; } } @@ -223,9 +223,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); if (ptr) { - gfg.push_back(ptr->inner); + gfg.push_back(ptr->inner()); } else { - auto p = boost::static_pointer_cast(fp)->inner; + auto p = boost::static_pointer_cast(fp)->inner(); if (p) { gfg.push_back(boost::static_pointer_cast(p)); } else { @@ -251,9 +251,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); if (ptr) { - dfg.push_back(ptr->inner); + dfg.push_back(ptr->inner()); } else { - auto p = boost::static_pointer_cast(fp)->inner; + auto p = boost::static_pointer_cast(fp)->inner(); if (p) { dfg.push_back(boost::static_pointer_cast(p)); } else { @@ -288,7 +288,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { std::vector deferredFactors; for (auto &f : factors) { - if (f->isHybrid_) { + if (f->isHybrid()) { auto cgmf = boost::dynamic_pointer_cast(f); if (cgmf) { sum = cgmf->add(sum); @@ -299,9 +299,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { sum = gm->asMixture()->add(sum); } - } else if (f->isContinuous_) { + } else if (f->isContinuous()) { deferredFactors.push_back( - boost::dynamic_pointer_cast(f)->inner); + boost::dynamic_pointer_cast(f)->inner()); } else { // We need to handle the case where the object is actually an // BayesTreeOrphanWrapper! diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp index 0db30f1f3..f743b92f1 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridISAM.cpp @@ -58,7 +58,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, KeySet allDiscrete; for (auto& factor : factors) { - for (auto& k : factor->discreteKeys_) { + for (auto& k : factor->discreteKeys()) { allDiscrete.insert(k.first); } } diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index bf088c5aa..d1e19f852 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -63,7 +63,7 @@ struct HybridConstructorTraversalData { std::cout << "Getting discrete info: "; #endif for (HybridFactor::shared_ptr& f : node->factors) { - for (auto& k : f->discreteKeys_) { + for (auto& k : f->discreteKeys()) { #ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n"; #endif From 6d26818e79c12a022cd9d18d56e88fe1ceeb7bad Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 15:17:22 -0400 Subject: [PATCH 45/70] HybridDiscreteFactor docs and minor refactor --- gtsam/hybrid/HybridDiscreteFactor.cpp | 17 ++++++++++------- gtsam/hybrid/HybridDiscreteFactor.h | 16 ++++++++++++++-- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 54b193196..989127a28 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -24,25 +24,28 @@ namespace gtsam { +/* ************************************************************************ */ // TODO(fan): THIS IS VERY VERY DIRTY! We need to get DiscreteFactor right! HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) : Base(boost::dynamic_pointer_cast(other) - ->discreteKeys()) { - inner = other; -} + ->discreteKeys()), + inner_(other) {} +/* ************************************************************************ */ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : Base(dtf.discreteKeys()), - inner(boost::make_shared(std::move(dtf))) {} + inner_(boost::make_shared(std::move(dtf))) {} +/* ************************************************************************ */ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { - return false; + return Base::equals(lf, tol); } +/* ************************************************************************ */ void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner->print("inner: ", formatter); + inner_->print("inner: ", formatter); }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 0f731f8b5..572ddfbcd 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -13,6 +13,7 @@ * @file HybridDiscreteFactor.h * @date Mar 11, 2022 * @author Fan Jiang + * @author Varun Agrawal */ #pragma once @@ -29,12 +30,16 @@ namespace gtsam { * inheritance. */ class HybridDiscreteFactor : public HybridFactor { + private: + DiscreteFactor::shared_ptr inner_; + public: using Base = HybridFactor; using This = HybridDiscreteFactor; using shared_ptr = boost::shared_ptr; - DiscreteFactor::shared_ptr inner; + /// @name Constructors + /// @{ // Implicit conversion from a shared ptr of DF HybridDiscreteFactor(DiscreteFactor::shared_ptr other); @@ -42,11 +47,18 @@ class HybridDiscreteFactor : public HybridFactor { // Forwarding constructor from concrete DecisionTreeFactor HybridDiscreteFactor(DecisionTreeFactor &&dtf); - public: + /// @} + /// @name Testable + /// @{ virtual bool equals(const HybridFactor &lf, double tol) const override; void print( const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + + /// @} + + /// Return pointer to the internal discrete factor + DiscreteFactor::shared_ptr inner() const { return inner_; } }; } // namespace gtsam From 7bfa0118862b4ee1b745597e169e4d99dcaa6321 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 15:17:30 -0400 Subject: [PATCH 46/70] update tests --- gtsam/hybrid/tests/Switching.h | 2 +- gtsam/hybrid/tests/testHybridFactorGraph.cpp | 21 ++++++++++---------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 74d682372..1a4db7898 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -38,7 +38,7 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain( // keyFunc(1) to keyFunc(n+1) for (size_t t = 1; t < n; t++) { - hfg.add(GaussianMixtureFactor::FromFactorList( + hfg.add(GaussianMixtureFactor::FromFactors( {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, {boost::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Z_3x1), diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index 4986cc2a7..55b2a9b3b 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -79,13 +79,14 @@ TEST(HybridFactorGraph, creation) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GaussianMixtureConditional clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - GaussianMixtureConditional::Conditionals( - C(0), - boost::make_shared( - X(0), Z_3x1, I_3x3, X(1), I_3x3), - boost::make_shared( - X(0), Vector3::Ones(), I_3x3, X(1), I_3x3))); + GaussianMixtureConditional clgc( + {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), + GaussianMixtureConditional::Conditionals( + C(0), + boost::make_shared(X(0), Z_3x1, I_3x3, X(1), + I_3x3), + boost::make_shared(X(0), Vector3::Ones(), I_3x3, + X(1), I_3x3))); GTSAM_PRINT(clgc); } @@ -182,7 +183,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { // boost::make_shared(X(1), I_3x3, Vector3::Ones())); // hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); - hfg.add(GaussianMixtureFactor::FromFactorList( + hfg.add(GaussianMixtureFactor::FromFactors( {X(1)}, {{C(1), 2}}, {boost::make_shared(X(1), I_3x3, Z_3x1), boost::make_shared(X(1), I_3x3, Vector3::Ones())})); @@ -234,7 +235,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { */ } -/** +/* * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ @@ -251,7 +252,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { // C(0), boost::make_shared(X(0), I_3x3, Z_3x1), // boost::make_shared(X(0), I_3x3, Vector3::Ones())); - hfg.add(GaussianMixtureFactor::FromFactorList( + hfg.add(GaussianMixtureFactor::FromFactors( {X(0)}, {{C(0), 2}}, {boost::make_shared(X(0), I_3x3, Z_3x1), boost::make_shared(X(0), I_3x3, Vector3::Ones())})); From d60c4aca5a30312e935319af335efd1f675c3c80 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 15:28:11 -0400 Subject: [PATCH 47/70] add demarkers --- gtsam/hybrid/hybrid.i | 6 +++--- gtsam/hybrid/tests/testHybridFactorGraph.cpp | 22 +++++++++----------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 5a76aaf48..d3ff98719 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -23,12 +23,12 @@ virtual class HybridConditional { bool equals(const gtsam::HybridConditional& other, double tol = 1e-9) const; size_t nrFrontals() const; size_t nrParents() const; - Factor* getInner(); + Factor* inner(); }; #include class GaussianMixtureFactor : gtsam::HybridFactor { - static GaussianMixtureFactor FromFactorList( + static GaussianMixtureFactor FromFactors( const gtsam::KeyVector& continuousKeys, const gtsam::DiscreteKeys& discreteKeys, const std::vector& factorsList); @@ -40,7 +40,7 @@ class GaussianMixtureFactor : gtsam::HybridFactor { #include class GaussianMixtureConditional : gtsam::HybridFactor { - static GaussianMixtureConditional FromConditionalList( + static GaussianMixtureConditional FromConditionals( const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index 55b2a9b3b..860fdfdb8 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -90,6 +90,7 @@ TEST(HybridFactorGraph, creation) { GTSAM_PRINT(clgc); } +/* ************************************************************************* */ TEST(HybridFactorGraph, eliminate) { HybridFactorGraph hfg; @@ -100,6 +101,7 @@ TEST(HybridFactorGraph, eliminate) { EXPECT_LONGS_EQUAL(result.first->size(), 1); } +/* ************************************************************************* */ TEST(HybridFactorGraph, eliminateMultifrontal) { HybridFactorGraph hfg; @@ -116,6 +118,7 @@ TEST(HybridFactorGraph, eliminateMultifrontal) { EXPECT_LONGS_EQUAL(result.second->size(), 1); } +/* ************************************************************************* */ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { HybridFactorGraph hfg; @@ -139,9 +142,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3); } +/* ************************************************************************* */ TEST(HybridFactorGraph, eliminateFullSequentialSimple) { - std::cout << ">>>>>>>>>>>>>>\n"; - HybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -168,9 +170,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) { GTSAM_PRINT(*result); } +/* ************************************************************************* */ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { - std::cout << ">>>>>>>>>>>>>>\n"; - HybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -202,9 +203,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { GTSAM_PRINT(*result->marginalFactor(C(2))); } +/* ************************************************************************* */ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { - std::cout << ">>>>>>>>>>>>>>\n"; - HybridFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -235,13 +235,12 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { */ } +/* ************************************************************************* */ /* * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { - std::cout << ">>>>>>>>>>>>>>\n"; - HybridFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); @@ -308,6 +307,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { */ } +/* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one TEST(HybridFactorGraph, Switching) { auto N = 12; @@ -415,6 +415,7 @@ TEST(HybridFactorGraph, Switching) { hbt->marginalFactor(C(11))->print("HBT: "); } +/* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one TEST(HybridFactorGraph, SwitchingISAM) { auto N = 11; @@ -510,6 +511,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { } } +/* ************************************************************************* */ TEST(HybridFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); @@ -599,10 +601,6 @@ TEST(HybridFactorGraph, SwitchingTwoVar) { /* ************************************************************************* */ int main() { -#ifdef HYBRID_DEBUG - ::signal(SIGSEGV, &my_signal_handler); - ::signal(SIGBUS, &my_signal_handler); -#endif TestResult tr; return TestRegistry::runAllTests(tr); } From adcbb65e5c33f3a9f803a6d0a89379bd5830450d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 16:07:52 -0400 Subject: [PATCH 48/70] HybridFactorGraph docs and minor updates --- gtsam/hybrid/HybridFactorGraph.cpp | 21 +++++++++++++-------- gtsam/hybrid/HybridFactorGraph.h | 22 +++++++++++++--------- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 450636ab3..344b4220b 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -63,6 +63,7 @@ static std::string RESET = "\033[0m"; constexpr bool DEBUG = false; +/* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { using Y = GaussianFactorGraph; @@ -406,20 +407,24 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } } +/* ************************************************************************ */ void HybridFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } -void HybridFactorGraph::add(DecisionTreeFactor &&factor) { - FactorGraph::add(boost::make_shared(std::move(factor))); -} - -void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { - FactorGraph::add(boost::make_shared(factor)); -} - +/* ************************************************************************ */ void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } +/* ************************************************************************ */ +void HybridFactorGraph::add(DecisionTreeFactor &&factor) { + FactorGraph::add(boost::make_shared(std::move(factor))); +} + +/* ************************************************************************ */ +void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { + FactorGraph::add(boost::make_shared(factor)); +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index bfd6a8690..10670b6b7 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -81,30 +81,34 @@ class HybridFactorGraph : public FactorGraph, using Values = gtsam::Values; ///< backwards compatibility using Indices = KeyVector; ///> map from keys to values - public: + /// @name Constructors + /// @{ + HybridFactorGraph() = default; - // /** Construct from container of factors (shared_ptr or plain objects) */ - // template - // explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} - - /** Implicit copy/downcast constructor to override explicit template container + /** + * Implicit copy/downcast constructor to override explicit template container * constructor. In BayesTree this is used for: * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + /// @} + using FactorGraph::add; - /// Add a factor directly using a shared_ptr. + /// Add a Jacobian factor to the factor graph. void add(JacobianFactor&& factor); + /// Add a Jacobian factor as a shared ptr. + void add(boost::shared_ptr factor); + + /// Add a DecisionTreeFactor to the factor graph. void add(DecisionTreeFactor&& factor); + /// Add a DecisionTreeFactor as a shared ptr. void add(boost::shared_ptr factor); - - void add(boost::shared_ptr factor); }; } // namespace gtsam From 5d3ffb7fe825a526810a8052cebeae9945528646 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 16:14:39 -0400 Subject: [PATCH 49/70] docs update --- gtsam/inference/BayesTree.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 0c7d0a8fb..924a505a2 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -281,10 +281,18 @@ namespace gtsam { boost::shared_ptr clique; + /** + * @brief Construct a new Bayes Tree Orphan Wrapper object + * + * This object stores parent keys in our base type factor so that + * eliminating those parent keys will pull this subtree into the + * elimination. + * + * @param clique Orphan clique to add for further consideration in + * elimination. + */ BayesTreeOrphanWrapper(const boost::shared_ptr& clique) : clique(clique) { - // Store parent keys in our base type factor so that eliminating those - // parent keys will pull this subtree into the elimination. this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); } From e2c775302aa8d6f364d1a9f72b012c133cb197bd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 16:15:53 -0400 Subject: [PATCH 50/70] remove debug statements --- gtsam/hybrid/HybridJunctionTree.cpp | 24 +----------------------- 1 file changed, 1 insertion(+), 23 deletions(-) diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index d1e19f852..981cd6f32 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -23,7 +23,6 @@ #include -// #undef NDEBUG namespace gtsam { // Instantiate base classes @@ -59,15 +58,8 @@ struct HybridConstructorTraversalData { myData.myJTNode = boost::make_shared(node->key, node->factors); parentData.myJTNode->addChild(myData.myJTNode); -#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG - std::cout << "Getting discrete info: "; -#endif for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { -#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG - std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n"; -#endif - myData.discreteKeys.insert(k.first); } } @@ -104,11 +96,6 @@ struct HybridConstructorTraversalData { boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); -#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG - std::cout << "Symbolic: "; - myConditional->print(); -#endif - // Store symbolic elimination results in the parent myData.parentData->childSymbolicConditionals.push_back(myConditional); myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); @@ -136,19 +123,10 @@ struct HybridConstructorTraversalData { myData.discreteKeys.exists(myConditional->frontals()[0]); const bool theirType = myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); -#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG - std::cout << "Type: " - << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs " - << DefaultKeyFormatter(childConditionals[i]->frontals()[0]) - << "\n"; -#endif + if (myType == theirType) { // Increment number of frontal variables myNrFrontals += nrFrontals[i]; -#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG - std::cout << "Merging "; - childConditionals[i]->print(); -#endif merge[i] = true; } } From 30c8e1dfa723d987fd2ad5c35474646e448bdbc1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 16:16:30 -0400 Subject: [PATCH 51/70] fix doxygen section --- gtsam/hybrid/HybridISAM.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h index ac9e17e83..7f9e77b8b 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridISAM.h @@ -41,6 +41,8 @@ class GTSAM_EXPORT HybridISAM : public ISAM { /** Copy constructor */ HybridISAM(const HybridBayesTree& bayesTree); + /// @} + void updateInternal( const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = @@ -49,7 +51,6 @@ class GTSAM_EXPORT HybridISAM : public ISAM { void update(const HybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); - /// @} }; /// traits From 4ee4b37f2f85eeeda35c728cd6e3fc2c860b2727 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 16:23:35 -0400 Subject: [PATCH 52/70] HybridISAM docs --- gtsam/hybrid/HybridISAM.cpp | 4 ++-- gtsam/hybrid/HybridISAM.h | 9 +++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp index f743b92f1..057f784ad 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridISAM.cpp @@ -36,6 +36,7 @@ HybridISAM::HybridISAM() {} /* ************************************************************************* */ HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} +/* ************************************************************************* */ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function) { @@ -79,8 +80,6 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), true); - ordering.print("ORD"); - // eliminate all factors (top, added, orphans) into a new Bayes tree auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); @@ -90,6 +89,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); } +/* ************************************************************************* */ void HybridISAM::update(const HybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function) { Cliques orphans; diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h index 7f9e77b8b..0738f46c8 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridISAM.h @@ -43,11 +43,20 @@ class GTSAM_EXPORT HybridISAM : public ISAM { /// @} + private: + /// Internal method that performs the ISAM update. void updateInternal( const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); + public: + /** + * @brief Perform update step with new factors. + * + * @param newFactors Factor graph of new factors to add and eliminate. + * @param function Elimination function. + */ void update(const HybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); From 9d26a3dc9d2433e937ec41256073578cc28c134c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 16:43:19 -0400 Subject: [PATCH 53/70] remove debug statements and add docs --- gtsam/hybrid/HybridFactorGraph.cpp | 82 +---------------------------- gtsam/hybrid/HybridGaussianFactor.h | 7 +++ 2 files changed, 8 insertions(+), 81 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 344b4220b..bffb0327c 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -138,18 +138,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { KeySet continuousFrontals; KeySet continuousSeparator; - if (DEBUG) { - std::cout << RED_BOLD << "Begin Eliminate: " << RESET; - frontalKeys.print(); - } - // This initializes separatorKeys and mapFromKeyToDiscreteKey for (auto &&factor : factors) { - if (DEBUG) { - std::cout << ">>> Adding factor: " << GREEN; - factor->print(); - std::cout << RESET; - } separatorKeys.insert(factor->begin(), factor->end()); if (!factor->isContinuous()) { for (auto &k : factor->discreteKeys()) { @@ -183,43 +173,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } } - // Only for printing - if (DEBUG) { - std::cout << RED_BOLD << "Keys: " << RESET; - for (auto &f : frontalKeys) { - if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) { - auto &key = mapFromKeyToDiscreteKey.at(f); - std::cout << boost::format(" (%1%,%2%),") % - DefaultKeyFormatter(key.first) % key.second; - } else { - std::cout << " " << DefaultKeyFormatter(f) << ","; - } - } - - if (separatorKeys.size() > 0) { - std::cout << " | "; - } - - for (auto &f : separatorKeys) { - if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) { - auto &key = mapFromKeyToDiscreteKey.at(f); - std::cout << boost::format(" (%1%,%2%),") % - DefaultKeyFormatter(key.first) % key.second; - } else { - std::cout << DefaultKeyFormatter(f) << ","; - } - } - std::cout << "\n" << RESET; - } - // NOTE: We should really defer the product here because of pruning // Case 1: we are only dealing with continuous if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) { - if (DEBUG) { - std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n"; - } - GaussianFactorGraph gfg; for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); @@ -231,7 +188,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { gfg.push_back(boost::static_pointer_cast(p)); } else { // It is an orphan wrapped conditional - if (DEBUG) std::cout << "Got an orphan conditional\n"; } } } @@ -244,10 +200,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Case 2: we are only dealing with discrete if (allContinuousKeys.empty()) { - if (DEBUG) { - std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n"; - } - DiscreteFactorGraph dfg; for (auto &fp : factors) { auto ptr = boost::dynamic_pointer_cast(fp); @@ -259,7 +211,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { dfg.push_back(boost::static_pointer_cast(p)); } else { // It is an orphan wrapper - if (DEBUG) std::cout << "Got an orphan wrapper conditional\n"; } } } @@ -280,10 +231,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // sum out frontals, this is the factor on the separator gttic(sum); - if (DEBUG) { - std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n"; - } - GaussianMixtureFactor::Sum sum; std::vector deferredFactors; @@ -308,9 +255,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // BayesTreeOrphanWrapper! auto orphan = boost::dynamic_pointer_cast< BayesTreeOrphanWrapper>(f); - if (orphan) { - if (DEBUG) std::cout << "Got an orphan wrapper conditional\n"; - } else { + if (!orphan) { auto &fr = *f; throw std::invalid_argument( std::string("factor is discrete in continuous elimination") + @@ -320,21 +265,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } for (auto &f : deferredFactors) { - if (DEBUG) { - std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n"; - } sum = addGaussian(sum, f); } - if (DEBUG) { - std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET; - sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) { - RedirectCout rd; - gfg.print(""); - return rd.str(); - }); - } - gttoc(sum); using EliminationPair = GaussianFactorGraph::EliminationResult; @@ -370,19 +303,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { auto conditional = boost::make_shared( frontalKeys, keysOfSeparator, discreteSeparator, conditionals); - if (DEBUG) { - std::cout << GREEN_BOLD << "[Conditional]\n" << RESET; - conditional->print(); - std::cout << GREEN_BOLD << "[Separator]\n" << RESET; - separatorFactors.print("", DefaultKeyFormatter, - [](GaussianFactor::shared_ptr gc) { - RedirectCout rd; - gc->print(""); - return rd.str(); - }); - std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; - } - // If there are no more continuous parents, then we should create here a // DiscreteFactor, with the error for each discrete choice. if (keysOfSeparator.empty()) { diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 1749c8e41..8d457e778 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -43,12 +43,19 @@ class HybridGaussianFactor : public HybridFactor { explicit HybridGaussianFactor(JacobianFactor &&jf); public: + /// @name Testable + /// @{ + + /// Check equality. virtual bool equals(const HybridFactor &lf, double tol) const override; + /// GTSAM print utility. void print( const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + /// @} + GaussianFactor::shared_ptr inner() const { return inner_; } }; } // namespace gtsam From 3bde044248385442101532b91490359c8eb175ba Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 18:13:06 -0400 Subject: [PATCH 54/70] add doc strings to python unit test and add assertions --- gtsam/hybrid/GaussianMixtureConditional.cpp | 10 ++-- gtsam/hybrid/HybridConditional.cpp | 50 +++++++++++--------- gtsam/hybrid/HybridFactor.cpp | 6 +-- python/gtsam/tests/test_HybridFactorGraph.py | 24 +++++----- 4 files changed, 49 insertions(+), 41 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp index 68c3f505e..726af6d5f 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixtureConditional.cpp @@ -85,12 +85,12 @@ bool GaussianMixtureConditional::equals(const HybridFactor &lf, /* *******************************************************************************/ void GaussianMixtureConditional::print(const std::string &s, const KeyFormatter &formatter) const { - std::cout << s << ": "; - if (isContinuous()) std::cout << "Cont. "; - if (isDiscrete()) std::cout << "Disc. "; - if (isHybrid()) std::cout << "Hybr. "; + std::cout << s; + if (isContinuous()) std::cout << "Continuous "; + if (isDiscrete()) std::cout << "Discrete "; + if (isHybrid()) std::cout << "Hybrid "; BaseConditional::print("", formatter); - std::cout << "Discrete Keys = "; + std::cout << "\nDiscrete Keys = "; for (auto &dk : discreteKeys()) { std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index e70d100c3..7d1b72067 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -67,30 +67,36 @@ HybridConditional::HybridConditional( void HybridConditional::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; - if (isContinuous()) std::cout << "Cont. "; - if (isDiscrete()) std::cout << "Disc. "; - if (isHybrid()) std::cout << "Hybr. "; - std::cout << "P("; - size_t 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 (inner_) { + inner_->print("", formatter); + + } else { + if (isContinuous()) std::cout << "Continuous "; + if (isDiscrete()) std::cout << "Discrete "; + if (isHybrid()) std::cout << "Hybrid "; + BaseConditional::print("", formatter); + + std::cout << "P("; + size_t 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++; } - 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); } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 9358c473d..815ea0415 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -77,9 +77,9 @@ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { void HybridFactor::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; - if (isContinuous_) std::cout << "Cont. "; - if (isDiscrete_) std::cout << "Disc. "; - if (isHybrid_) std::cout << "Hybr. "; + if (isContinuous_) std::cout << "Continuous "; + if (isDiscrete_) std::cout << "Discrete "; + if (isHybrid_) std::cout << "Hybrid "; this->printKeys("", formatter); } diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 48187b7a2..144183816 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -16,24 +16,25 @@ import unittest import gtsam import numpy as np -from gtsam.symbol_shorthand import X, C +from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase class TestHybridFactorGraph(GtsamTestCase): + """Unit tests for HybridFactorGraph.""" + def test_create(self): + """Test contruction of hybrid factor graph.""" noiseModel = gtsam.noiseModel.Unit.Create(3) dk = gtsam.DiscreteKeys() dk.push_back((C(0), 2)) - # print(dk.at(0)) jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), noiseModel) jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), noiseModel) - gmf = gtsam.GaussianMixtureFactor.FromFactorList([X(0)], dk, - [jf1, jf2]) + gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) hfg = gtsam.HybridFactorGraph() hfg.add(jf1) @@ -41,16 +42,17 @@ class TestHybridFactorGraph(GtsamTestCase): hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph( - hfg, [C(0)])) + gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)])) - print("hbn = ", hbn) + # print("hbn = ", hbn) + self.assertEqual(hbn.size(), 2) - mixture = hbn.at(0).getInner() - print(mixture) + mixture = hbn.at(0).inner() + self.assertIsInstance(mixture, gtsam.GaussianMixtureConditional) + self.assertEqual(len(mixture.keys()), 2) - discrete_conditional = hbn.at(hbn.size()-1).getInner() - print(discrete_conditional) + discrete_conditional = hbn.at(hbn.size() - 1).inner() + self.assertIsInstance(discrete_conditional, gtsam.DiscreteConditional) if __name__ == "__main__": From f443cf30e0c2517da1da26a99ec13460297c7ccf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 18:32:19 -0400 Subject: [PATCH 55/70] add docs for HybridFactor --- gtsam/hybrid/HybridFactor.h | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index da103da43..244fba4cc 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -34,6 +34,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, /** * Base class for hybrid probabilistic factors + * * Examples: * - HybridGaussianFactor * - HybridDiscreteFactor @@ -64,13 +65,29 @@ class GTSAM_EXPORT HybridFactor : public Factor { /** Default constructor creates empty factor */ HybridFactor() = default; + /** + * @brief Construct hybrid factor from continuous keys. + * + * @param keys Vector of continuous keys. + */ explicit HybridFactor(const KeyVector &keys); + /** + * @brief Construct hybrid factor from discrete keys. + * + * @param keys Vector of discrete keys. + */ + explicit HybridFactor(const DiscreteKeys &discreteKeys); + + /** + * @brief Construct a new Hybrid Factor object. + * + * @param continuousKeys Vector of keys for continuous variables. + * @param discreteKeys Vector of keys for discrete variables. + */ HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); - explicit HybridFactor(const DiscreteKeys &discreteKeys); - /// Virtual destructor virtual ~HybridFactor() = default; From 6cd20fba4d866092c3da220bc1323ebae5a8b901 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 18:34:13 -0400 Subject: [PATCH 56/70] break up EliminateHybrid into smaller functions --- gtsam/hybrid/HybridFactorGraph.cpp | 310 +++++++++++++++-------------- 1 file changed, 161 insertions(+), 149 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index bffb0327c..426c86fa3 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -55,14 +55,6 @@ namespace gtsam { template class EliminateableFactorGraph; -static std::string BLACK_BOLD = "\033[1;30m"; -static std::string RED_BOLD = "\033[1;31m"; -static std::string GREEN = "\033[0;32m"; -static std::string GREEN_BOLD = "\033[1;32m"; -static std::string RESET = "\033[0m"; - -constexpr bool DEBUG = false; - /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { @@ -84,6 +76,163 @@ static GaussianMixtureFactor::Sum &addGaussian( return sum; } +/* ************************************************************************ */ +std::pair +continuousElimination(const HybridFactorGraph &factors, + const Ordering &frontalKeys) { + GaussianFactorGraph gfg; + for (auto &fp : factors) { + auto ptr = boost::dynamic_pointer_cast(fp); + if (ptr) { + gfg.push_back(ptr->inner()); + } else { + auto p = boost::static_pointer_cast(fp)->inner(); + if (p) { + gfg.push_back(boost::static_pointer_cast(p)); + } else { + // It is an orphan wrapped conditional + } + } + } + + auto result = EliminatePreferCholesky(gfg, frontalKeys); + return std::make_pair( + boost::make_shared(result.first), + boost::make_shared(result.second)); +} + +/* ************************************************************************ */ +std::pair +discreteElimination(const HybridFactorGraph &factors, + const Ordering &frontalKeys) { + DiscreteFactorGraph dfg; + for (auto &fp : factors) { + auto ptr = boost::dynamic_pointer_cast(fp); + if (ptr) { + dfg.push_back(ptr->inner()); + } else { + auto p = boost::static_pointer_cast(fp)->inner(); + if (p) { + dfg.push_back(boost::static_pointer_cast(p)); + } else { + // It is an orphan wrapper + } + } + } + + auto result = EliminateDiscrete(dfg, frontalKeys); + + return std::make_pair( + boost::make_shared(result.first), + boost::make_shared(result.second)); +} + +/* ************************************************************************ */ +std::pair +hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, + const KeySet &continuousSeparator, + const std::set &discreteSeparatorSet) { + // NOTE: since we use the special JunctionTree, + // only possiblity is continuous conditioned on discrete. + DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), + discreteSeparatorSet.end()); + + // sum out frontals, this is the factor on the separator + gttic(sum); + + GaussianMixtureFactor::Sum sum; + std::vector deferredFactors; + + for (auto &f : factors) { + if (f->isHybrid()) { + auto cgmf = boost::dynamic_pointer_cast(f); + if (cgmf) { + sum = cgmf->add(sum); + } + + auto gm = boost::dynamic_pointer_cast(f); + if (gm) { + sum = gm->asMixture()->add(sum); + } + + } else if (f->isContinuous()) { + deferredFactors.push_back( + boost::dynamic_pointer_cast(f)->inner()); + } else { + // We need to handle the case where the object is actually an + // BayesTreeOrphanWrapper! + auto orphan = boost::dynamic_pointer_cast< + BayesTreeOrphanWrapper>(f); + if (!orphan) { + auto &fr = *f; + throw std::invalid_argument( + std::string("factor is discrete in continuous elimination") + + typeid(fr).name()); + } + } + } + + for (auto &f : deferredFactors) { + sum = addGaussian(sum, f); + } + + gttoc(sum); + + using EliminationPair = GaussianFactorGraph::EliminationResult; + + KeyVector keysOfEliminated; // Not the ordering + KeyVector keysOfSeparator; // TODO(frank): Is this just (keys - ordering)? + + auto eliminate = [&](const GaussianFactorGraph &graph) + -> GaussianFactorGraph::EliminationResult { + if (graph.empty()) { + return {nullptr, nullptr}; + } + auto result = EliminatePreferCholesky(graph, frontalKeys); + if (keysOfEliminated.empty()) { + keysOfEliminated = + result.first->keys(); // Initialize the keysOfEliminated to be the + } + // keysOfEliminated of the GaussianConditional + if (keysOfSeparator.empty()) { + keysOfSeparator = result.second->keys(); + } + return result; + }; + + DecisionTree eliminationResults(sum, eliminate); + + auto pair = unzip(eliminationResults); + + const GaussianMixtureFactor::Factors &separatorFactors = pair.second; + + // Create the GaussianMixtureConditional from the conditionals + auto conditional = boost::make_shared( + frontalKeys, keysOfSeparator, discreteSeparator, pair.first); + + // If there are no more continuous parents, then we should create here a + // DiscreteFactor, with the error for each discrete choice. + if (keysOfSeparator.empty()) { + VectorValues empty_values; + auto factorError = [&](const GaussianFactor::shared_ptr &factor) { + if (!factor) return 0.0; // TODO(fan): does this make sense? + return exp(-factor->error(empty_values)); + }; + DecisionTree fdt(separatorFactors, factorError); + auto discreteFactor = + boost::make_shared(discreteSeparator, fdt); + + return {boost::make_shared(conditional), + boost::make_shared(discreteFactor)}; + + } else { + // Create a resulting DCGaussianMixture on the separator. + auto factor = boost::make_shared( + KeyVector(continuousSeparator.begin(), continuousSeparator.end()), + discreteSeparator, separatorFactors); + return {boost::make_shared(conditional), factor}; + } +} /* ************************************************************************ */ std::pair // EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { @@ -177,154 +326,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // Case 1: we are only dealing with continuous if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) { - GaussianFactorGraph gfg; - for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { - gfg.push_back(ptr->inner()); - } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - gfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapped conditional - } - } - } - - auto result = EliminatePreferCholesky(gfg, frontalKeys); - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); + return continuousElimination(factors, frontalKeys); } // Case 2: we are only dealing with discrete if (allContinuousKeys.empty()) { - DiscreteFactorGraph dfg; - for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { - dfg.push_back(ptr->inner()); - } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - dfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapper - } - } - } - - auto result = EliminateDiscrete(dfg, frontalKeys); - - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); + return discreteElimination(factors, frontalKeys); } // Case 3: We are now in the hybrid land! - // NOTE: since we use the special JunctionTree, only possiblity is cont. - // conditioned on disc. - DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(), - discreteSeparatorSet.end()); - - // sum out frontals, this is the factor on the separator - gttic(sum); - - GaussianMixtureFactor::Sum sum; - - std::vector deferredFactors; - - for (auto &f : factors) { - if (f->isHybrid()) { - auto cgmf = boost::dynamic_pointer_cast(f); - if (cgmf) { - sum = cgmf->add(sum); - } - - auto gm = boost::dynamic_pointer_cast(f); - if (gm) { - sum = gm->asMixture()->add(sum); - } - - } else if (f->isContinuous()) { - deferredFactors.push_back( - boost::dynamic_pointer_cast(f)->inner()); - } else { - // We need to handle the case where the object is actually an - // BayesTreeOrphanWrapper! - auto orphan = boost::dynamic_pointer_cast< - BayesTreeOrphanWrapper>(f); - if (!orphan) { - auto &fr = *f; - throw std::invalid_argument( - std::string("factor is discrete in continuous elimination") + - typeid(fr).name()); - } - } - } - - for (auto &f : deferredFactors) { - sum = addGaussian(sum, f); - } - - gttoc(sum); - - using EliminationPair = GaussianFactorGraph::EliminationResult; - - KeyVector keysOfEliminated; // Not the ordering - KeyVector keysOfSeparator; // TODO(frank): Is this just (keys - ordering)? - - auto eliminate = [&](const GaussianFactorGraph &graph) - -> GaussianFactorGraph::EliminationResult { - if (graph.empty()) { - return {nullptr, nullptr}; - } - auto result = EliminatePreferCholesky(graph, frontalKeys); - if (keysOfEliminated.empty()) { - keysOfEliminated = - result.first->keys(); // Initialize the keysOfEliminated to be the - } - // keysOfEliminated of the GaussianConditional - if (keysOfSeparator.empty()) { - keysOfSeparator = result.second->keys(); - } - return result; - }; - - DecisionTree eliminationResults(sum, eliminate); - - auto pair = unzip(eliminationResults); - - const GaussianMixtureConditional::Conditionals &conditionals = pair.first; - const GaussianMixtureFactor::Factors &separatorFactors = pair.second; - - // Create the GaussianMixtureConditional from the conditionals - auto conditional = boost::make_shared( - frontalKeys, keysOfSeparator, discreteSeparator, conditionals); - - // If there are no more continuous parents, then we should create here a - // DiscreteFactor, with the error for each discrete choice. - if (keysOfSeparator.empty()) { - VectorValues empty_values; - auto factorError = [&](const GaussianFactor::shared_ptr &factor) { - if (!factor) return 0.0; // TODO(fan): does this make sense? - return exp(-factor->error(empty_values)); - }; - DecisionTree fdt(separatorFactors, factorError); - auto discreteFactor = - boost::make_shared(discreteSeparator, fdt); - - return {boost::make_shared(conditional), - boost::make_shared(discreteFactor)}; - - } else { - // Create a resulting DCGaussianMixture on the separator. - auto factor = boost::make_shared( - KeyVector(continuousSeparator.begin(), continuousSeparator.end()), - discreteSeparator, separatorFactors); - return {boost::make_shared(conditional), factor}; - } + return hybridElimination(factors, frontalKeys, continuousSeparator, + discreteSeparatorSet); } /* ************************************************************************ */ From c3a59cfd620ec3de15f381c7d2cd9585c7641731 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 18:46:52 -0400 Subject: [PATCH 57/70] update the EliminateHybrid note to be more specific --- gtsam/hybrid/HybridFactorGraph.cpp | 53 ++++++++++++++++-------------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 426c86fa3..dbb4a62b3 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -236,43 +236,46 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, /* ************************************************************************ */ std::pair // EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { - // NOTE(fan): Because we are in the Conditional Gaussian regime there are only + // NOTE: Because we are in the Conditional Gaussian regime there are only // 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. + // 1. continuous variable, make a Gaussian Mixture if there are hybrid + // factors; + // 2. continuous variable, we make a Gaussian Factor if there are no hybrid + // factors; + // 3. discrete variable, no continuous factor is allowed + // (escapes Conditional Gaussian regime), 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) + // hybrid variables, for example x1, c1. + // In this scenario, we will have a density P(x1, c1) that is a Conditional + // Linear Gaussian 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? // 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! - - // Because of all these reasons, we need to think very carefully about how to + // Because of all these reasons, we carefully consider how to // implement the hybrid factors so that we do not get poor performance. - // - // The first thing is how to represent the GaussianMixtureConditional. A very - // possible scenario is that the incoming factors will have different levels - // of discrete keys. For example, imagine we are going to eliminate the - // fragment: - // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will - // need to know how to retrieve the corresponding continuous densities for the - // assi- -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). - // And we also need to consider when there is pruning. Two mixture factors - // could have different pruning patterns-one could have (c1=0,c2=1) pruned, - // and another could have (c2=0,c3=1) pruned, and this creates a big problem - // in how to identify the intersection of non-pruned branches. - // One possible approach is first building the collection of all discrete - // keys. After that we enumerate the space of all key combinations *lazily* so - // that the exploration branch terminates whenever an assignment yields NULL - // in any of the hybrid factors. + // The first thing is how to represent the GaussianMixtureConditional. + // A very possible scenario is that the incoming factors will have different + // levels of discrete keys. For example, imagine we are going to eliminate the + // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. + // Now we will need to know how to retrieve the corresponding continuous + // densities for the assignment (c1,c2,c3) (OR (c2,c3,c1), note there is NO + // defined order!). We also need to consider when there is pruning. Two + // mixture factors could have different pruning patterns - one could have + // (c1=0,c2=1) pruned, and another could have (c2=0,c3=1) pruned, and this + // creates a big problem in how to identify the intersection of non-pruned + // branches. + + // Our approach is first building the collection of all discrete keys. After + // that we enumerate the space of all key combinations *lazily* so that the + // exploration branch terminates whenever an assignment yields NULL in any of + // the hybrid factors. // When the number of assignments is large we may encounter stack overflows. // However this is also the case with iSAM2, so no pressure :) From 852a9b9ef65ff4f621f843d635484ca8b8375ccb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 May 2022 14:24:03 -0400 Subject: [PATCH 58/70] rename HybridFactorGraph to GaussianHybridFactorGraph --- ...raph.cpp => GaussianHybridFactorGraph.cpp} | 24 +++++---- ...torGraph.h => GaussianHybridFactorGraph.h} | 35 +++++++------ gtsam/hybrid/HybridBayesTree.cpp | 3 +- gtsam/hybrid/HybridBayesTree.h | 8 +-- gtsam/hybrid/HybridConditional.h | 7 +-- gtsam/hybrid/HybridEliminationTree.cpp | 8 +-- gtsam/hybrid/HybridEliminationTree.h | 10 ++-- gtsam/hybrid/HybridISAM.cpp | 6 +-- gtsam/hybrid/HybridISAM.h | 7 +-- gtsam/hybrid/HybridJunctionTree.cpp | 16 +++--- gtsam/hybrid/HybridJunctionTree.h | 6 +-- gtsam/hybrid/hybrid.i | 16 +++--- gtsam/hybrid/tests/Switching.h | 8 +-- ....cpp => testGaussianHybridFactorGraph.cpp} | 52 +++++++++---------- gtsam/inference/inference.i | 14 ++--- python/gtsam/tests/test_HybridFactorGraph.py | 9 ++-- 16 files changed, 122 insertions(+), 107 deletions(-) rename gtsam/hybrid/{HybridFactorGraph.cpp => GaussianHybridFactorGraph.cpp} (94%) rename gtsam/hybrid/{HybridFactorGraph.h => GaussianHybridFactorGraph.h} (74%) rename gtsam/hybrid/tests/{testHybridFactorGraph.cpp => testGaussianHybridFactorGraph.cpp} (93%) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/GaussianHybridFactorGraph.cpp similarity index 94% rename from gtsam/hybrid/HybridFactorGraph.cpp rename to gtsam/hybrid/GaussianHybridFactorGraph.cpp index dbb4a62b3..3354d5b4d 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/GaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridFactorGraph.cpp + * @file GaussianHybridFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang * @author Varun Agrawal @@ -23,13 +23,13 @@ #include #include #include +#include #include #include #include #include #include #include -#include #include #include #include @@ -53,7 +53,7 @@ namespace gtsam { -template class EliminateableFactorGraph; +template class EliminateableFactorGraph; /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( @@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian( /* ************************************************************************ */ std::pair -continuousElimination(const HybridFactorGraph &factors, +continuousElimination(const GaussianHybridFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { @@ -103,7 +103,7 @@ continuousElimination(const HybridFactorGraph &factors, /* ************************************************************************ */ std::pair -discreteElimination(const HybridFactorGraph &factors, +discreteElimination(const GaussianHybridFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { @@ -129,7 +129,8 @@ discreteElimination(const HybridFactorGraph &factors, /* ************************************************************************ */ std::pair -hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, +hybridElimination(const GaussianHybridFactorGraph &factors, + const Ordering &frontalKeys, const KeySet &continuousSeparator, const std::set &discreteSeparatorSet) { // NOTE: since we use the special JunctionTree, @@ -235,7 +236,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, } /* ************************************************************************ */ std::pair // -EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { +EliminateHybrid(const GaussianHybridFactorGraph &factors, + const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only // a few cases: // 1. continuous variable, make a Gaussian Mixture if there are hybrid @@ -343,22 +345,22 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } /* ************************************************************************ */ -void HybridFactorGraph::add(JacobianFactor &&factor) { +void GaussianHybridFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) { +void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } /* ************************************************************************ */ -void HybridFactorGraph::add(DecisionTreeFactor &&factor) { +void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { +void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/GaussianHybridFactorGraph.h similarity index 74% rename from gtsam/hybrid/HybridFactorGraph.h rename to gtsam/hybrid/GaussianHybridFactorGraph.h index 10670b6b7..c8e0718fc 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/GaussianHybridFactorGraph.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridFactorGraph.h - * @brief Hybrid factor graph that uses type erasure + * @file GaussianHybridFactorGraph.h + * @brief Linearized Hybrid factor graph that uses type erasure * @author Fan Jiang * @date Mar 11, 2022 */ @@ -26,7 +26,7 @@ namespace gtsam { // Forward declarations -class HybridFactorGraph; +class GaussianHybridFactorGraph; class HybridConditional; class HybridBayesNet; class HybridEliminationTree; @@ -36,17 +36,18 @@ class DecisionTreeFactor; class JacobianFactor; -/** Main elimination function for HybridFactorGraph */ +/** Main elimination function for GaussianHybridFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); +EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ template <> -struct EliminationTraits { +struct EliminationTraits { typedef HybridFactor FactorType; ///< Type of factors in factor graph - typedef HybridFactorGraph - FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) + typedef GaussianHybridFactorGraph + FactorGraphType; ///< Type of the factor graph (e.g. + ///< GaussianHybridFactorGraph) typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination typedef HybridBayesNet @@ -64,16 +65,17 @@ struct EliminationTraits { }; /** - * Hybrid Factor Graph + * Gaussian Hybrid Factor Graph * ----------------------- - * This is the linear version of a hybrid factor graph. Everything inside needs - * to be hybrid factor or hybrid conditional. + * This is the linearized version of a hybrid factor graph. + * Everything inside needs to be hybrid factor or hybrid conditional. */ -class HybridFactorGraph : public FactorGraph, - public EliminateableFactorGraph { +class GaussianHybridFactorGraph + : public FactorGraph, + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = HybridFactorGraph; ///< this class + using This = GaussianHybridFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This @@ -84,7 +86,7 @@ class HybridFactorGraph : public FactorGraph, /// @name Constructors /// @{ - HybridFactorGraph() = default; + GaussianHybridFactorGraph() = default; /** * Implicit copy/downcast constructor to override explicit template container @@ -92,7 +94,8 @@ class HybridFactorGraph : public FactorGraph, * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template - HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + GaussianHybridFactorGraph(const FactorGraph& graph) + : Base(graph) {} /// @} diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index c8d70e67e..400cac5e7 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -25,7 +25,8 @@ namespace gtsam { // Instantiate base class -template class BayesTreeCliqueBase; +template class BayesTreeCliqueBase; template class BayesTree; /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index f8a90d6b4..610181458 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -18,8 +18,8 @@ #pragma once +#include #include -#include #include #include #include @@ -37,10 +37,12 @@ class VectorValues; * which is a HybridConditional internally. */ class GTSAM_EXPORT HybridBayesTreeClique - : public BayesTreeCliqueBase { + : public BayesTreeCliqueBase { public: typedef HybridBayesTreeClique This; - typedef BayesTreeCliqueBase Base; + typedef BayesTreeCliqueBase + Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; HybridBayesTreeClique() {} diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index b942773cb..9592b0c69 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,9 +18,9 @@ #pragma once #include +#include #include #include -#include #include #include #include @@ -34,7 +34,7 @@ namespace gtsam { -class HybridFactorGraph; +class GaussianHybridFactorGraph; /** * Hybrid Conditional Density @@ -146,7 +146,8 @@ class GTSAM_EXPORT HybridConditional * @return DiscreteConditional::shared_ptr */ DiscreteConditional::shared_ptr asDiscreteConditional() { - if (!isDiscrete()) throw std::invalid_argument("Not a discrete conditional"); + if (!isDiscrete()) + throw std::invalid_argument("Not a discrete conditional"); return boost::static_pointer_cast(inner_); } diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index ecac96724..148685327 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -21,17 +21,17 @@ namespace gtsam { // Instantiate base class -template class EliminationTree; +template class EliminationTree; /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const HybridFactorGraph& factorGraph, const VariableIndex& structure, - const Ordering& order) + const GaussianHybridFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order) : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const HybridFactorGraph& factorGraph, const Ordering& order) + const GaussianHybridFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 27766724a..04bd9cd35 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -17,8 +17,8 @@ #pragma once +#include #include -#include #include namespace gtsam { @@ -27,12 +27,12 @@ namespace gtsam { * Elimination Tree type for Hybrid */ class GTSAM_EXPORT HybridEliminationTree - : public EliminationTree { + : public EliminationTree { private: friend class ::EliminationTreeTester; public: - typedef EliminationTree + typedef EliminationTree Base; ///< Base class typedef HybridEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class @@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree * named constructor instead. * @return The elimination tree */ - HybridEliminationTree(const HybridFactorGraph& factorGraph, + HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order); /** Build the elimination tree of a factor graph. Note that this has to @@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree * this precomputed, use the other constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - HybridEliminationTree(const HybridFactorGraph& factorGraph, + HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, const Ordering& order); /// @} diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp index 057f784ad..476149126 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridISAM.cpp @@ -17,8 +17,8 @@ * @author Richard Roberts */ +#include #include -#include #include #include #include @@ -37,7 +37,7 @@ HybridISAM::HybridISAM() {} HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} /* ************************************************************************* */ -void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, +void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree @@ -90,7 +90,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridISAM::update(const HybridFactorGraph& newFactors, +void HybridISAM::update(const GaussianHybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function) { Cliques orphans; this->updateInternal(newFactors, &orphans, function); diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h index 0738f46c8..fae7efafd 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridISAM.h @@ -20,8 +20,8 @@ #pragma once #include +#include #include -#include #include namespace gtsam { @@ -46,7 +46,8 @@ class GTSAM_EXPORT HybridISAM : public ISAM { private: /// Internal method that performs the ISAM update. void updateInternal( - const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, + const GaussianHybridFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -57,7 +58,7 @@ class GTSAM_EXPORT HybridISAM : public ISAM { * @param newFactors Factor graph of new factors to add and eliminate. * @param function Elimination function. */ - void update(const HybridFactorGraph& newFactors, + void update(const GaussianHybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); }; diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 981cd6f32..8fa3aa033 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -15,8 +15,8 @@ * @author Fan Jiang */ +#include #include -#include #include #include #include @@ -26,13 +26,17 @@ namespace gtsam { // Instantiate base classes -template class EliminatableClusterTree; -template class JunctionTree; +template class EliminatableClusterTree; +template class JunctionTree; struct HybridConstructorTraversalData { - typedef typename JunctionTree::Node Node; - typedef typename JunctionTree::sharedNode - sharedNode; + typedef + typename JunctionTree::Node + Node; + typedef + typename JunctionTree::sharedNode sharedNode; HybridConstructorTraversalData* const parentData; sharedNode myJTNode; diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index 824fa4f85..ce9b818e6 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -17,8 +17,8 @@ #pragma once +#include #include -#include #include namespace gtsam { @@ -49,9 +49,9 @@ class HybridEliminationTree; * \nosubgrouping */ class GTSAM_EXPORT HybridJunctionTree - : public JunctionTree { + : public JunctionTree { public: - typedef JunctionTree + typedef JunctionTree Base; ///< Base class typedef HybridJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index d3ff98719..612f3abc5 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -98,15 +98,15 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class HybridFactorGraph { - HybridFactorGraph(); - HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); +#include +class GaussianHybridFactorGraph { + GaussianHybridFactorGraph(); + GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); // Building the graph void push_back(const gtsam::HybridFactor* factor); void push_back(const gtsam::HybridConditional* conditional); - void push_back(const gtsam::HybridFactorGraph& graph); + void push_back(const gtsam::GaussianHybridFactorGraph& graph); void push_back(const gtsam::HybridBayesNet& bayesNet); void push_back(const gtsam::HybridBayesTree& bayesTree); void push_back(const gtsam::GaussianMixtureFactor* gmm); @@ -120,13 +120,13 @@ class HybridFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "") const; - bool equals(const gtsam::HybridFactorGraph& fg, double tol = 1e-9) const; + bool equals(const gtsam::GaussianHybridFactorGraph& fg, double tol = 1e-9) const; gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential( gtsam::Ordering::OrderingType type); gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - pair + pair eliminatePartialSequential(const gtsam::Ordering& ordering); gtsam::HybridBayesTree* eliminateMultifrontal(); @@ -134,7 +134,7 @@ class HybridFactorGraph { gtsam::Ordering::OrderingType type); gtsam::HybridBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering); - pair + pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); string dot( diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 1a4db7898..77d8182c8 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -18,8 +18,8 @@ #include #include +#include #include -#include #include #include @@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C; using gtsam::symbol_shorthand::X; namespace gtsam { -inline HybridFactorGraph::shared_ptr makeSwitchingChain( +inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { - HybridFactorGraph hfg; + GaussianHybridFactorGraph hfg; hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); @@ -51,7 +51,7 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain( } } - return boost::make_shared(std::move(hfg)); + return boost::make_shared(std::move(hfg)); } inline std::pair> makeBinaryOrdering( diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp similarity index 93% rename from gtsam/hybrid/tests/testHybridFactorGraph.cpp rename to gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 860fdfdb8..853353278 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testHybridFactorGraph.cpp + * @file testGaussianHybridFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -27,7 +28,6 @@ #include #include #include -#include #include #include #include @@ -72,10 +72,10 @@ void my_signal_handler(int signum) { #endif /* ************************************************************************* */ -TEST(HybridFactorGraph, creation) { +TEST(GaussianHybridFactorGraph, creation) { HybridConditional test; - HybridFactorGraph hfg; + GaussianHybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -91,8 +91,8 @@ TEST(HybridFactorGraph, creation) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminate) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminate) { + GaussianHybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -102,8 +102,8 @@ TEST(HybridFactorGraph, eliminate) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateMultifrontal) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { + GaussianHybridFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -119,8 +119,8 @@ TEST(HybridFactorGraph, eliminateMultifrontal) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { + GaussianHybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -143,8 +143,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullSequentialSimple) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { + GaussianHybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -171,8 +171,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { + GaussianHybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -204,8 +204,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { + GaussianHybridFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -240,8 +240,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ -TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { + GaussianHybridFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); @@ -290,7 +290,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); @@ -309,7 +309,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(HybridFactorGraph, Switching) { +TEST(GaussianHybridFactorGraph, Switching) { auto N = 12; auto hfg = makeSwitchingChain(N); @@ -381,7 +381,7 @@ TEST(HybridFactorGraph, Switching) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -417,7 +417,7 @@ TEST(HybridFactorGraph, Switching) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(HybridFactorGraph, SwitchingISAM) { +TEST(GaussianHybridFactorGraph, SwitchingISAM) { auto N = 11; auto hfg = makeSwitchingChain(N); @@ -473,7 +473,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -502,7 +502,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { auto isam = HybridISAM(*hbt); { - HybridFactorGraph factorGraph; + GaussianHybridFactorGraph factorGraph; factorGraph.push_back(new_fg->at(new_fg->size() - 2)); factorGraph.push_back(new_fg->at(new_fg->size() - 1)); isam.update(factorGraph); @@ -512,7 +512,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, SwitchingTwoVar) { +TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); @@ -582,7 +582,7 @@ TEST(HybridFactorGraph, SwitchingTwoVar) { } { HybridBayesNet::shared_ptr hbn; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::shared_ptr remaining; std::tie(hbn, remaining) = hfg->eliminatePartialSequential(ordering_partial); diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index bbf1b2daa..e8d918a1d 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -9,7 +9,7 @@ namespace gtsam { #include #include #include -#include +#include #include @@ -107,36 +107,36 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> static gtsam::Ordering ColamdConstrainedLast( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> static gtsam::Ordering ColamdConstrainedFirst( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, const FACTOR_GRAPH& graph); diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 144183816..895c9e14e 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase -class TestHybridFactorGraph(GtsamTestCase): - """Unit tests for HybridFactorGraph.""" +class TestGaussianHybridFactorGraph(GtsamTestCase): + """Unit tests for GaussianHybridFactorGraph.""" def test_create(self): """Test contruction of hybrid factor graph.""" @@ -36,13 +36,14 @@ class TestHybridFactorGraph(GtsamTestCase): gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) - hfg = gtsam.HybridFactorGraph() + hfg = gtsam.GaussianHybridFactorGraph() hfg.add(jf1) hfg.add(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)])) + gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph( + hfg, [C(0)])) # print("hbn = ", hbn) self.assertEqual(hbn.size(), 2) From 841e6b01df76685ca8357305906731d32e2963b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 May 2022 18:03:52 -0400 Subject: [PATCH 59/70] improved equality checks --- gtsam/hybrid/GaussianMixtureConditional.cpp | 3 ++- gtsam/hybrid/GaussianMixtureFactor.cpp | 3 ++- gtsam/hybrid/GaussianMixtureFactor.h | 10 ++++++---- gtsam/hybrid/HybridConditional.cpp | 3 ++- gtsam/hybrid/HybridDiscreteFactor.cpp | 4 +++- gtsam/hybrid/HybridFactor.cpp | 5 ++++- gtsam/hybrid/HybridGaussianFactor.cpp | 15 ++++++++++----- gtsam/hybrid/HybridGaussianFactor.h | 3 ++- 8 files changed, 31 insertions(+), 15 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp index 726af6d5f..81d1244c2 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixtureConditional.cpp @@ -79,7 +79,8 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const { /* *******************************************************************************/ bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const { - return BaseFactor::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && BaseFactor::equals(*e, tol); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 589e5c660..a81cf341d 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -34,7 +34,8 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && Base::equals(*e, tol); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index b2fbe4aef..b3569183b 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -11,7 +11,7 @@ /** * @file GaussianMixtureFactor.h - * @brief A factor that is a function of discrete and continuous variables. + * @brief A set of GaussianFactors, indexed by a set of discrete keys. * @author Fan Jiang * @author Varun Agrawal * @author Frank Dellaert @@ -32,9 +32,10 @@ class GaussianFactorGraph; using GaussianFactorVector = std::vector; /** - * @brief A linear factor that is a function of both discrete and continuous - * variables, i.e. P(X, M | Z) where X is the set of continuous variables, M is - * the set of discrete variables and Z is the measurement set. + * @brief Implementation of a discrete conditional mixture factor. + * Implements a joint discrete-continuous factor where the discrete variable + * serves to "select" a mixture component corresponding to a GaussianFactor type + * of measurement. * * Represents the underlying Gaussian Mixture as a Decision Tree, where the set * of discrete variables indexes to the continuous gaussian distribution. @@ -52,6 +53,7 @@ class GaussianMixtureFactor : public HybridFactor { using Factors = DecisionTree; private: + /// Decision tree of Gaussian factors indexed by discrete keys. Factors factors_; /** diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7d1b72067..e3932308b 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -101,7 +101,8 @@ void HybridConditional::print(const std::string &s, /* ************************************************************************ */ bool HybridConditional::equals(const HybridFactor &other, double tol) const { - return BaseFactor::equals(other, tol); + const This *e = dynamic_cast(&other); + return e != nullptr && BaseFactor::equals(*e, tol); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 989127a28..2bdcdee8c 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -38,7 +38,9 @@ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) /* ************************************************************************ */ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + // TODO(Varun) How to compare inner_ when they are abstract types? + return e != nullptr && Base::equals(*e, tol); } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 815ea0415..127c9761c 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -70,7 +70,10 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) /* ************************************************************************ */ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && Base::equals(*e, tol) && + isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ && + isHybrid_ == e->isHybrid_ && nrContinuous_ == e->nrContinuous_; } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 721cb4cc7..59d20fb79 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -21,18 +21,23 @@ namespace gtsam { +/* ************************************************************************* */ HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) - : Base(other->keys()) { - inner_ = other; -} + : Base(other->keys()), inner_(other) {} +/* ************************************************************************* */ HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner_(boost::make_shared(std::move(jf))) {} -bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { - return false; +/* ************************************************************************* */ +bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { + const This *e = dynamic_cast(&other); + // TODO(Varun) How to compare inner_ when they are abstract types? + return e != nullptr && Base::equals(*e, tol); } + +/* ************************************************************************* */ void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 8d457e778..6fa83b726 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -25,7 +25,8 @@ namespace gtsam { /** * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have - * a diamond inheritance. + * a diamond inheritance i.e. an extra factor type that inherits from both + * HybridFactor and GaussianFactor. */ class HybridGaussianFactor : public HybridFactor { private: From 7c7b5dd03096c1e1516a964563b0b840a2b77a45 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jun 2022 23:52:21 -0400 Subject: [PATCH 60/70] Rename files so that everything is HybridX --- gtsam/hybrid/HybridBayesTree.cpp | 5 +- gtsam/hybrid/HybridBayesTree.h | 9 +-- gtsam/hybrid/HybridConditional.h | 4 +- gtsam/hybrid/HybridEliminationTree.cpp | 6 +- gtsam/hybrid/HybridEliminationTree.h | 10 +-- ...raph.cpp => HybridGaussianFactorGraph.cpp} | 24 +++---- ...torGraph.h => HybridGaussianFactorGraph.h} | 33 ++++----- ...{HybridISAM.cpp => HybridGaussianISAM.cpp} | 22 +++--- .../{HybridISAM.h => HybridGaussianISAM.h} | 18 ++--- ...ree.cpp => HybridGaussianJunctionTree.cpp} | 70 +++++++++---------- ...ionTree.h => HybridGaussianJunctionTree.h} | 14 ++-- gtsam/hybrid/hybrid.i | 16 ++--- gtsam/hybrid/tests/Switching.h | 8 +-- .../tests/testGaussianHybridFactorGraph.cpp | 70 ++++++++----------- gtsam/inference/inference.i | 14 ++-- python/gtsam/tests/test_HybridFactorGraph.py | 8 +-- 16 files changed, 161 insertions(+), 170 deletions(-) rename gtsam/hybrid/{GaussianHybridFactorGraph.cpp => HybridGaussianFactorGraph.cpp} (94%) rename gtsam/hybrid/{GaussianHybridFactorGraph.h => HybridGaussianFactorGraph.h} (77%) rename gtsam/hybrid/{HybridISAM.cpp => HybridGaussianISAM.cpp} (84%) rename gtsam/hybrid/{HybridISAM.h => HybridGaussianISAM.h} (76%) rename gtsam/hybrid/{HybridJunctionTree.cpp => HybridGaussianJunctionTree.cpp} (73%) rename gtsam/hybrid/{HybridJunctionTree.h => HybridGaussianJunctionTree.h} (84%) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 400cac5e7..ff07f1817 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -11,7 +11,8 @@ /** * @file HybridBayesTree.cpp - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridGaussianJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ @@ -26,7 +27,7 @@ namespace gtsam { // Instantiate base class template class BayesTreeCliqueBase; + HybridGaussianFactorGraph>; template class BayesTree; /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 610181458..28d9ef809 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -11,15 +11,16 @@ /** * @file HybridBayesTree.h - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridGaussianJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ #pragma once -#include #include +#include #include #include #include @@ -38,10 +39,10 @@ class VectorValues; */ class GTSAM_EXPORT HybridBayesTreeClique : public BayesTreeCliqueBase { + HybridGaussianFactorGraph> { public: typedef HybridBayesTreeClique This; - typedef BayesTreeCliqueBase + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 9592b0c69..887b49f12 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,9 +18,9 @@ #pragma once #include -#include #include #include +#include #include #include #include @@ -34,7 +34,7 @@ namespace gtsam { -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; /** * Hybrid Conditional Density diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index 148685327..c2df2dd60 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -21,17 +21,17 @@ namespace gtsam { // Instantiate base class -template class EliminationTree; +template class EliminationTree; /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const GaussianHybridFactorGraph& factorGraph, + const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order) : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const GaussianHybridFactorGraph& factorGraph, const Ordering& order) + const HybridGaussianFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 04bd9cd35..77a84fea8 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -17,8 +17,8 @@ #pragma once -#include #include +#include #include namespace gtsam { @@ -27,12 +27,12 @@ namespace gtsam { * Elimination Tree type for Hybrid */ class GTSAM_EXPORT HybridEliminationTree - : public EliminationTree { + : public EliminationTree { private: friend class ::EliminationTreeTester; public: - typedef EliminationTree + typedef EliminationTree Base; ///< Base class typedef HybridEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class @@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree * named constructor instead. * @return The elimination tree */ - HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order); /** Build the elimination tree of a factor graph. Note that this has to @@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree * this precomputed, use the other constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const Ordering& order); /// @} diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp similarity index 94% rename from gtsam/hybrid/GaussianHybridFactorGraph.cpp rename to gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3354d5b4d..0ac2c2656 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.cpp + * @file HybridGaussianFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang * @author Varun Agrawal @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -31,7 +30,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -53,7 +53,7 @@ namespace gtsam { -template class EliminateableFactorGraph; +template class EliminateableFactorGraph; /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( @@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian( /* ************************************************************************ */ std::pair -continuousElimination(const GaussianHybridFactorGraph &factors, +continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { @@ -103,7 +103,7 @@ continuousElimination(const GaussianHybridFactorGraph &factors, /* ************************************************************************ */ std::pair -discreteElimination(const GaussianHybridFactorGraph &factors, +discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { @@ -129,7 +129,7 @@ discreteElimination(const GaussianHybridFactorGraph &factors, /* ************************************************************************ */ std::pair -hybridElimination(const GaussianHybridFactorGraph &factors, +hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, const KeySet &continuousSeparator, const std::set &discreteSeparatorSet) { @@ -236,7 +236,7 @@ hybridElimination(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ std::pair // -EliminateHybrid(const GaussianHybridFactorGraph &factors, +EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only // a few cases: @@ -345,22 +345,22 @@ EliminateHybrid(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor &&factor) { +void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h similarity index 77% rename from gtsam/hybrid/GaussianHybridFactorGraph.h rename to gtsam/hybrid/HybridGaussianFactorGraph.h index c8e0718fc..6944af510 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.h + * @file HybridGaussianFactorGraph.h * @brief Linearized Hybrid factor graph that uses type erasure * @author Fan Jiang * @date Mar 11, 2022 @@ -26,36 +26,37 @@ namespace gtsam { // Forward declarations -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; class HybridConditional; class HybridBayesNet; class HybridEliminationTree; class HybridBayesTree; -class HybridJunctionTree; +class HybridGaussianJunctionTree; class DecisionTreeFactor; class JacobianFactor; -/** Main elimination function for GaussianHybridFactorGraph */ +/** Main elimination function for HybridGaussianFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys); +EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ template <> -struct EliminationTraits { +struct EliminationTraits { typedef HybridFactor FactorType; ///< Type of factors in factor graph - typedef GaussianHybridFactorGraph + typedef HybridGaussianFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. - ///< GaussianHybridFactorGraph) + ///< HybridGaussianFactorGraph) typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination typedef HybridBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination typedef HybridEliminationTree - EliminationTreeType; ///< Type of elimination tree - typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree - typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree + EliminationTreeType; ///< Type of elimination tree + typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree + typedef HybridGaussianJunctionTree + JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function static std::pair, boost::shared_ptr > @@ -70,12 +71,12 @@ struct EliminationTraits { * This is the linearized version of a hybrid factor graph. * Everything inside needs to be hybrid factor or hybrid conditional. */ -class GaussianHybridFactorGraph +class HybridGaussianFactorGraph : public FactorGraph, - public EliminateableFactorGraph { + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = GaussianHybridFactorGraph; ///< this class + using This = HybridGaussianFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This @@ -86,7 +87,7 @@ class GaussianHybridFactorGraph /// @name Constructors /// @{ - GaussianHybridFactorGraph() = default; + HybridGaussianFactorGraph() = default; /** * Implicit copy/downcast constructor to override explicit template container @@ -94,7 +95,7 @@ class GaussianHybridFactorGraph * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template - GaussianHybridFactorGraph(const FactorGraph& graph) + HybridGaussianFactorGraph(const FactorGraph& graph) : Base(graph) {} /// @} diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp similarity index 84% rename from gtsam/hybrid/HybridISAM.cpp rename to gtsam/hybrid/HybridGaussianISAM.cpp index 476149126..7783a88dd 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -10,16 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert * @author Richard Roberts */ -#include #include -#include +#include +#include #include #include @@ -31,15 +31,17 @@ namespace gtsam { // template class ISAM; /* ************************************************************************* */ -HybridISAM::HybridISAM() {} +HybridGaussianISAM::HybridGaussianISAM() {} /* ************************************************************************* */ -HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} +HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree) + : Base(bayesTree) {} /* ************************************************************************* */ -void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, - HybridBayesTree::Cliques* orphans, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::updateInternal( + const HybridGaussianFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, + const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree BayesNetType bn; const KeySet newFactorKeys = newFactors.keys(); @@ -90,8 +92,8 @@ void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridISAM::update(const GaussianHybridFactorGraph& newFactors, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, + const HybridBayesTree::Eliminate& function) { Cliques orphans; this->updateInternal(newFactors, &orphans, function); } diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridGaussianISAM.h similarity index 76% rename from gtsam/hybrid/HybridISAM.h rename to gtsam/hybrid/HybridGaussianISAM.h index fae7efafd..d5b6271da 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert @@ -20,33 +20,33 @@ #pragma once #include -#include #include +#include #include namespace gtsam { -class GTSAM_EXPORT HybridISAM : public ISAM { +class GTSAM_EXPORT HybridGaussianISAM : public ISAM { public: typedef ISAM Base; - typedef HybridISAM This; + typedef HybridGaussianISAM This; typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ /** Create an empty Bayes Tree */ - HybridISAM(); + HybridGaussianISAM(); /** Copy constructor */ - HybridISAM(const HybridBayesTree& bayesTree); + HybridGaussianISAM(const HybridBayesTree& bayesTree); /// @} private: /// Internal method that performs the ISAM update. void updateInternal( - const GaussianHybridFactorGraph& newFactors, + const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -58,13 +58,13 @@ class GTSAM_EXPORT HybridISAM : public ISAM { * @param newFactors Factor graph of new factors to add and eliminate. * @param function Elimination function. */ - void update(const GaussianHybridFactorGraph& newFactors, + void update(const HybridGaussianFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); }; /// traits template <> -struct traits : public Testable {}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridGaussianJunctionTree.cpp similarity index 73% rename from gtsam/hybrid/HybridJunctionTree.cpp rename to gtsam/hybrid/HybridGaussianJunctionTree.cpp index 8fa3aa033..8ceb7c87b 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridGaussianJunctionTree.cpp @@ -10,14 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridJunctionTree.cpp + * @file HybridGaussianJunctionTree.cpp * @date Mar 11, 2022 * @author Fan Jiang */ -#include #include -#include +#include +#include #include #include @@ -27,19 +27,19 @@ namespace gtsam { // Instantiate base classes template class EliminatableClusterTree; -template class JunctionTree; + HybridGaussianFactorGraph>; +template class JunctionTree; struct HybridConstructorTraversalData { typedef - typename JunctionTree::Node + typename JunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; + HybridGaussianFactorGraph>::sharedNode sharedNode; HybridConstructorTraversalData* const parentData; - sharedNode myJTNode; + sharedNode junctionTreeNode; FastVector childSymbolicConditionals; FastVector childSymbolicFactors; KeySet discreteKeys; @@ -57,24 +57,24 @@ struct HybridConstructorTraversalData { // On the pre-order pass, before children have been visited, we just set up // a traversal data structure with its own JT node, and create a child // pointer in its parent. - HybridConstructorTraversalData myData = + HybridConstructorTraversalData data = HybridConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared(node->key, node->factors); - parentData.myJTNode->addChild(myData.myJTNode); + data.junctionTreeNode = boost::make_shared(node->key, node->factors); + parentData.junctionTreeNode->addChild(data.junctionTreeNode); for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { - myData.discreteKeys.insert(k.first); + data.discreteKeys.insert(k.first); } } - return myData; + return data; } // Post-order visitor function static void ConstructorTraversalVisitorPostAlg2( const boost::shared_ptr& ETreeNode, - const HybridConstructorTraversalData& myData) { + const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current // elimination tree node. We then check whether each of our elimination @@ -87,50 +87,50 @@ struct HybridConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; symbolicFactors.reserve(ETreeNode->factors.size() + - myData.childSymbolicFactors.size()); + data.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + symbolicFactors += data.childSymbolicFactors; Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - SymbolicConditional::shared_ptr myConditional; - SymbolicFactor::shared_ptr mySeparatorFactor; - boost::tie(myConditional, mySeparatorFactor) = + SymbolicConditional::shared_ptr conditional; + SymbolicFactor::shared_ptr separatorFactor; + boost::tie(conditional, separatorFactor) = internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back(myConditional); - myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - myData.parentData->discreteKeys.merge(myData.discreteKeys); + data.parentData->childSymbolicConditionals.push_back(conditional); + data.parentData->childSymbolicFactors.push_back(separatorFactor); + data.parentData->discreteKeys.merge(data.discreteKeys); - sharedNode node = myData.myJTNode; + sharedNode node = data.junctionTreeNode; const FastVector& childConditionals = - myData.childSymbolicConditionals; - node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size()); + data.childSymbolicConditionals; + node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. - const size_t myNrParents = myConditional->nrParents(); + const size_t nrParents = conditional->nrParents(); const size_t nrChildren = node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children - std::vector nrFrontals = node->nrFrontalsOfChildren(); + std::vector nrChildrenFrontals = node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); - size_t myNrFrontals = 1; + size_t nrFrontals = 1; for (size_t i = 0; i < nrChildren; i++) { // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + if (nrParents + nrFrontals == childConditionals[i]->nrParents()) { const bool myType = - myData.discreteKeys.exists(myConditional->frontals()[0]); + data.discreteKeys.exists(conditional->frontals()[0]); const bool theirType = - myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); + data.discreteKeys.exists(childConditionals[i]->frontals()[0]); if (myType == theirType) { // Increment number of frontal variables - myNrFrontals += nrFrontals[i]; + nrFrontals += nrChildrenFrontals[i]; merge[i] = true; } } @@ -142,7 +142,7 @@ struct HybridConstructorTraversalData { }; /* ************************************************************************* */ -HybridJunctionTree::HybridJunctionTree( +HybridGaussianJunctionTree::HybridGaussianJunctionTree( const HybridEliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, @@ -156,7 +156,7 @@ HybridJunctionTree::HybridJunctionTree( // as we go. Gather the created junction tree roots in a dummy Node. typedef HybridConstructorTraversalData Data; Data rootData(0); - rootData.myJTNode = + rootData.junctionTreeNode = boost::make_shared(); // Make a dummy node to gather // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, @@ -164,7 +164,7 @@ HybridJunctionTree::HybridJunctionTree( Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - this->addChildrenAsRoots(rootData.myJTNode); + this->addChildrenAsRoots(rootData.junctionTreeNode); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridGaussianJunctionTree.h similarity index 84% rename from gtsam/hybrid/HybridJunctionTree.h rename to gtsam/hybrid/HybridGaussianJunctionTree.h index ce9b818e6..314e7daa6 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridGaussianJunctionTree.h @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridJunctionTree.h + * @file HybridGaussianJunctionTree.h * @date Mar 11, 2022 * @author Fan Jiang */ #pragma once -#include #include +#include #include namespace gtsam { @@ -48,12 +48,12 @@ class HybridEliminationTree; * \addtogroup Multifrontal * \nosubgrouping */ -class GTSAM_EXPORT HybridJunctionTree - : public JunctionTree { +class GTSAM_EXPORT HybridGaussianJunctionTree + : public JunctionTree { public: - typedef JunctionTree + typedef JunctionTree Base; ///< Base class - typedef HybridJunctionTree This; ///< This class + typedef HybridGaussianJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** @@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridJunctionTree * named constructor instead. * @return The elimination tree */ - HybridJunctionTree(const HybridEliminationTree& eliminationTree); + HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree); }; } // namespace gtsam diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 612f3abc5..94eaa88b2 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -98,15 +98,15 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class GaussianHybridFactorGraph { - GaussianHybridFactorGraph(); - GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); +#include +class HybridGaussianFactorGraph { + HybridGaussianFactorGraph(); + HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet); // Building the graph void push_back(const gtsam::HybridFactor* factor); void push_back(const gtsam::HybridConditional* conditional); - void push_back(const gtsam::GaussianHybridFactorGraph& graph); + void push_back(const gtsam::HybridGaussianFactorGraph& graph); void push_back(const gtsam::HybridBayesNet& bayesNet); void push_back(const gtsam::HybridBayesTree& bayesTree); void push_back(const gtsam::GaussianMixtureFactor* gmm); @@ -120,13 +120,13 @@ class GaussianHybridFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "") const; - bool equals(const gtsam::GaussianHybridFactorGraph& fg, double tol = 1e-9) const; + bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const; gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential( gtsam::Ordering::OrderingType type); gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - pair + pair eliminatePartialSequential(const gtsam::Ordering& ordering); gtsam::HybridBayesTree* eliminateMultifrontal(); @@ -134,7 +134,7 @@ class GaussianHybridFactorGraph { gtsam::Ordering::OrderingType type); gtsam::HybridBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering); - pair + pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); string dot( diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 77d8182c8..c081b8e87 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include @@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C; using gtsam::symbol_shorthand::X; namespace gtsam { -inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( +inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); @@ -51,7 +51,7 @@ inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( } } - return boost::make_shared(std::move(hfg)); + return boost::make_shared(std::move(hfg)); } inline std::pair> makeBinaryOrdering( diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 853353278..8ff959d74 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testGaussianHybridFactorGraph.cpp + * @file testHybridGaussianFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -29,7 +28,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -57,25 +57,11 @@ using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::Y; -#ifdef HYBRID_DEBUG -#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED - -#include // ::signal, ::raise - -#include - -void my_signal_handler(int signum) { - ::signal(signum, SIG_DFL); - std::cout << boost::stacktrace::stacktrace(); - ::raise(SIGABRT); -} -#endif - /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, creation) { +TEST(HybridGaussianFactorGraph, creation) { HybridConditional test; - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -91,8 +77,8 @@ TEST(GaussianHybridFactorGraph, creation) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminate) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminate) { + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -102,8 +88,8 @@ TEST(GaussianHybridFactorGraph, eliminate) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateMultifrontal) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -119,8 +105,8 @@ TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -143,8 +129,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -171,8 +157,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -204,8 +190,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -240,8 +226,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); @@ -290,7 +276,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); @@ -309,7 +295,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, Switching) { +TEST(HybridGaussianFactorGraph, Switching) { auto N = 12; auto hfg = makeSwitchingChain(N); @@ -381,7 +367,7 @@ TEST(GaussianHybridFactorGraph, Switching) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -417,7 +403,7 @@ TEST(GaussianHybridFactorGraph, Switching) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, SwitchingISAM) { +TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto N = 11; auto hfg = makeSwitchingChain(N); @@ -473,7 +459,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -499,10 +485,10 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { } auto new_fg = makeSwitchingChain(12); - auto isam = HybridISAM(*hbt); + auto isam = HybridGaussianISAM(*hbt); { - GaussianHybridFactorGraph factorGraph; + HybridGaussianFactorGraph factorGraph; factorGraph.push_back(new_fg->at(new_fg->size() - 2)); factorGraph.push_back(new_fg->at(new_fg->size() - 1)); isam.update(factorGraph); @@ -512,7 +498,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { +TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); @@ -582,7 +568,7 @@ TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { } { HybridBayesNet::shared_ptr hbn; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbn, remaining) = hfg->eliminatePartialSequential(ordering_partial); diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index e8d918a1d..fbdd70fdf 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -9,7 +9,7 @@ namespace gtsam { #include #include #include -#include +#include #include @@ -107,36 +107,36 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedLast( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedFirst( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, const FACTOR_GRAPH& graph); diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 895c9e14e..07a8178e6 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase -class TestGaussianHybridFactorGraph(GtsamTestCase): - """Unit tests for GaussianHybridFactorGraph.""" +class TestHybridGaussianFactorGraph(GtsamTestCase): + """Unit tests for HybridGaussianFactorGraph.""" def test_create(self): """Test contruction of hybrid factor graph.""" @@ -36,13 +36,13 @@ class TestGaussianHybridFactorGraph(GtsamTestCase): gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) - hfg = gtsam.GaussianHybridFactorGraph() + hfg = gtsam.HybridGaussianFactorGraph() hfg.add(jf1) hfg.add(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph( + gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( hfg, [C(0)])) # print("hbn = ", hbn) From 31ab1a32f3b93c9c7d2e97d95e9bd1c47013ef73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:03:31 -0400 Subject: [PATCH 61/70] update description of GaussianMixtureConditional --- gtsam/hybrid/GaussianMixtureConditional.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index d12fa09d7..a743c00f3 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -27,13 +27,19 @@ namespace gtsam { /** - * @brief A conditional of gaussian mixtures indexed by discrete variables. + * @brief A conditional of gaussian mixtures indexed by discrete variables, as + * part of a Bayes Network. * * Represents the conditional density P(X | M, Z) where X is a continuous random - * variable, M is the discrete variable and Z is the set of measurements. + * variable, M is the selection of discrete variables corresponding to a subset + * of the Gaussian variables and Z is parent of this node + * + * The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2} + * |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing + * coefficient. * */ -class GaussianMixtureConditional +class GTSAM_EXPORT GaussianMixtureConditional : public HybridFactor, public Conditional { public: From d2029f3d0339175fbf8b1e4fe0734e9b8e5a37bb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:09:38 -0400 Subject: [PATCH 62/70] Add more information on conditionals requirement for GaussianMixture --- gtsam/hybrid/GaussianMixtureConditional.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index a743c00f3..ab0e2ce43 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -68,13 +68,17 @@ class GTSAM_EXPORT GaussianMixtureConditional /// Defaut constructor, mainly for serialization. GaussianMixtureConditional() = default; + /** - * @brief Construct a new GaussianMixtureConditional object + * @brief Construct a new GaussianMixtureConditional object. * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. * @param discreteParents the discrete parents. Will be placed last. - * @param conditionals a decision tree of GaussianConditionals. + * @param conditionals a decision tree of GaussianConditionals. The number of + * conditionals should be C^(number of discrete parents), where C is the + * cardinality of the DiscreteKeys in discreteParents, since the + * discreteParents will be used as the labels in the decision tree. */ GaussianMixtureConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, From c8bf9d350ca9ace6de6218607dabc9357b7340a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:13:02 -0400 Subject: [PATCH 63/70] rename GaussianMixtureConditional to GaussianMixture --- ...ureConditional.cpp => GaussianMixture.cpp} | 28 +++++++++---------- ...MixtureConditional.h => GaussianMixture.h} | 20 ++++++------- gtsam/hybrid/HybridConditional.cpp | 2 +- gtsam/hybrid/HybridConditional.h | 14 +++++----- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 +++--- gtsam/hybrid/hybrid.i | 8 +++--- .../tests/testGaussianHybridFactorGraph.cpp | 6 ++-- 7 files changed, 43 insertions(+), 43 deletions(-) rename gtsam/hybrid/{GaussianMixtureConditional.cpp => GaussianMixture.cpp} (80%) rename gtsam/hybrid/{GaussianMixtureConditional.h => GaussianMixture.h} (87%) diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixture.cpp similarity index 80% rename from gtsam/hybrid/GaussianMixtureConditional.cpp rename to gtsam/hybrid/GaussianMixture.cpp index 726af6d5f..1663236f0 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.cpp + * @file GaussianMixture.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -20,41 +20,41 @@ #include #include -#include +#include #include #include namespace gtsam { -GaussianMixtureConditional::GaussianMixtureConditional( +GaussianMixture::GaussianMixture( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const GaussianMixtureConditional::Conditionals &conditionals) + const GaussianMixture::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), discreteParents), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixtureConditional::Conditionals & -GaussianMixtureConditional::conditionals() { +const GaussianMixture::Conditionals & +GaussianMixture::conditionals() { return conditionals_; } /* *******************************************************************************/ -GaussianMixtureConditional GaussianMixtureConditional::FromConditionals( +GaussianMixture GaussianMixture::FromConditionals( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixtureConditional(continuousFrontals, continuousParents, + return GaussianMixture(continuousFrontals, continuousParents, discreteParents, dt); } /* *******************************************************************************/ -GaussianMixtureConditional::Sum GaussianMixtureConditional::add( - const GaussianMixtureConditional::Sum &sum) const { +GaussianMixture::Sum GaussianMixture::add( + const GaussianMixture::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -66,8 +66,8 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add( } /* *******************************************************************************/ -GaussianMixtureConditional::Sum -GaussianMixtureConditional::asGaussianFactorGraphTree() const { +GaussianMixture::Sum +GaussianMixture::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -77,13 +77,13 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -bool GaussianMixtureConditional::equals(const HybridFactor &lf, +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { return BaseFactor::equals(lf, tol); } /* *******************************************************************************/ -void GaussianMixtureConditional::print(const std::string &s, +void GaussianMixture::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; if (isContinuous()) std::cout << "Continuous "; diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixture.h similarity index 87% rename from gtsam/hybrid/GaussianMixtureConditional.h rename to gtsam/hybrid/GaussianMixture.h index ab0e2ce43..756e7b77b 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.h + * @file GaussianMixture.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -39,14 +39,14 @@ namespace gtsam { * coefficient. * */ -class GTSAM_EXPORT GaussianMixtureConditional +class GTSAM_EXPORT GaussianMixture : public HybridFactor, - public Conditional { + public Conditional { public: - using This = GaussianMixtureConditional; - using shared_ptr = boost::shared_ptr; + using This = GaussianMixture; + using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using BaseConditional = Conditional; /// Alias for DecisionTree of GaussianFactorGraphs using Sum = DecisionTree; @@ -67,10 +67,10 @@ class GTSAM_EXPORT GaussianMixtureConditional /// @{ /// Defaut constructor, mainly for serialization. - GaussianMixtureConditional() = default; + GaussianMixture() = default; /** - * @brief Construct a new GaussianMixtureConditional object. + * @brief Construct a new GaussianMixture object. * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. @@ -80,7 +80,7 @@ class GTSAM_EXPORT GaussianMixtureConditional * cardinality of the DiscreteKeys in discreteParents, since the * discreteParents will be used as the labels in the decision tree. */ - GaussianMixtureConditional(const KeyVector &continuousFrontals, + GaussianMixture(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const Conditionals &conditionals); @@ -107,7 +107,7 @@ class GTSAM_EXPORT GaussianMixtureConditional /* print utility */ void print( - const std::string &s = "GaussianMixtureConditional\n", + const std::string &s = "GaussianMixture\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7d1b72067..8f95caf34 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -54,7 +54,7 @@ HybridConditional::HybridConditional( /* ************************************************************************ */ HybridConditional::HybridConditional( - boost::shared_ptr gaussianMixture) + boost::shared_ptr gaussianMixture) : BaseFactor(KeyVector(gaussianMixture->keys().begin(), gaussianMixture->keys().begin() + gaussianMixture->nrContinuous()), diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 887b49f12..3ba5da393 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ class HybridGaussianFactorGraph; * As a type-erased variant of: * - DiscreteConditional * - GaussianConditional - * - GaussianMixtureConditional + * - GaussianMixture * * The reason why this is important is that `Conditional` is a CRTP class. * CRTP is static polymorphism such that all CRTP classes, while bearing the @@ -128,16 +128,16 @@ class GTSAM_EXPORT HybridConditional * HybridConditional. */ HybridConditional( - boost::shared_ptr gaussianMixture); + boost::shared_ptr gaussianMixture); /** - * @brief Return HybridConditional as a GaussianMixtureConditional + * @brief Return HybridConditional as a GaussianMixture * - * @return GaussianMixtureConditional::shared_ptr + * @return GaussianMixture::shared_ptr */ - GaussianMixtureConditional::shared_ptr asMixture() { + GaussianMixture::shared_ptr asMixture() { if (!isHybrid()) throw std::invalid_argument("Not a mixture"); - return boost::static_pointer_cast(inner_); + return boost::static_pointer_cast(inner_); } /** diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 0ac2c2656..f4f09701f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include @@ -207,8 +207,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors, const GaussianMixtureFactor::Factors &separatorFactors = pair.second; - // Create the GaussianMixtureConditional from the conditionals - auto conditional = boost::make_shared( + // Create the GaussianMixture from the conditionals + auto conditional = boost::make_shared( frontalKeys, keysOfSeparator, discreteSeparator, pair.first); // If there are no more continuous parents, then we should create here a @@ -262,7 +262,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, // Because of all these reasons, we carefully consider how to // implement the hybrid factors so that we do not get poor performance. - // The first thing is how to represent the GaussianMixtureConditional. + // The first thing is how to represent the GaussianMixture. // A very possible scenario is that the incoming factors will have different // levels of discrete keys. For example, imagine we are going to eliminate the // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 94eaa88b2..bbe1e2400 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor { gtsam::DefaultKeyFormatter) const; }; -#include -class GaussianMixtureConditional : gtsam::HybridFactor { - static GaussianMixtureConditional FromConditionals( +#include +class GaussianMixture : gtsam::HybridFactor { + static GaussianMixture FromConditionals( const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const std::vector& conditionalsList); - void print(string s = "GaussianMixtureConditional\n", + void print(string s = "GaussianMixture\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 8ff959d74..552bb18f5 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -65,9 +65,9 @@ TEST(HybridGaussianFactorGraph, creation) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GaussianMixtureConditional clgc( + GaussianMixture clgc( {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - GaussianMixtureConditional::Conditionals( + GaussianMixture::Conditionals( C(0), boost::make_shared(X(0), Z_3x1, I_3x3, X(1), I_3x3), From 932e65c7a2d418c711a67019fb8763da298dd97f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:39:52 -0400 Subject: [PATCH 64/70] Add GTSAM_EXPORT and Testable traits --- gtsam/hybrid/GaussianMixture.h | 10 +++++++--- gtsam/hybrid/GaussianMixtureFactor.h | 7 ++++++- gtsam/hybrid/HybridDiscreteFactor.h | 7 ++++++- gtsam/hybrid/HybridGaussianFactor.h | 7 ++++++- gtsam/hybrid/HybridGaussianFactorGraph.h | 2 +- 5 files changed, 26 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 756e7b77b..f805c76c6 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -81,9 +81,9 @@ class GTSAM_EXPORT GaussianMixture * discreteParents will be used as the labels in the decision tree. */ GaussianMixture(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const Conditionals &conditionals); + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const Conditionals &conditionals); /** * @brief Make a Gaussian Mixture from a list of Gaussian conditionals @@ -125,4 +125,8 @@ class GTSAM_EXPORT GaussianMixture Sum add(const Sum &sum) const; }; +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index b3569183b..21770f836 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -41,7 +41,7 @@ using GaussianFactorVector = std::vector; * of discrete variables indexes to the continuous gaussian distribution. * */ -class GaussianMixtureFactor : public HybridFactor { +class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { public: using Base = HybridFactor; using This = GaussianMixtureFactor; @@ -113,4 +113,9 @@ class GaussianMixtureFactor : public HybridFactor { Sum add(const Sum &sum) const; }; +// traits +template <> +struct traits : public Testable { +}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 572ddfbcd..9cbea8170 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -29,7 +29,7 @@ namespace gtsam { * us to hide the implementation of DiscreteFactor and thus avoid diamond * inheritance. */ -class HybridDiscreteFactor : public HybridFactor { +class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor { private: DiscreteFactor::shared_ptr inner_; @@ -61,4 +61,9 @@ class HybridDiscreteFactor : public HybridFactor { /// Return pointer to the internal discrete factor DiscreteFactor::shared_ptr inner() const { return inner_; } }; + +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6fa83b726..2a92c717c 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -28,7 +28,7 @@ namespace gtsam { * a diamond inheritance i.e. an extra factor type that inherits from both * HybridFactor and GaussianFactor. */ -class HybridGaussianFactor : public HybridFactor { +class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { private: GaussianFactor::shared_ptr inner_; @@ -59,4 +59,9 @@ class HybridGaussianFactor : public HybridFactor { GaussianFactor::shared_ptr inner() const { return inner_; } }; + +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 6944af510..e7dc3dfca 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -71,7 +71,7 @@ struct EliminationTraits { * This is the linearized version of a hybrid factor graph. * Everything inside needs to be hybrid factor or hybrid conditional. */ -class HybridGaussianFactorGraph +class GTSAM_EXPORT HybridGaussianFactorGraph : public FactorGraph, public EliminateableFactorGraph { public: From 2afa6781f80044d2841a4de436e42a5df8129c3b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 09:20:43 -0400 Subject: [PATCH 65/70] fix python test --- python/gtsam/tests/test_HybridFactorGraph.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 07a8178e6..781cfd924 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -49,7 +49,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): self.assertEqual(hbn.size(), 2) mixture = hbn.at(0).inner() - self.assertIsInstance(mixture, gtsam.GaussianMixtureConditional) + self.assertIsInstance(mixture, gtsam.GaussianMixture) self.assertEqual(len(mixture.keys()), 2) discrete_conditional = hbn.at(hbn.size() - 1).inner() From 3e10514846c8d32000ab6d51de296ce5722ad8f5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:45:12 -0400 Subject: [PATCH 66/70] if checks for dynamic_cast --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 42 +++++++++------------- 1 file changed, 16 insertions(+), 26 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index f4f09701f..99202ba6a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -82,23 +82,19 @@ continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { + if (auto ptr = boost::dynamic_pointer_cast(fp)) { gfg.push_back(ptr->inner()); + } else if (auto p = + boost::static_pointer_cast(fp)->inner()) { + gfg.push_back(boost::static_pointer_cast(p)); } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - gfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapped conditional - } + // It is an orphan wrapped conditional } } auto result = EliminatePreferCholesky(gfg, frontalKeys); - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); + return {boost::make_shared(result.first), + boost::make_shared(result.second)}; } /* ************************************************************************ */ @@ -107,24 +103,20 @@ discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { + if (auto ptr = boost::dynamic_pointer_cast(fp)) { dfg.push_back(ptr->inner()); + } else if (auto p = + boost::static_pointer_cast(fp)->inner()) { + dfg.push_back(boost::static_pointer_cast(p)); } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - dfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapper - } + // It is an orphan wrapper } } auto result = EliminateDiscrete(dfg, frontalKeys); - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); + return {boost::make_shared(result.first), + boost::make_shared(result.second)}; } /* ************************************************************************ */ @@ -146,13 +138,11 @@ hybridElimination(const HybridGaussianFactorGraph &factors, for (auto &f : factors) { if (f->isHybrid()) { - auto cgmf = boost::dynamic_pointer_cast(f); - if (cgmf) { + if (auto cgmf = boost::dynamic_pointer_cast(f)) { sum = cgmf->add(sum); } - auto gm = boost::dynamic_pointer_cast(f); - if (gm) { + if (auto gm = boost::dynamic_pointer_cast(f)) { sum = gm->asMixture()->add(sum); } From dd877479fa7a3fc124435408afcf186ccc46293e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:47:45 -0400 Subject: [PATCH 67/70] separate out summing of frontals into separate function --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 83 ++++++++++++---------- 1 file changed, 46 insertions(+), 37 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 99202ba6a..3fa6e3e6b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -76,6 +76,51 @@ static GaussianMixtureFactor::Sum &addGaussian( return sum; } +/* ************************************************************************ */ +GaussianMixtureFactor::Sum sumFrontals( + const HybridGaussianFactorGraph &factors) { + // sum out frontals, this is the factor on the separator + gttic(sum); + + GaussianMixtureFactor::Sum sum; + std::vector deferredFactors; + + for (auto &f : factors) { + if (f->isHybrid()) { + if (auto cgmf = boost::dynamic_pointer_cast(f)) { + sum = cgmf->add(sum); + } + + if (auto gm = boost::dynamic_pointer_cast(f)) { + sum = gm->asMixture()->add(sum); + } + + } else if (f->isContinuous()) { + deferredFactors.push_back( + boost::dynamic_pointer_cast(f)->inner()); + } else { + // We need to handle the case where the object is actually an + // BayesTreeOrphanWrapper! + auto orphan = boost::dynamic_pointer_cast< + BayesTreeOrphanWrapper>(f); + if (!orphan) { + auto &fr = *f; + throw std::invalid_argument( + std::string("factor is discrete in continuous elimination") + + typeid(fr).name()); + } + } + } + + for (auto &f : deferredFactors) { + sum = addGaussian(sum, f); + } + + gttoc(sum); + + return sum; +} + /* ************************************************************************ */ std::pair continuousElimination(const HybridGaussianFactorGraph &factors, @@ -131,43 +176,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, discreteSeparatorSet.end()); // sum out frontals, this is the factor on the separator - gttic(sum); - - GaussianMixtureFactor::Sum sum; - std::vector deferredFactors; - - for (auto &f : factors) { - if (f->isHybrid()) { - if (auto cgmf = boost::dynamic_pointer_cast(f)) { - sum = cgmf->add(sum); - } - - if (auto gm = boost::dynamic_pointer_cast(f)) { - sum = gm->asMixture()->add(sum); - } - - } else if (f->isContinuous()) { - deferredFactors.push_back( - boost::dynamic_pointer_cast(f)->inner()); - } else { - // We need to handle the case where the object is actually an - // BayesTreeOrphanWrapper! - auto orphan = boost::dynamic_pointer_cast< - BayesTreeOrphanWrapper>(f); - if (!orphan) { - auto &fr = *f; - throw std::invalid_argument( - std::string("factor is discrete in continuous elimination") + - typeid(fr).name()); - } - } - } - - for (auto &f : deferredFactors) { - sum = addGaussian(sum, f); - } - - gttoc(sum); + GaussianMixtureFactor::Sum sum = sumFrontals(factors); using EliminationPair = GaussianFactorGraph::EliminationResult; From 92176db645bed9374cf3a6fda677fbc8b59d31de Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:50:44 -0400 Subject: [PATCH 68/70] add comments --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3fa6e3e6b..c1251309c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -183,6 +183,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, KeyVector keysOfEliminated; // Not the ordering KeyVector keysOfSeparator; // TODO(frank): Is this just (keys - ordering)? + // This is the elimination method on the leaf nodes auto eliminate = [&](const GaussianFactorGraph &graph) -> GaussianFactorGraph::EliminationResult { if (graph.empty()) { @@ -200,8 +201,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, return result; }; + // Perform elimination! DecisionTree eliminationResults(sum, eliminate); + // Separate out decision tree into conditionals and remaining factors. auto pair = unzip(eliminationResults); const GaussianMixtureFactor::Factors &separatorFactors = pair.second; From b47cd9d97bec48b0c4598a61dcda16b407d11c0f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:55:46 -0400 Subject: [PATCH 69/70] update GaussianMixture docstring --- gtsam/hybrid/GaussianMixture.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index f805c76c6..e85506715 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -34,9 +34,10 @@ namespace gtsam { * variable, M is the selection of discrete variables corresponding to a subset * of the Gaussian variables and Z is parent of this node * - * The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2} - * |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing - * coefficient. + * The probability P(x|y,z,...) is proportional to + * \f$ \sum_i k_i \exp - \frac{1}{2} |R_i x - (d_i - S_i y - T_i z - ...)|^2 \f$ + * where i indexes the components and k_i is a component-wise normalization + * constant. * */ class GTSAM_EXPORT GaussianMixture From eeecb27f14aca16f38c46821f5a5f2f16a512ef9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Jun 2022 13:29:05 -0400 Subject: [PATCH 70/70] rename back to HybridJunctionTree --- gtsam/hybrid/HybridBayesTree.cpp | 2 +- gtsam/hybrid/HybridBayesTree.h | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.h | 4 ++-- ...ridGaussianJunctionTree.cpp => HybridJunctionTree.cpp} | 6 +++--- ...{HybridGaussianJunctionTree.h => HybridJunctionTree.h} | 8 ++++---- 6 files changed, 12 insertions(+), 12 deletions(-) rename gtsam/hybrid/{HybridGaussianJunctionTree.cpp => HybridJunctionTree.cpp} (97%) rename gtsam/hybrid/{HybridGaussianJunctionTree.h => HybridJunctionTree.h} (91%) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index ff07f1817..d65270f91 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -12,7 +12,7 @@ /** * @file HybridBayesTree.cpp * @brief Hybrid Bayes Tree, the result of eliminating a - * HybridGaussianJunctionTree + * HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 28d9ef809..0b89ca8c4 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -12,7 +12,7 @@ /** * @file HybridBayesTree.h * @brief Hybrid Bayes Tree, the result of eliminating a - * HybridGaussianJunctionTree + * HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c1251309c..88730cae9 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index e7dc3dfca..0188aa652 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -31,7 +31,7 @@ class HybridConditional; class HybridBayesNet; class HybridEliminationTree; class HybridBayesTree; -class HybridGaussianJunctionTree; +class HybridJunctionTree; class DecisionTreeFactor; class JacobianFactor; @@ -55,7 +55,7 @@ struct EliminationTraits { typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree - typedef HybridGaussianJunctionTree + typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function static std::pair, diff --git a/gtsam/hybrid/HybridGaussianJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp similarity index 97% rename from gtsam/hybrid/HybridGaussianJunctionTree.cpp rename to gtsam/hybrid/HybridJunctionTree.cpp index 8ceb7c87b..7725742cf 100644 --- a/gtsam/hybrid/HybridGaussianJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -10,14 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridGaussianJunctionTree.cpp + * @file HybridJunctionTree.cpp * @date Mar 11, 2022 * @author Fan Jiang */ #include #include -#include +#include #include #include @@ -142,7 +142,7 @@ struct HybridConstructorTraversalData { }; /* ************************************************************************* */ -HybridGaussianJunctionTree::HybridGaussianJunctionTree( +HybridJunctionTree::HybridJunctionTree( const HybridEliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, diff --git a/gtsam/hybrid/HybridGaussianJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h similarity index 91% rename from gtsam/hybrid/HybridGaussianJunctionTree.h rename to gtsam/hybrid/HybridJunctionTree.h index 314e7daa6..cad1e15a1 100644 --- a/gtsam/hybrid/HybridGaussianJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridGaussianJunctionTree.h + * @file HybridJunctionTree.h * @date Mar 11, 2022 * @author Fan Jiang */ @@ -48,12 +48,12 @@ class HybridEliminationTree; * \addtogroup Multifrontal * \nosubgrouping */ -class GTSAM_EXPORT HybridGaussianJunctionTree +class GTSAM_EXPORT HybridJunctionTree : public JunctionTree { public: typedef JunctionTree Base; ///< Base class - typedef HybridGaussianJunctionTree This; ///< This class + typedef HybridJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** @@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridGaussianJunctionTree * named constructor instead. * @return The elimination tree */ - HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree); + HybridJunctionTree(const HybridEliminationTree& eliminationTree); }; } // namespace gtsam