From 39286f667214a4f7fd88c224943bfb850d5e2a32 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 19 Dec 2021 10:41:07 -0500 Subject: [PATCH 001/121] added clone to play well with gnc --- gtsam_unstable/slam/PoseToPointFactor.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index cab48e506..b9b2ad5ce 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -61,6 +61,12 @@ class PoseToPointFactor : public NoiseModelFactor2 { traits::Equals(this->measured_, e->measured_, tol); } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + /** implement functions needed to derive from Factor */ /** vector of errors From c59fbc825fe2f77c96d2e8a1bd7c509c3bf7189c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 5 Jan 2022 23:01:57 -0500 Subject: [PATCH 002/121] ruled out corner case where weights are outside [0,1] in TLS --- gtsam/nonlinear/GncOptimizer.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..9601e5f8c 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -439,13 +439,11 @@ class GTSAM_EXPORT GncOptimizer { for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - if (u2_k >= upperbound) { + weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; + if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; - } else if (u2_k <= lowerbound) { + } else if (u2_k <= lowerbound || weights[k] > 1) { weights[k] = 1; - } else { - weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - - mu; } } } From 859e4cb37af125b9351db22dc86837077d5c62f9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 12 Jan 2022 21:31:22 -0500 Subject: [PATCH 003/121] thresholded mu to avoid case mu = 0 in TLS. improved verbosity handling --- gtsam/nonlinear/GncOptimizer.h | 25 +++++++++++++++++++------ gtsam/nonlinear/GncParams.h | 2 ++ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..6c8519aac 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -206,9 +206,11 @@ class GTSAM_EXPORT GncOptimizer { std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers" << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::MU) { + std::cout << "mu: " << mu << std::endl; + } if (params_.verbosity >= GncParameters::Verbosity::VALUES) { result.print("result\n"); - std::cout << "mu: " << mu << std::endl; } return result; } @@ -217,12 +219,16 @@ class GTSAM_EXPORT GncOptimizer { for (iter = 0; iter < params_.maxIterations; iter++) { // display info - if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::MU) { std::cout << "iter: " << iter << std::endl; - result.print("result\n"); std::cout << "mu: " << mu << std::endl; + } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { std::cout << "weights: " << weights_ << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + result.print("result\n"); + } // weights update weights_ = calculateWeights(result, mu); @@ -253,10 +259,12 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { + std::cout << "final weights: " << weights_ << std::endl; + } return result; } @@ -291,6 +299,9 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } + if (mu_init >= 0 && mu_init < 1e-6) + mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) @@ -338,8 +349,10 @@ class GTSAM_EXPORT GncOptimizer { bool checkCostConvergence(const double cost, const double prev_cost) const { bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) < params_.relativeCostTol; - if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) - std::cout << "checkCostConvergence = true " << std::endl; + if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){ + std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost + << ", curr. cost = " << cost << ")" << std::endl; + } return costConverged; } diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 086f08acc..1f324ae38 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams { enum Verbosity { SILENT = 0, SUMMARY, + MU, + WEIGHTS, VALUES }; From ea4ebb6843daec9cd42bb7579e431d6686d1d8f7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 11 Mar 2022 15:51:34 -0500 Subject: [PATCH 004/121] 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 005/121] 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 006/121] 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 007/121] 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 008/121] 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 009/121] 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 010/121] 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 011/121] 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 012/121] 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 013/121] 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 014/121] 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 015/121] 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 016/121] 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 017/121] 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 7b0356cbea59c3660c8e85e81f07917b007d0e9c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 22 Mar 2022 20:40:08 -0400 Subject: [PATCH 018/121] edited --- gtsam/nonlinear/GncOptimizer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index c5c5d1883..96b03e803 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -447,11 +447,11 @@ class GTSAM_EXPORT GncOptimizer { return weights; } case GncLossType::TLS: { // use eq (14) in GNC paper - double upperbound = (mu + 1) / mu * barcSq_.maxCoeff(); - double lowerbound = mu / (mu + 1) * barcSq_.minCoeff(); for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + double upperbound = (mu + 1) / mu * barcSq_[k]; + double lowerbound = mu / (mu + 1) * barcSq_[k]; weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; From d42fc214176fb8ced3436edc4c9662dc2f1ef65a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Mar 2022 20:16:05 -0400 Subject: [PATCH 019/121] 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 020/121] 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 021/121] 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 022/121] 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 023/121] 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 024/121] 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 025/121] 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 026/121] 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 027/121] 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 028/121] 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 029/121] 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 030/121] 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 031/121] 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 215682e64f4063911639320dcab209aab2f062fb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 7 Apr 2022 21:33:54 -0400 Subject: [PATCH 032/121] FIX: Robust loss error calculation --- gtsam/linear/NoiseModel.h | 6 +++++ tests/testRobust.cpp | 53 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 tests/testRobust.cpp diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 5c379beb8..5a29e5d7d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -697,6 +697,12 @@ namespace gtsam { return robust_->loss(std::sqrt(squared_distance)); } + // NOTE: This is special because in whiten the base version will do the reweighting + // which is incorrect! + double squaredMahalanobisDistance(const Vector& v) const override { + return noise_->squaredMahalanobisDistance(v); + } + // These are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; void WhitenSystem(std::vector& A, Vector& b) const override; diff --git a/tests/testRobust.cpp b/tests/testRobust.cpp new file mode 100644 index 000000000..ad361f6d9 --- /dev/null +++ b/tests/testRobust.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRobust.cpp + * @brief Unit tests for Robust loss functions + * @author Fan Jiang + * @author Yetong Zhang + * @date Apr 7, 2022 + **/ + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +TEST(RobustNoise, loss) { + // Keys. + gtsam::Key x1_key = 1; + gtsam::Key x2_key = 2; + + auto gm = noiseModel::mEstimator::GemanMcClure::Create(1.0); + auto noise = noiseModel::Robust::Create(gm, noiseModel::Unit::Create(1)); + + auto factor = PriorFactor(x1_key, 0.0, noise); + auto between_factor = BetweenFactor(x1_key, x2_key, 0.0, noise); + + Values values; + values.insert(x1_key, 10.0); + values.insert(x2_key, 0.0); + + EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5); + EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5); + EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5); +} + +int main() { + TestResult tr; + + return TestRegistry::runAllTests(tr); +} From 7a47815e0b0c69828a1c5ebf8f43ec9a38502b21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 8 Apr 2022 16:30:40 -0400 Subject: [PATCH 033/121] Fix the test --- gtsam/linear/tests/testNoiseModel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index b974b6cd5..92774a133 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -668,7 +668,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) for (int i = 0; i < 5; i++) { Vector3 error = Vector3(i, 0, 0); DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, - robust->squaredMahalanobisDistance(error), 1e-8); + robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8); } } From 2de3169976e876578c736e844605495dac6c7f30 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 19 Apr 2022 21:50:15 -0400 Subject: [PATCH 034/121] 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 035/121] 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 036/121] 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 fa743ae0ac861897bd8d63f7e7815314ec48d3c4 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 14 May 2022 00:18:04 -0700 Subject: [PATCH 037/121] intialize values that may have between factors --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++++---- gtsam/sfm/TranslationRecovery.h | 6 ++++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 810fe7de9..8f1108806 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -134,6 +134,7 @@ void TranslationRecovery::addPrior( Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly @@ -155,14 +156,19 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + for (auto edge : betweenTranslations) { + insert(edge.key1()); + insert(edge.key2()); + } return initial; } Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, - initialValues); + return initializeRandomly(relativeTranslations, betweenTranslations, + &kRandomNumberGenerator, initialValues); } Values TranslationRecovery::run( @@ -187,8 +193,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = - initializeRandomly(nonzeroRelativeTranslations, initialValues); + Values initial = initializeRandomly( + nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues); // If there are no valid edges, but zero-distance edges exist, initialize one // of the nodes in a connected component of zero-distance edges. diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 7863f5133..6ccc39ddb 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -112,12 +112,15 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param rng random number generator * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues = Values()) const; /** @@ -125,11 +128,14 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues = Values()) const; /** From 2c4990b613833a8fd38346a008965a60aac28dda Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 21 May 2022 14:23:05 -0700 Subject: [PATCH 038/121] 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 039/121] 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 040/121] 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 041/121] 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 042/121] 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 043/121] 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 044/121] 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 045/121] 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 046/121] 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 047/121] 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 10001f4cea1927d3f67f84770c41faa6d5e1b581 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 25 May 2022 17:52:49 -0400 Subject: [PATCH 048/121] use This for templated type in geometry.i --- gtsam/geometry/geometry.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8e3c93224..8079e2c2a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -873,7 +873,7 @@ template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); - PinholeCamera(const gtsam::PinholeCamera other); + PinholeCamera(const This other); PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, @@ -942,7 +942,7 @@ template class PinholePose { // Standard Constructors and Named Constructors PinholePose(); - PinholePose(const gtsam::PinholePose other); + PinholePose(const This other); PinholePose(const gtsam::Pose3& pose); PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K); static This Level(const gtsam::Pose2& pose, double height); From f95b449904d55b56f82f4befe962f8cd01eff485 Mon Sep 17 00:00:00 2001 From: "lilong.huang" Date: Thu, 26 May 2022 09:21:29 +0800 Subject: [PATCH 049/121] fix typo issue in comment --- gtsam/geometry/Rot3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8f8088a74..08213b065 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -292,8 +292,8 @@ pair RQ(const Matrix3& A, OptionalJacobian<3, 9> H) { (*H)(1, 8) = yHb22 * cx; // Next, calculate the derivate of z. We have - // c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy - // c22 = a11 * cx - a12 * sx + // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy + // c11 = a11 * cx - a12 * sx const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy; const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy; Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1); From c3a92a4705642f8708b610d4a5f9e81aba1ddcb9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 15:12:19 -0400 Subject: [PATCH 050/121] 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 051/121] 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 052/121] 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 053/121] 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 054/121] 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 055/121] 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 056/121] 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 057/121] 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 058/121] 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 059/121] 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 060/121] 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 061/121] 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 062/121] 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 063/121] 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 064/121] 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 065/121] 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 066/121] 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 067/121] 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 068/121] 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 069/121] 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 070/121] 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 071/121] 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 072/121] 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 073/121] 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 074/121] 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 075/121] 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 076/121] 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 077/121] 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 078/121] 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 079/121] 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 From 8482cee10b66f8bcc7c3674235c6fbbcb07373e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Jun 2022 13:53:25 -0400 Subject: [PATCH 080/121] improvements made during the old hybrid scheme implementation --- gtsam/discrete/DecisionTree-inl.h | 9 +++------ gtsam/discrete/DecisionTree.h | 2 +- gtsam/discrete/DiscreteFactorGraph.h | 4 ++++ gtsam/inference/FactorGraph.h | 2 +- gtsam/inference/Ordering.cpp | 11 +++++++++++ gtsam/inference/Ordering.h | 18 +++++++++++++++++- gtsam/inference/tests/testOrdering.cpp | 26 ++++++++++++++++++++++++++ gtsam/linear/GaussianBayesNet.h | 2 +- gtsam/linear/GaussianConditional.cpp | 4 ++-- gtsam/linear/GaussianConditional.h | 1 + 10 files changed, 67 insertions(+), 12 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 99f29b8e5..f72743206 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -39,10 +39,10 @@ #include #include -using boost::assign::operator+=; - namespace gtsam { + using boost::assign::operator+=; + /****************************************************************************/ // Node /****************************************************************************/ @@ -291,10 +291,7 @@ namespace gtsam { } os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; - if (B == 2) { - if (i == 0) os << " [style=dashed]"; - if (i > 1) os << " [style=bold]"; - } + if (B == 2 && i == 0) os << " [style=dashed]"; os << std::endl; branch->dot(os, labelFormatter, valueFormatter, showZero); } diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 1f45d320b..7449ae995 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -220,7 +220,7 @@ namespace gtsam { /// @{ /// Make virtual - virtual ~DecisionTree() {} + virtual ~DecisionTree() = default; /// Check if tree is empty. bool empty() const { return !root_; } diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index f962b1802..bfa57e7db 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph /// @} }; // \ DiscreteFactorGraph +std::pair // +EliminateForMPE(const DiscreteFactorGraph& factors, + const Ordering& frontalKeys); + /// traits template <> struct traits : public Testable {}; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index afea63da8..101134c83 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -361,7 +361,7 @@ class FactorGraph { * less than the original, factors at the end will be removed. If the new * size is larger than the original, null factors will be appended. */ - void resize(size_t size) { factors_.resize(size); } + virtual void resize(size_t size) { factors_.resize(size); } /** delete factor without re-arranging indexes by inserting a nullptr pointer */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 2ac2c0dde..7b7ff1403 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -283,6 +283,17 @@ void Ordering::print(const std::string& str, cout.flush(); } +/* ************************************************************************* */ +Ordering::This& Ordering::operator+=(KeyVector& keys) { + this->insert(this->end(), keys.begin(), keys.end()); + return *this; +} + +/* ************************************************************************* */ +bool Ordering::contains(const Key& key) const { + return std::find(this->begin(), this->end(), key) != this->end(); +} + /* ************************************************************************* */ bool Ordering::equals(const Ordering& other, double tol) const { return (*this) == other; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index a2d165792..b366b3b3a 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -70,7 +70,23 @@ public: boost::assign_detail::call_push_back(*this))(key); } - /// Invert (not reverse) the ordering - returns a map from key to order position + /** + * @brief Append new keys to the ordering as `ordering += keys`. + * + * @param key + * @return The ordering variable with appended keys. + */ + This& operator+=(KeyVector& keys); + + /// Check if key exists in ordering. + bool contains(const Key& key) const; + + /** + * @brief Invert (not reverse) the ordering - returns a map from key to order + * position. + * + * @return FastMap + */ FastMap invert() const; /// @name Fill-reducing Orderings @{ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 6fdca0d89..1afa1cfde 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -199,6 +199,32 @@ TEST(Ordering, csr_format_3) { EXPECT(adjExpected == adjAcutal); } +/* ************************************************************************* */ +TEST(Ordering, AppendVector) { + using symbol_shorthand::X; + Ordering actual; + KeyVector keys = {X(0), X(1), X(2)}; + actual += keys; + + Ordering expected; + expected += X(0); + expected += X(1); + expected += X(2); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Ordering, Contains) { + using symbol_shorthand::X; + Ordering ordering; + ordering += X(0); + ordering += X(1); + ordering += X(2); + + EXPECT(ordering.contains(X(1))); + EXPECT(!ordering.contains(X(4))); +} + /* ************************************************************************* */ #ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, csr_format_4) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 940ffd882..e4f2950ed 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -45,7 +45,7 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Construct empty factor graph */ + /** Construct empty bayes net */ GaussianBayesNet() {} /** Construct from iterator over conditionals */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 6199f91a7..951577641 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -15,10 +15,10 @@ * @author Christian Potthast, Frank Dellaert */ -#include #include -#include #include +#include +#include #include #ifdef __GNUC__ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index b2b616dab..11fe1f318 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include // for std::mt19937_64 From 2396bca22f4c3345e6a76f74a71ee0073a68cf4b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 10:07:53 -0400 Subject: [PATCH 081/121] Add GaussianMixture tests --- gtsam/hybrid/GaussianMixture.cpp | 27 ++++++- gtsam/hybrid/GaussianMixture.h | 12 +++ gtsam/hybrid/tests/testGaussianMixture.cpp | 92 ++++++++++++++++++++++ 3 files changed, 128 insertions(+), 3 deletions(-) create mode 100644 gtsam/hybrid/tests/testGaussianMixture.cpp diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 000057518..3d4eb79fa 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -19,7 +19,7 @@ */ #include -#include +#include #include #include #include @@ -77,8 +77,29 @@ GaussianMixture::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -bool GaussianMixture::equals(const HybridFactor &lf, - double tol) const { +size_t GaussianMixture::nrComponents() const { + size_t total = 0; + conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) { + if (node) total += 1; + }); + return total; +} + +/* *******************************************************************************/ +GaussianConditional::shared_ptr GaussianMixture::operator()( + const DiscreteValues &discreteVals) const { + auto &ptr = conditionals_(discreteVals); + if (!ptr) return nullptr; + auto conditional = boost::dynamic_pointer_cast(ptr); + if (conditional) + return conditional; + else + throw std::logic_error( + "A GaussianMixture unexpectedly contained a non-conditional"); +} + +/* *******************************************************************************/ +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { const This *e = dynamic_cast(&lf); return e != nullptr && BaseFactor::equals(*e, tol); } diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index e85506715..fc1eb0f06 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -19,7 +19,9 @@ #pragma once +#include #include +#include #include #include #include @@ -99,6 +101,16 @@ class GTSAM_EXPORT GaussianMixture const DiscreteKeys &discreteParents, const std::vector &conditionals); + /// @} + /// @name Standard API + /// @{ + + GaussianConditional::shared_ptr operator()( + const DiscreteValues &discreteVals) const; + + /// Returns the total number of continuous components + size_t nrComponents() const; + /// @} /// @name Testable /// @{ diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp new file mode 100644 index 000000000..8a91a86cc --- /dev/null +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussianMixture.cpp + * @brief Unit tests for GaussianMixture class + * @author Varun Agrawal + * @author Fan Jiang + * @author Frank Dellaert + * @date December 2021 + */ + +#include +#include +#include +#include + +#include + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(GaussianConditional, Equals) { + // create a conditional gaussian node + Matrix S1(2, 2); + S1(0, 0) = 1; + S1(1, 0) = 2; + S1(0, 1) = 3; + S1(1, 1) = 4; + + Matrix S2(2, 2); + S2(0, 0) = 6; + S2(1, 0) = 0.2; + S2(0, 1) = 8; + S2(1, 1) = 0.4; + + Matrix R1(2, 2); + R1(0, 0) = 0.1; + R1(1, 0) = 0.3; + R1(0, 1) = 0.0; + R1(1, 1) = 0.34; + + Matrix R2(2, 2); + R2(0, 0) = 0.1; + R2(1, 0) = 0.3; + R2(0, 1) = 0.0; + R2(1, 1) = 0.34; + + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); + + Vector2 d1(0.2, 0.5), d2(0.5, 0.2); + + auto conditional0 = boost::make_shared(X(1), d1, R1, + X(2), S1, model), + conditional1 = boost::make_shared(X(1), d2, R2, + X(2), S2, model); + + // Create decision tree + DiscreteKey m1(1, 2); + GaussianMixture::Conditionals conditionals( + {m1}, + vector{conditional0, conditional1}); + GaussianMixture mixtureFactor({X(1)}, {X(2)}, {m1}, conditionals); + + // Let's check that this worked: + DiscreteValues mode; + mode[m1.first] = 1; + auto actual = mixtureFactor(mode); + EXPECT(actual == conditional1); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ \ No newline at end of file From fc939b08e045d443a31858db107f676b693a3948 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 10:48:08 -0400 Subject: [PATCH 082/121] GaussianMixtureFactor tests --- gtsam/hybrid/GaussianMixtureFactor.cpp | 7 +- gtsam/hybrid/GaussianMixtureFactor.h | 19 +++ gtsam/hybrid/HybridFactor.cpp | 23 ++- gtsam/hybrid/HybridFactor.h | 3 + .../tests/testGaussianMixtureFactor.cpp | 159 ++++++++++++++++++ 5 files changed, 205 insertions(+), 6 deletions(-) create mode 100644 gtsam/hybrid/tests/testGaussianMixtureFactor.cpp diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index a81cf341d..8f832d8ea 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -51,16 +51,19 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactors( void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); + std::cout << "]{\n"; factors_.print( - "mixture = ", [&](Key k) { return formatter(k); }, + "", [&](Key k) { return formatter(k); }, [&](const GaussianFactor::shared_ptr &gf) -> std::string { RedirectCout rd; - if (!gf->empty()) + std::cout << ":\n"; + if (gf) gf->print("", formatter); else return {"nullptr"}; return rd.str(); }); + std::cout << "}" << std::endl; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 21770f836..6c90ee6a7 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -84,6 +84,19 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { const DiscreteKeys &discreteKeys, const Factors &factors); + /** + * @brief Construct a new GaussianMixtureFactor object using a vector of + * GaussianFactor shared pointers. + * + * @param keys Vector of keys for continuous factors. + * @param discreteKeys Vector of discrete keys. + * @param factors Vector of gaussian factor shared pointers. + */ + GaussianMixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, + const std::vector &factors) + : GaussianMixtureFactor(keys, discreteKeys, + Factors(discreteKeys, factors)) {} + static This FromFactors( const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector &factors); @@ -111,6 +124,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @return Sum */ Sum add(const Sum &sum) const; + + /// Add MixtureFactor to a Sum, syntactic sugar. + friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) { + sum = factor.add(sum); + return sum; + } }; // traits diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 127c9761c..8df2d524f 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -50,7 +50,10 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &keys) - : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {} + : Base(keys), + isContinuous_(true), + nrContinuous_(keys.size()), + continuousKeys_(keys) {} /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &continuousKeys, @@ -60,13 +63,15 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys, isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), nrContinuous_(continuousKeys.size()), - discreteKeys_(discreteKeys) {} + discreteKeys_(discreteKeys), + continuousKeys_(continuousKeys) {} /* ************************************************************************ */ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true), - discreteKeys_(discreteKeys) {} + discreteKeys_(discreteKeys), + continuousKeys_({}) {} /* ************************************************************************ */ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { @@ -83,7 +88,17 @@ void HybridFactor::print(const std::string &s, if (isContinuous_) std::cout << "Continuous "; if (isDiscrete_) std::cout << "Discrete "; if (isHybrid_) std::cout << "Hybrid "; - this->printKeys("", formatter); + for (size_t c=0; c +#include +#include +#include +#include +#include + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ************************************************************************* */ +// Check iterators of empty mixture. +TEST(GaussianMixtureFactor, Constructor) { + GaussianMixtureFactor factor; + GaussianMixtureFactor::const_iterator const_it = factor.begin(); + CHECK(const_it == factor.end()); + GaussianMixtureFactor::iterator it = factor.begin(); + CHECK(it == factor.end()); +} + +/* ************************************************************************* */ +// "Add" two mixture factors together. +TEST(GaussianMixtureFactor, Sum) { + DiscreteKey m1(1, 2), m2(2, 3); + + auto A1 = Matrix::Zero(2, 1); + auto A2 = Matrix::Zero(2, 2); + auto A3 = Matrix::Zero(2, 3); + auto b = Matrix::Zero(2, 1); + Vector2 sigmas; + sigmas << 1, 2; + auto model = noiseModel::Diagonal::Sigmas(sigmas, true); + + auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f20 = boost::make_shared(X(1), A1, X(3), A3, b); + auto f21 = boost::make_shared(X(1), A1, X(3), A3, b); + auto f22 = boost::make_shared(X(1), A1, X(3), A3, b); + std::vector factorsA{f10, f11}; + std::vector factorsB{f20, f21, f22}; + + // TODO(Frank): why specify keys at all? And: keys in factor should be *all* + // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? + // Design review! + GaussianMixtureFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA); + GaussianMixtureFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB); + + // Check that number of keys is 3 + EXPECT_LONGS_EQUAL(3, mixtureFactorA.keys().size()); + + // Check that number of discrete keys is 1 // TODO(Frank): should not exist? + EXPECT_LONGS_EQUAL(1, mixtureFactorA.discreteKeys().size()); + + // Create sum of two mixture factors: it will be a decision tree now on both + // discrete variables m1 and m2: + GaussianMixtureFactor::Sum sum; + sum += mixtureFactorA; + sum += mixtureFactorB; + + // Let's check that this worked: + Assignment mode; + mode[m1.first] = 1; + mode[m2.first] = 2; + auto actual = sum(mode); + EXPECT(actual.at(0) == f11); + EXPECT(actual.at(1) == f22); +} + +TEST(GaussianMixtureFactor, Printing) { + DiscreteKey m1(1, 2); + auto A1 = Matrix::Zero(2, 1); + auto A2 = Matrix::Zero(2, 2); + auto b = Matrix::Zero(2, 1); + auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); + std::vector factors{f10, f11}; + + GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + + std::string expected = + R"(Hybrid x1 x2; 1 ]{ + Choice(1) + 0 Leaf : + A[x1] = [ + 0; + 0 +] + A[x2] = [ + 0, 0; + 0, 0 +] + b = [ 0 0 ] + No noise model + + 1 Leaf : + A[x1] = [ + 0; + 0 +] + A[x2] = [ + 0, 0; + 0, 0 +] + b = [ 0 0 ] + No noise model + +} +)"; + EXPECT(assert_print_equal(expected, mixtureFactor)); +} + +TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) { + KeyVector keys; + keys.push_back(X(0)); + keys.push_back(X(1)); + + DiscreteKeys dKeys; + dKeys.emplace_back(M(0), 2); + dKeys.emplace_back(M(1), 2); + + auto gaussians = boost::make_shared(); + GaussianMixture::Conditionals conditionals(gaussians); + GaussianMixture gm({}, keys, dKeys, conditionals); + + EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ \ No newline at end of file From 60e3321da35ec4d8536da91e528db7adbf1b4812 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 14:11:49 -0400 Subject: [PATCH 083/121] some formatting and improved printing --- gtsam/hybrid/GaussianMixture.cpp | 14 ++++++-------- gtsam/hybrid/HybridDiscreteFactor.cpp | 2 +- gtsam/hybrid/HybridGaussianFactor.cpp | 2 +- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 3d4eb79fa..b04800d21 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -36,8 +36,7 @@ GaussianMixture::GaussianMixture( conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixture::Conditionals & -GaussianMixture::conditionals() { +const GaussianMixture::Conditionals &GaussianMixture::conditionals() { return conditionals_; } @@ -48,8 +47,8 @@ GaussianMixture GaussianMixture::FromConditionals( const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixture(continuousFrontals, continuousParents, - discreteParents, dt); + return GaussianMixture(continuousFrontals, continuousParents, discreteParents, + dt); } /* *******************************************************************************/ @@ -66,8 +65,7 @@ GaussianMixture::Sum GaussianMixture::add( } /* *******************************************************************************/ -GaussianMixture::Sum -GaussianMixture::asGaussianFactorGraphTree() const { +GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -106,13 +104,13 @@ 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 << "Continuous "; if (isDiscrete()) std::cout << "Discrete "; if (isHybrid()) std::cout << "Hybrid "; BaseConditional::print("", formatter); - std::cout << "\nDiscrete Keys = "; + std::cout << " Discrete Keys = "; for (auto &dk : discreteKeys()) { std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 2bdcdee8c..0455e1e90 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -47,7 +47,7 @@ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner_->print("inner: ", formatter); + inner_->print("\n", formatter); }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 59d20fb79..4d7490e49 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -41,7 +41,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner_->print("inner: ", formatter); + inner_->print("\n", formatter); }; } // namespace gtsam From 8b03eb5b2d3abb3b9f3784e570b114e776f553df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 17:57:26 -0400 Subject: [PATCH 084/121] move += operator inside namespace --- gtsam/discrete/DecisionTree-inl.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 99f29b8e5..ffb8d7c69 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -39,10 +39,10 @@ #include #include -using boost::assign::operator+=; - namespace gtsam { + using boost::assign::operator+=; + /****************************************************************************/ // Node /****************************************************************************/ From f911ccf176570cd93be5e1dea290283ec37ff3ff Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Wed, 8 Jun 2022 16:30:10 +0800 Subject: [PATCH 085/121] Move Rot3::quaternion to the deprecated block --- gtsam/geometry/Rot3.cpp | 2 ++ gtsam/geometry/Rot3.h | 7 ++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 08213b065..6db5e1919 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 Vector Rot3::quaternion() const { gtsam::Quaternion q = toQuaternion(); Vector v(4); @@ -237,6 +238,7 @@ Vector Rot3::quaternion() const { v(3) = q.z(); return v; } +#endif /* ************************************************************************* */ pair Rot3::axisAngle() const { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 18bd88b52..01ca7778c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup { */ gtsam::Quaternion toQuaternion() const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * Converts to a generic matrix to allow for use with matlab * In format: w x y z + * @deprecated: use Rot3::toQuaternion() instead. + * If still using this API, remind that in the returned Vector `V`, + * `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'. */ - Vector quaternion() const; + Vector GTSAM_DEPRECATED quaternion() const; +#endif /** * @brief Spherical Linear intERPolation between *this and other From 27ddedfc630206a87c980c6b64f1b90dc7f26747 Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Wed, 8 Jun 2022 16:42:50 +0800 Subject: [PATCH 086/121] Replace the usage of Rot3::quaternion to Rot3::toQuaternion in 'timeShonanAveraging.cpp' --- gtsam_unstable/timing/timeShonanAveraging.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp index e932b6400..74dd04a78 100644 --- a/gtsam_unstable/timing/timeShonanAveraging.cpp +++ b/gtsam_unstable/timing/timeShonanAveraging.cpp @@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map poses myfile << "VERTEX_SE3:QUAT" << " "; myfile << i << " "; myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " "; - Vector quaternion = Rot3(R).quaternion(); - myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) - << " " << quaternion(0); + Quaternion quaternion = Rot3(R).toQuaternion(); + myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() + << " " << quaternion.w(); myfile << "\n"; } myfile.close(); From 5ac71d20d56c594e6ef118d01e47706a90e78c6e Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Thu, 9 Jun 2022 17:56:35 +0800 Subject: [PATCH 087/121] Remove 'Rot3::quaternion()' API from 'geometry.i' --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8079e2c2a..517558265 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -355,7 +355,7 @@ class Rot3 { double yaw() const; pair axisAngle() const; gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; + // Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219 gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; // enabling serialization functionality From f6b86fb17eab1788edc25630ab6f78508d0aaec6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Jun 2022 04:05:18 -0400 Subject: [PATCH 088/121] address review comments --- gtsam/hybrid/tests/testGaussianMixture.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 8a91a86cc..420e22315 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -35,7 +35,10 @@ using symbol_shorthand::M; using symbol_shorthand::X; /* ************************************************************************* */ -TEST(GaussianConditional, Equals) { +/* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a + * specific mode i.e. P(x1 | x2, m1=1). + */ +TEST(GaussianMixture, Equals) { // create a conditional gaussian node Matrix S1(2, 2); S1(0, 0) = 1; @@ -89,4 +92,4 @@ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ \ No newline at end of file +/* ************************************************************************* */ From c49ad326d1f34327e9d751b5e2fdbacd4f71ba38 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 17 Jun 2022 10:57:51 -0700 Subject: [PATCH 089/121] initial LOST implementation tested --- gtsam/geometry/tests/testTriangulation.cpp | 18 +++++++++ gtsam/geometry/triangulation.cpp | 46 ++++++++++++++++++++++ gtsam/geometry/triangulation.h | 45 +++++++++++++++++++++ 3 files changed, 109 insertions(+) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index fb66fb6a2..838a5e07f 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -96,6 +96,24 @@ TEST(triangulation, twoPoses) { EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } +TEST(triangulation, twoCamerasLOST) { + std::vector> cameras = {camera1, camera2}; + std::vector measurements = {z1, z2}; + + // 1. Test simple triangulation, perfect in no noise situation + Point3 actual1 = // + triangulateLOSTPoint3(cameras, measurements); + EXPECT(assert_equal(landmark, actual1, 1e-12)); + + // 3. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) + measurements[0] += Point2(0.1, 0.5); + measurements[1] += Point2(-0.2, 0.3); + Point3 actual2 = // + triangulateLOSTPoint3(cameras, measurements); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4)); +} + //****************************************************************************** // Simple test with a well-behaved two camera situation with Cal3DS2 calibration. TEST(triangulation, twoPosesCal3DS2) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 026afef24..a02da2eff 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -39,6 +39,11 @@ Vector4 triangulateHomogeneousDLT( const Point2& p = measurements.at(i); // build system of equations + // [A_1; A_2; A_3] x = [b_1; b_2; b_3] + // [b_3 * A_1 - b_1 * A_3] x = 0 + // [b_3 * A_2 - b_2 * A_3] x = 0 + // A' x = 0 + // A' 2x4 = [b_3 * A_1 - b_1 * A_3; b_3 * A_2 - b_2 * A_3] A.row(row) = p.x() * projection.row(2) - projection.row(0); A.row(row + 1) = p.y() * projection.row(2) - projection.row(1); } @@ -53,6 +58,47 @@ Vector4 triangulateHomogeneousDLT( return v; } +Vector3 triangulateLOSTHomogeneous( + const std::vector& poses, + const std::vector& calibrated_measurements) { + + // TODO(akshay-krishnan): make this an argument. + const double sigma_x = 1e-3; + + size_t m = calibrated_measurements.size(); + assert(m == poses.size()); + + // Construct the system matrices. + Matrix A = Matrix::Zero(m * 2, 3); + Matrix b = Matrix::Zero(m * 2, 1); + + for (size_t i = 0; i < m; i++) { + const Pose3& wTi = poses[i]; + // TODO(akshay-krishnan): are there better ways to select j? + const int j = (i + 1) % m; + const Pose3& wTj = poses[j]; + + Point3 d_ij = wTj.translation() - wTi.translation(); + + Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); + Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); + + double numerator = w_measurement_i.cross(w_measurement_j).norm(); + double denominator = d_ij.cross(w_measurement_j).norm(); + + double q_i = numerator / (sigma_x * denominator); + Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); + + A.row(2 * i) = coefficient_mat.row(0); + A.row(2 * i + 1) = coefficient_mat.row(1); + Point2 p = coefficient_mat * wTi.translation(); + b(2 * i) = p.x(); + b(2 * i + 1) = p.y(); + } + // Solve Ax = b, using QR decomposition + return A.colPivHouseholderQr().solve(b); +} + Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, const std::vector& measurements, double rank_tol) { diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 401fd2d0b..3b834c615 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -62,6 +62,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol = 1e-9); +/** + * @brief + * + * @param projection_matrices + * @param measurements + * @param rank_tol + * @return GTSAM_EXPORT + */ +GTSAM_EXPORT Vector3 +triangulateLOSTHomogeneous(const std::vector& poses, + const std::vector& calibrated_measurements); + /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -382,6 +394,39 @@ Point3 triangulatePoint3(const std::vector& poses, return point; } +template +Point3 triangulateLOSTPoint3(const std::vector>& cameras, + const std::vector& measurements) { + const size_t num_cameras = cameras.size(); + assert(measurements.size() == num_cameras); + + if (num_cameras < 2) throw(TriangulationUnderconstrainedException()); + + // Convert measurements to image plane coordinates. + std::vector calibrated_measurements; + calibrated_measurements.reserve(measurements.size()); + for (int i = 0; i < measurements.size(); ++i) { + Point2 p = cameras[i].calibration().calibrate(measurements[i]); + calibrated_measurements.emplace_back(p.x(), p.y(), 1.0); + } + + std::vector poses; + poses.reserve(cameras.size()); + for (const auto& camera : cameras) poses.push_back(camera.pose()); + + Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + for (const auto& camera : cameras) { + const Point3& p_local = camera.pose().transformTo(point); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); + } +#endif + + return point; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. This function is similar to the one From 5ec6127c0b5bebe03149068f276e53870a0aa8bb Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:35:46 -0700 Subject: [PATCH 090/121] add unit test, update doc --- gtsam/sfm/TranslationRecovery.cpp | 1 + tests/testTranslationRecovery.cpp | 30 ++++++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 8f1108806..28c2b2e8a 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -156,6 +156,7 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + // There may be nodes in betweenTranslations that do not have a measurement. for (auto edge : betweenTranslations) { insert(edge.key1()); insert(edge.key2()); diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 5dd319d30..15f1caa1b 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } + +TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) { + // Checks that valid results are obtained when a between translation edge is + // provided with a node that does not have any other relative translations. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + poses.insert(4, Pose3(Rot3(), Point3(1, 2, 1))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + // Node 4 only has this between translation prior, no relative translations. + betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1)); + + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); + EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 6fae6f42313552ca900b9d8af26b1f6698d889e8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:55:55 -0700 Subject: [PATCH 091/121] update docstring --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 28c2b2e8a..c7ef2e526 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -119,16 +119,18 @@ void TranslationRecovery::addPrior( graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), priorNoiseModel); - // Add between factors for optional relative translations. - for (auto edge : betweenTranslations) { - graph->emplace_shared>( - edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); - } - // Add a scale prior only if no other between factors were added. if (betweenTranslations.empty()) { graph->emplace_shared>( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); + return; + } + + // Add between factors for optional relative translations. + for (auto prior_edge : betweenTranslations) { + graph->emplace_shared>( + prior_edge.key1(), prior_edge.key2(), prior_edge.measured(), + prior_edge.noiseModel()); } } From e18840baffd3e02fb3c05e76ee2586cf2ae83e77 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:56:21 -0700 Subject: [PATCH 092/121] update doc in header --- gtsam/sfm/TranslationRecovery.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 6ccc39ddb..44a5ef43e 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -143,11 +143,15 @@ class TranslationRecovery { * * @param relativeTranslations the relative translations, in world coordinate * frames, vector of BinaryMeasurements of Unit3, where each key of a - * measurement is a point in 3D. + * measurement is a point in 3D. If a relative translation magnitude is zero, + * it is treated as a hard same-point constraint (the result of all nodes + * connected by a zero-magnitude edge will be the same). * @param scale scale for first relative translation which fixes gauge. * The scale is only used if betweenTranslations is empty. * @param betweenTranslations relative translations (with scale) between 2 - * points in world coordinate frame known a priori. + * points in world coordinate frame known a priori. Unlike + * relativeTranslations, zero-magnitude betweenTranslations are not treated as + * hard constraints. * @param initialValues intial values for optimization. Initializes randomly * if not provided. * @return Values From 0984fea3f506fa42675e92edbeda4ac9d4df3f5c Mon Sep 17 00:00:00 2001 From: vik748 Date: Mon, 20 Jun 2022 01:18:07 -0700 Subject: [PATCH 093/121] Address matplotlib deprecation warnings for fig.gca() and window.set_title(). --- python/gtsam/examples/Pose3ISAM2Example.py | 5 ++++- python/gtsam/examples/VisualISAM2Example.py | 5 ++++- python/gtsam/utils/plot.py | 19 ++++++++++++++----- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index cb71813c5..986bd5b9c 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -34,7 +34,10 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa # Plot the newly updated iSAM2 inference. fig = plt.figure(0) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() i = 1 diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 4b480fab7..60d4fb376 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -33,7 +33,10 @@ def visual_ISAM2_plot(result): fignum = 0 fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() # Plot points diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a4d19f72b..880c436e8 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -333,7 +333,10 @@ def plot_point3( """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plot_point3_on_axes(axes, point, linespec, P) axes.set_xlabel(axis_labels[0]) @@ -388,7 +391,7 @@ def plot_3d_points(fignum, fig = plt.figure(fignum) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): @@ -490,7 +493,10 @@ def plot_trajectory( axis_labels (iterable[string]): List of axis labels to set. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] axes.set_xlabel(axis_labels[0]) axes.set_ylabel(axis_labels[1]) @@ -522,7 +528,7 @@ def plot_trajectory( plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_incremental_trajectory(fignum: int, @@ -545,7 +551,10 @@ def plot_incremental_trajectory(fignum: int, Used to create animation effect. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] poses = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(poses.keys()) From 5ea8f2529fc91b82a414a770c027eac14fc3a205 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 20 Jun 2022 23:20:29 -0700 Subject: [PATCH 094/121] make noise input, add LOST vs DLT unit test --- gtsam/geometry/tests/testTriangulation.cpp | 39 +++++++++++++++++++++- gtsam/geometry/triangulation.cpp | 19 +++++------ gtsam/geometry/triangulation.h | 11 +++--- 3 files changed, 54 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 838a5e07f..0b885d434 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -109,11 +109,48 @@ TEST(triangulation, twoCamerasLOST) { // 0.499167, 1.19814) measurements[0] += Point2(0.1, 0.5); measurements[1] += Point2(-0.2, 0.3); + const double measurement_sigma = 1e-3; Point3 actual2 = // - triangulateLOSTPoint3(cameras, measurements); + triangulateLOSTPoint3(cameras, measurements, measurement_sigma); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4)); } +TEST(triangulation, twoCamerasLOSTvsDLT) { + // LOST has been shown to preform better when the point is much closer to one + // camera compared to another. This unit test checks this configuration. + Cal3_S2 identity_K; + Pose3 pose_1; + Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + PinholeCamera camera_1(pose_1, identity_K); + PinholeCamera camera_2(pose_2, identity_K); + + Point3 gt_point(0, 0, 1); + Point2 x1 = camera_1.project(gt_point); + Point2 x2 = camera_2.project(gt_point); + + Point2 x1_noisy = x1 + Point2(0.00817, 0.00977); + Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969); + + const double measurement_sigma = 1e-2; + Point3 estimate_lost = triangulateLOSTPoint3( + {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma); + + // These values are from a MATLAB implementation. + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), estimate_lost, 1e-3)); + + double rank_tol = 1e-9; + + Pose3Vector poses = {pose_1, pose_2}; + Point2Vector points = {x1_noisy, x2_noisy}; + boost::shared_ptr cal = boost::make_shared(identity_K); + boost::optional estimate_dlt = + triangulatePoint3(poses, cal, points, rank_tol, false); + + // The LOST estimate should have a smaller error. + EXPECT((gt_point - estimate_lost).norm() <= + (gt_point - *estimate_dlt).norm()); +} + //****************************************************************************** // Simple test with a well-behaved two camera situation with Cal3DS2 calibration. TEST(triangulation, twoPosesCal3DS2) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index a02da2eff..bec239240 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -60,11 +60,8 @@ Vector4 triangulateHomogeneousDLT( Vector3 triangulateLOSTHomogeneous( const std::vector& poses, - const std::vector& calibrated_measurements) { - - // TODO(akshay-krishnan): make this an argument. - const double sigma_x = 1e-3; - + const std::vector& calibrated_measurements, + const double measurement_sigma) { size_t m = calibrated_measurements.size(); assert(m == poses.size()); @@ -78,16 +75,18 @@ Vector3 triangulateLOSTHomogeneous( const int j = (i + 1) % m; const Pose3& wTj = poses[j]; - Point3 d_ij = wTj.translation() - wTi.translation(); + Point3 d_ij = wTj.translation() - wTi.translation(); Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); - - double numerator = w_measurement_i.cross(w_measurement_j).norm(); + + double numerator = w_measurement_i.cross(w_measurement_j).norm(); double denominator = d_ij.cross(w_measurement_j).norm(); - double q_i = numerator / (sigma_x * denominator); - Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); + double q_i = numerator / (measurement_sigma * denominator); + Matrix23 coefficient_mat = + q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); A.row(2 * i) = coefficient_mat.row(0); A.row(2 * i + 1) = coefficient_mat.row(1); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 3b834c615..d31630e67 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -72,7 +72,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( */ GTSAM_EXPORT Vector3 triangulateLOSTHomogeneous(const std::vector& poses, - const std::vector& calibrated_measurements); + const std::vector& calibrated_measurements, + const double measurement_sigma); /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors @@ -395,8 +396,10 @@ Point3 triangulatePoint3(const std::vector& poses, } template -Point3 triangulateLOSTPoint3(const std::vector>& cameras, - const std::vector& measurements) { +Point3 triangulateLOSTPoint3( + const std::vector>& cameras, + const std::vector& measurements, + const double measurement_sigma = 1e-2) { const size_t num_cameras = cameras.size(); assert(measurements.size() == num_cameras); @@ -414,7 +417,7 @@ Point3 triangulateLOSTPoint3(const std::vector>& came poses.reserve(cameras.size()); for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements); + Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras From a1e45e0df502b8d02f3f9b8fdf6d456dfb6fc956 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 25 Jun 2022 23:34:16 -0400 Subject: [PATCH 095/121] add this-> to fix ROS issue --- gtsam/sam/BearingRangeFactor.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index da352f2e9..9874760c4 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -79,9 +79,8 @@ class BearingRangeFactor { std::vector Hs(2); const auto &keys = Factor::keys(); - const Vector error = unwhitenedError( - {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, - Hs); + const Vector error = this->unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs); if (H1) *H1 = Hs[0]; if (H2) *H2 = Hs[1]; return error; From ca4b450e7ee98a2d6344df2120148c958e6ab410 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 28 Jun 2022 00:59:20 -0700 Subject: [PATCH 096/121] experimenting with LS and TLS --- gtsam/geometry/triangulation.cpp | 66 +++++++++++++++++++++++++------- gtsam/geometry/triangulation.h | 17 ++++++-- 2 files changed, 65 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index bec239240..9d8325d68 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -65,6 +65,50 @@ Vector3 triangulateLOSTHomogeneous( size_t m = calibrated_measurements.size(); assert(m == poses.size()); + // Construct the system matrices. + Matrix A = Matrix::Zero(m * 2, 4); + + for (size_t i = 0; i < m; i++) { + const Pose3& wTi = poses[i]; + // TODO(akshay-krishnan): are there better ways to select j? + const int j = (i + 1) % m; + const Pose3& wTj = poses[j]; + + const Point3 d_ij = wTj.translation() - wTi.translation(); + + const Point3 w_measurement_i = + wTi.rotation().rotate(calibrated_measurements[i]); + const Point3 w_measurement_j = + wTj.rotation().rotate(calibrated_measurements[j]); + + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / + (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + const Matrix23 coefficient_mat = + q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); + + A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation(); + } + + const double rank_tol = 1e-6; + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A, rank_tol); + + if (rank < 3) + throw(TriangulationUnderconstrainedException()); + + return Point3(v.head<3>() / v[3]); +} + +Vector3 triangulateLOSTHomogeneousLS( + const std::vector& poses, + const std::vector& calibrated_measurements, + const double measurement_sigma) { + size_t m = calibrated_measurements.size(); + assert(m == poses.size()); + // Construct the system matrices. Matrix A = Matrix::Zero(m * 2, 3); Matrix b = Matrix::Zero(m * 2, 1); @@ -75,26 +119,20 @@ Vector3 triangulateLOSTHomogeneous( const int j = (i + 1) % m; const Pose3& wTj = poses[j]; - Point3 d_ij = wTj.translation() - wTi.translation(); + const Point3 d_ij = wTj.translation() - wTi.translation(); - Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); - Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); + const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); + const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); - double numerator = w_measurement_i.cross(w_measurement_j).norm(); - double denominator = d_ij.cross(w_measurement_j).norm(); - - double q_i = numerator / (measurement_sigma * denominator); - Matrix23 coefficient_mat = + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / + (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + const Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.row(2 * i) = coefficient_mat.row(0); - A.row(2 * i + 1) = coefficient_mat.row(1); - Point2 p = coefficient_mat * wTi.translation(); - b(2 * i) = p.x(); - b(2 * i + 1) = p.y(); + A.block<2, 3>(2*i, 0) = coefficient_mat; + b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation(); } - // Solve Ax = b, using QR decomposition return A.colPivHouseholderQr().solve(b); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d31630e67..5bea4d93c 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -75,6 +75,11 @@ triangulateLOSTHomogeneous(const std::vector& poses, const std::vector& calibrated_measurements, const double measurement_sigma); +GTSAM_EXPORT Vector3 +triangulateLOSTHomogeneousLS(const std::vector& poses, + const std::vector& calibrated_measurements, + const double measurement_sigma); + /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -397,9 +402,9 @@ Point3 triangulatePoint3(const std::vector& poses, template Point3 triangulateLOSTPoint3( - const std::vector>& cameras, - const std::vector& measurements, - const double measurement_sigma = 1e-2) { + const CameraSet>& cameras, + const Point2Vector& measurements, + const double measurement_sigma = 1e-2, bool use_dlt = false) { const size_t num_cameras = cameras.size(); assert(measurements.size() == num_cameras); @@ -417,7 +422,11 @@ Point3 triangulateLOSTPoint3( poses.reserve(cameras.size()); for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma); + Point3 point = use_dlt + ? triangulateLOSTHomogeneous( + poses, calibrated_measurements, measurement_sigma) + : triangulateLOSTHomogeneousLS( + poses, calibrated_measurements, measurement_sigma); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras From 0d501b6de153bfc13044816480bc5752044e3bf6 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 30 Jun 2022 23:38:34 -0400 Subject: [PATCH 097/121] fix testNoiseModel --- gtsam/linear/tests/testNoiseModel.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 92774a133..f83ba7831 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -661,14 +661,15 @@ TEST(NoiseModel, robustNoiseDCS) TEST(NoiseModel, robustNoiseL2WithDeadZone) { double dead_zone_size = 1.0; - SharedNoiseModel robust = noiseModel::Robust::Create( + auto robust = noiseModel::Robust::Create( noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), Unit::Create(3)); for (int i = 0; i < 5; i++) { - Vector3 error = Vector3(i, 0, 0); + Vector error = Vector3(i, 0, 0); + robust->WhitenSystem(error); DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, - robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8); + robust->squaredMahalanobisDistance(error), 1e-8); } } From f947b973cdc5d59214f95b80531f17c4f538b771 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Jul 2022 16:02:42 -0400 Subject: [PATCH 098/121] addressed comments in PR --- gtsam/geometry/triangulation.h | 4 ++-- gtsam/nonlinear/GncOptimizer.h | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index af01d3a36..a4c6b1a13 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -107,7 +107,7 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = unit2) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; @@ -117,7 +117,7 @@ std::pair triangulationGraph( typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); graph.emplace_shared > // - (camera_i, measurements[i], model? model : unit2, landmarkKey); + (camera_i, measurements[i], model, landmarkKey); } return std::make_pair(graph, values); } diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 96b03e803..e5b4718d6 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -299,9 +299,11 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } - if (mu_init >= 0 && mu_init < 1e-6) + if (mu_init >= 0 && mu_init < 1e-6){ mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, - // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + } + return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) From 79305c1a9d51bdb62f43993a20ad32b5b6fc8ad2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Jul 2022 17:05:14 -0400 Subject: [PATCH 099/121] fixed compile issue --- gtsam/geometry/triangulation.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index a4c6b1a13..486850903 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -107,11 +107,10 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = unit2) { + const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; typedef PinholePose Camera; From aab6af3368ead2be9ef0aa885de5a4777e0c79a4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Jul 2022 11:09:46 -0400 Subject: [PATCH 100/121] replace boost::function with std::function --- gtsam/basis/Chebyshev2.h | 4 +--- gtsam/basis/tests/testChebyshev2.cpp | 2 +- gtsam/discrete/DecisionTree.h | 2 +- gtsam/geometry/tests/testPoint3.cpp | 2 -- 4 files changed, 3 insertions(+), 7 deletions(-) diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index e306c93d5..4c3542d56 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,8 +36,6 @@ #include #include -#include - namespace gtsam { /** @@ -134,7 +132,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * Create matrix of values at Chebyshev points given vector-valued function. */ template - static Matrix matrix(boost::function(double)> f, + static Matrix matrix(std::function(double)> f, size_t N, double a = -1, double b = 1) { Matrix Xmat(M, N); for (size_t j = 0; j < N; j++) { diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 9090757f4..73339e0f1 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -118,7 +118,7 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - boost::function)> f = boost::bind( + std::function)> f = boost::bind( &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); Matrix numericalH = numericalDerivative11, 2 * N>(f, X); diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 7449ae995..dede2a2f0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 315391ac8..e373e1d52 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -19,8 +19,6 @@ #include #include -#include - using namespace std::placeholders; using namespace gtsam; From b0369c48c8cab250589e28c12062c869699ca762 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Jul 2022 11:10:27 -0400 Subject: [PATCH 101/121] import VectorSerialization to Point3 to allow serialization for downstream classes --- gtsam/geometry/Point3.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 835c53d7c..f47531963 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include From 3595e8588c84a6e8488cd948664c765bce92bf40 Mon Sep 17 00:00:00 2001 From: yotams Date: Mon, 4 Jul 2022 11:27:49 +0300 Subject: [PATCH 102/121] added jacobians for all pose3 methods in python wrappers --- gtsam/geometry/Pose3.cpp | 5 +++++ gtsam/geometry/Pose3.h | 8 ++++++++ gtsam/geometry/geometry.i | 33 +++++++++++++++++++++++++++++++++ 3 files changed, 46 insertions(+) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2da51a625..f261a2302 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -489,6 +489,11 @@ boost::optional align(const Point3Pairs &baPointPairs) { } #endif +/* ************************************************************************* */ +Pose3 Pose3::interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { + return interpolate(*this, other, t, Hx, Hy); +} + /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { // Both Rot3 and Point3 have ostream definitions so we use them. diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index c36047349..e2128b28d 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -379,6 +379,14 @@ public: return std::make_pair(0, 2); } + /** + * @brief Spherical Linear interpolation between *this and other + * @param s a value between 0 and 1.5 + * @param other final point of interpolation geodesic on manifold + */ + Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, + OptionalJacobian<6, 6> Hy = boost::none) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 517558265..4deb5c48a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -446,24 +446,43 @@ class Pose3 { // Group static gtsam::Pose3 identity(); gtsam::Pose3 inverse() const; + gtsam::Pose3 inverse(Eigen::Ref H) const; gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 compose(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; gtsam::Pose3 between(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; + gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const; + gtsam::Pose3 interp(double t, const gtsam::Pose3& pose, + Eigen::Ref Hx, + Eigen::Ref Hy) const; // Operator Overloads gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; // Manifold gtsam::Pose3 retract(Vector v) const; + gtsam::Pose3 retract(Vector v, Eigen::Ref Hxi) const; Vector localCoordinates(const gtsam::Pose3& pose) const; + Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref Hxi) const; // Lie Group static gtsam::Pose3 Expmap(Vector v); + static gtsam::Pose3 Expmap(Vector v, Eigen::Ref Hxi); static Vector Logmap(const gtsam::Pose3& pose); + static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref Hpose); gtsam::Pose3 expmap(Vector v); Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + Vector Adjoint(Vector xi, Eigen::Ref H_this, + Eigen::Ref H_xib) const; Vector AdjointTranspose(Vector xi) const; + Vector AdjointTranspose(Vector xi, Eigen::Ref H_this, + Eigen::Ref H_x) const; static Matrix adjointMap(Vector xi); static Vector adjoint(Vector xi, Vector y); static Matrix adjointMap_(Vector xi); @@ -476,7 +495,11 @@ class Pose3 { // Group Action on Point3 gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformFrom(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; gtsam::Point3 transformTo(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; // Matrix versions Matrix transformFrom(const Matrix& points) const; @@ -484,15 +507,25 @@ class Pose3 { // Standard Interface gtsam::Rot3 rotation() const; + gtsam::Rot3 rotation(Eigen::Ref Hself) const; gtsam::Point3 translation() const; + gtsam::Point3 translation(Eigen::Ref Hself) const; double x() const; double y() const; double z() const; Matrix matrix() const; gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref HaTb) const; gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref HwTb) const; double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint); double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref Hpose); // enabling serialization functionality void serialize() const; From aaeeccf8f59e26c479b39d6e8a12c78008d4e43f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Jul 2022 14:55:39 -0400 Subject: [PATCH 103/121] Squashed 'wrap/' changes from 1a7dc9722..ca357ccdd ca357ccdd Merge pull request #149 from borglab/install-package 886846724 set the GTWRAP_PATH_SEPARATOR properly for MatlabWrap 4abed7fa0 install the python package explicitly git-subtree-dir: wrap git-subtree-split: ca357ccdd27f0661e73ff7a1771768dc4bf8f746 --- cmake/MatlabWrap.cmake | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cmake/MatlabWrap.cmake b/cmake/MatlabWrap.cmake index 3cb058102..eaffcc059 100644 --- a/cmake/MatlabWrap.cmake +++ b/cmake/MatlabWrap.cmake @@ -242,6 +242,13 @@ function(wrap_library_internal interfaceHeader moduleName linkLibraries extraInc find_package(PythonInterp ${WRAP_PYTHON_VERSION} EXACT) find_package(PythonLibs ${WRAP_PYTHON_VERSION} EXACT) + # Set the path separator for PYTHONPATH + if(UNIX) + set(GTWRAP_PATH_SEPARATOR ":") + else() + set(GTWRAP_PATH_SEPARATOR ";") + endif() + add_custom_command( OUTPUT ${generated_cpp_file} DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} From 7348586f60b04c464fe674ee939449980ae0efc2 Mon Sep 17 00:00:00 2001 From: yotams Date: Thu, 7 Jul 2022 14:09:10 +0300 Subject: [PATCH 104/121] 1. changed interp name to slerp 2. added python tests for jacobians for some pose3 apis --- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/geometry.i | 4 +- python/gtsam/tests/test_Pose3.py | 118 ++++++++++++++++++++++++++++++- 4 files changed, 121 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f261a2302..2938e90f5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -490,7 +490,7 @@ boost::optional align(const Point3Pairs &baPointPairs) { #endif /* ************************************************************************* */ -Pose3 Pose3::interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { +Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { return interpolate(*this, other, t, Hx, Hy); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e2128b28d..33fb55250 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -384,7 +384,7 @@ public: * @param s a value between 0 and 1.5 * @param other final point of interpolation geodesic on manifold */ - Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, + Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const; /// Output stream operator diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 4deb5c48a..92d9ec0a3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -455,8 +455,8 @@ class Pose3 { gtsam::Pose3 between(const gtsam::Pose3& pose, Eigen::Ref H1, Eigen::Ref H2) const; - gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const; - gtsam::Pose3 interp(double t, const gtsam::Pose3& pose, + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose) const; + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose, Eigen::Ref Hx, Eigen::Ref Hy) const; diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index cde71de53..cdc214a2c 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -19,6 +19,62 @@ from gtsam import Point3, Pose3, Rot3, Point3Pairs from gtsam.utils.test_case import GtsamTestCase +def numerical_derivative_pose(pose, method, delta=1e-5): + jacobian = np.zeros((6, 6)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + pose_plus = pose.retract(xplus).__getattribute__(method)() + pose_minus = pose.retract(xminus).__getattribute__(method)() + jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta) + return jacobian + + +def numerical_derivative_2_poses(pose, other_pose, method, delta=1e-5, inputs=()): + jacobian = np.zeros((6, 6)) + other_jacobian = np.zeros((6, 6)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + + pose_plus = pose.retract(xplus).__getattribute__(method)(*inputs, other_pose) + pose_minus = pose.retract(xminus).__getattribute__(method)(*inputs, other_pose) + jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta) + + other_pose_plus = pose.__getattribute__(method)(*inputs, other_pose.retract(xplus)) + other_pose_minus = pose.__getattribute__(method)(*inputs, other_pose.retract(xminus)) + other_jacobian[:, idx] = other_pose_minus.localCoordinates(other_pose_plus) / (2 * delta) + return jacobian, other_jacobian + + +def numerical_derivative_pose_point(pose, point, method, delta=1e-5): + jacobian = np.zeros((3, 6)) + point_jacobian = np.zeros((3, 3)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + + point_plus = pose.retract(xplus).__getattribute__(method)(point) + point_minus = pose.retract(xminus).__getattribute__(method)(point) + jacobian[:, idx] = (point_plus - point_minus) / (2 * delta) + + if idx < 3: + xplus = np.zeros(3) + xplus[idx] = delta + xminus = np.zeros(3) + xminus[idx] = -delta + point_plus = pose.__getattribute__(method)(point + xplus) + point_minus = pose.__getattribute__(method)(point + xminus) + point_jacobian[:, idx] = (point_plus - point_minus) / (2 * delta) + return jacobian, point_jacobian + + class TestPose3(GtsamTestCase): """Test selected Pose3 methods.""" @@ -30,6 +86,47 @@ class TestPose3(GtsamTestCase): actual = T2.between(T3) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + jacobian = np.zeros((6, 6), order='F') + jacobian_other = np.zeros((6, 6), order='F') + T2.between(T3, jacobian, jacobian_other) + jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(T2, T3, 'between') + self.gtsamAssertEquals(jacobian, jacobian_numerical) + self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other) + + def test_inverse(self): + """Test between method.""" + pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + expected = Pose3(Rot3.Rodrigues(0, 0, math.pi/2), Point3(4, -2, 0)) + actual = pose.inverse() + self.gtsamAssertEquals(actual, expected, 1e-6) + + #test jacobians + jacobian = np.zeros((6, 6), order='F') + pose.inverse(jacobian) + jacobian_numerical = numerical_derivative_pose(pose, 'inverse') + self.gtsamAssertEquals(jacobian, jacobian_numerical) + + def test_slerp(self): + """Test slerp method.""" + pose0 = gtsam.Pose3() + pose1 = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + actual_alpha_0 = pose0.slerp(0, pose1) + self.gtsamAssertEquals(actual_alpha_0, pose0) + actual_alpha_1 = pose0.slerp(1, pose1) + self.gtsamAssertEquals(actual_alpha_1, pose1) + actual_alpha_half = pose0.slerp(0.5, pose1) + expected_alpha_half = Pose3(Rot3.Rodrigues(0, 0, -math.pi/4), Point3(0.17157288, 2.41421356, 0)) + self.gtsamAssertEquals(actual_alpha_half, expected_alpha_half, tol=1e-6) + + # test jacobians + jacobian = np.zeros((6, 6), order='F') + jacobian_other = np.zeros((6, 6), order='F') + pose0.slerp(0.5, pose1, jacobian, jacobian_other) + jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(pose0, pose1, 'slerp', inputs=[0.5]) + self.gtsamAssertEquals(jacobian, jacobian_numerical) + self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other) + def test_transformTo(self): """Test transformTo method.""" pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) @@ -37,6 +134,15 @@ class TestPose3(GtsamTestCase): expected = Point3(2, 1, 10) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + point = Point3(3, 2, 10) + jacobian_pose = np.zeros((3, 6), order='F') + jacobian_point = np.zeros((3, 3), order='F') + pose.transformTo(point, jacobian_pose, jacobian_point) + jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformTo') + self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose) + self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point) + # multi-point version points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T actual_array = pose.transformTo(points) @@ -51,6 +157,15 @@ class TestPose3(GtsamTestCase): expected = Point3(3, 2, 10) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + point = Point3(3, 2, 10) + jacobian_pose = np.zeros((3, 6), order='F') + jacobian_point = np.zeros((3, 3), order='F') + pose.transformFrom(point, jacobian_pose, jacobian_point) + jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformFrom') + self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose) + self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point) + # multi-point version points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T actual_array = pose.transformFrom(points) @@ -122,4 +237,5 @@ class TestPose3(GtsamTestCase): if __name__ == "__main__": - unittest.main() + # unittest.main() + TestPose3().test_slerp() \ No newline at end of file From 7c077f8536c1a06710c251d16b1760ef55be11aa Mon Sep 17 00:00:00 2001 From: yotams Date: Thu, 7 Jul 2022 14:20:26 +0300 Subject: [PATCH 105/121] bugfix --- python/gtsam/tests/test_Pose3.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index cdc214a2c..4464e8d47 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -237,5 +237,4 @@ class TestPose3(GtsamTestCase): if __name__ == "__main__": - # unittest.main() - TestPose3().test_slerp() \ No newline at end of file + unittest.main() \ No newline at end of file From f5ec070f9fc87d52076846ff394b79b001f4bd13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 7 Jul 2022 15:09:55 -0400 Subject: [PATCH 106/121] wrap BearingRange classes and factors --- gtsam/geometry/geometry.i | 6 ++++++ gtsam/sam/sam.i | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 92d9ec0a3..f3f8a5c79 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1279,5 +1279,11 @@ class BearingRange { typedef gtsam::BearingRange BearingRange2D; +typedef gtsam::BearingRange + BearingRangePose2; +typedef gtsam::BearingRange + BearingRange3D; +typedef gtsam::BearingRange + BearingRangePose3; } // namespace gtsam diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index 90c319ede..a46a6de9e 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -108,5 +108,11 @@ typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; +typedef gtsam::BearingRangeFactor + BearingRangeFactor3D; +typedef gtsam::BearingRangeFactor + BearingRangeFactorPose3; } // namespace gtsam From 2ab09a580fa059b7dd5cd0750365927f81f5a5e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 7 Jul 2022 15:24:56 -0400 Subject: [PATCH 107/121] add Python unit tests --- gtsam/linear/linear.i | 2 +- python/gtsam/tests/test_Factors.py | 22 ++++++++++++++++++++-- python/gtsam/tests/test_sam.py | 28 ++++++++++++++++++++++++++++ 3 files changed, 49 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 943b661d8..fdf156ff9 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -279,7 +279,6 @@ virtual class GaussianFactor { virtual class JacobianFactor : gtsam::GaussianFactor { //Constructors JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); JacobianFactor(Vector b_in); JacobianFactor(size_t i1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); @@ -295,6 +294,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(const gtsam::GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const gtsam::VariableSlots& p_variableSlots); + JacobianFactor(const gtsam::GaussianFactor& factor); //Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py index 3ec8648a4..a5688eec8 100644 --- a/python/gtsam/tests/test_Factors.py +++ b/python/gtsam/tests/test_Factors.py @@ -11,9 +11,8 @@ Author: Varun Agrawal """ import unittest -import numpy as np - import gtsam +import numpy as np from gtsam.utils.test_case import GtsamTestCase @@ -21,6 +20,7 @@ class TestNonlinearEquality2Factor(GtsamTestCase): """ Test various instantiations of NonlinearEquality2. """ + def test_point3(self): """Test for Point3 version.""" factor = gtsam.NonlinearEquality2Point3(0, 1) @@ -30,5 +30,23 @@ class TestNonlinearEquality2Factor(GtsamTestCase): np.testing.assert_allclose(error, np.zeros(3)) +class TestJacobianFactor(GtsamTestCase): + """Test JacobianFactor""" + + def test_gaussian_factor_graph(self): + """Test construction from GaussianFactorGraph.""" + gfg = gtsam.GaussianFactorGraph() + jf = gtsam.JacobianFactor(gfg) + self.assertIsInstance(jf, gtsam.JacobianFactor) + + nfg = gtsam.NonlinearFactorGraph() + nfg.push_back(gtsam.PriorFactorDouble(1, 0.0, gtsam.noiseModel.Isotropic.Sigma(1, 1.0))) + values = gtsam.Values() + values.insert(1, 0.0) + gfg = nfg.linearize(values) + jf = gtsam.JacobianFactor(gfg) + self.assertIsInstance(jf, gtsam.JacobianFactor) + + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_sam.py b/python/gtsam/tests/test_sam.py index e01b9c1d1..fd8da5308 100644 --- a/python/gtsam/tests/test_sam.py +++ b/python/gtsam/tests/test_sam.py @@ -50,6 +50,34 @@ class TestSam(GtsamTestCase): self.assertEqual(range_measurement, measurement.range()) self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + def test_BearingRangeFactor3D(self): + """ + Test that `measured` works as expected for BearingRangeFactor3D. + """ + bearing_measurement = gtsam.Unit3() + range_measurement = 10.0 + factor = gtsam.BearingRangeFactor3D( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(3, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + + def test_BearingRangeFactorPose3(self): + """ + Test that `measured` works as expected for BearingRangeFactorPose3. + """ + range_measurement = 10.0 + bearing_measurement = gtsam.Unit3() + factor = gtsam.BearingRangeFactorPose3( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(3, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + if __name__ == "__main__": unittest.main() From 63e0446f0b04fe23e1224b19e9f819c0f9c9a249 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 16:49:15 -0700 Subject: [PATCH 108/121] update triangulation LOST API --- gtsam/geometry/Point3.h | 1 + gtsam/geometry/tests/testTriangulation.cpp | 63 +++--- gtsam/geometry/triangulation.cpp | 159 +++++++-------- gtsam/geometry/triangulation.h | 218 +++++++++++++-------- 4 files changed, 247 insertions(+), 194 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 835c53d7c..b1ac6195b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -33,6 +33,7 @@ namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; +typedef std::vector > Point3Vector; // Convenience typedef using Point3Pair = std::pair; diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0b885d434..731558327 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -96,23 +96,33 @@ TEST(triangulation, twoPoses) { EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } -TEST(triangulation, twoCamerasLOST) { - std::vector> cameras = {camera1, camera2}; - std::vector measurements = {z1, z2}; +TEST(triangulation, twoCamerasUsingLOST) { + CameraSet> cameras; + cameras.push_back(camera1); + cameras.push_back(camera2); + + Point2Vector measurements = {z1, z2}; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); + double rank_tol = 1e-9; // 1. Test simple triangulation, perfect in no noise situation - Point3 actual1 = // - triangulateLOSTPoint3(cameras, measurements); - EXPECT(assert_equal(landmark, actual1, 1e-12)); + boost::optional actual1 = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); + EXPECT(assert_equal(landmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) measurements[0] += Point2(0.1, 0.5); measurements[1] += Point2(-0.2, 0.3); - const double measurement_sigma = 1e-3; - Point3 actual2 = // - triangulateLOSTPoint3(cameras, measurements, measurement_sigma); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4)); + boost::optional actual2 = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual2, 1e-4)); } TEST(triangulation, twoCamerasLOSTvsDLT) { @@ -121,33 +131,32 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { Cal3_S2 identity_K; Pose3 pose_1; Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); - PinholeCamera camera_1(pose_1, identity_K); - PinholeCamera camera_2(pose_2, identity_K); + CameraSet> cameras; + cameras.emplace_back(pose_1, identity_K); + cameras.emplace_back(pose_2, identity_K); Point3 gt_point(0, 0, 1); - Point2 x1 = camera_1.project(gt_point); - Point2 x2 = camera_2.project(gt_point); + Point2 x1_noisy = cameras[0].project(gt_point) + Point2(0.00817, 0.00977); + Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969); + Point2Vector measurements = {x1_noisy, x2_noisy}; - Point2 x1_noisy = x1 + Point2(0.00817, 0.00977); - Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969); + double rank_tol = 1e-9; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - const double measurement_sigma = 1e-2; - Point3 estimate_lost = triangulateLOSTPoint3( - {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma); + boost::optional estimate_lost = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); // These values are from a MATLAB implementation. - EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), estimate_lost, 1e-3)); - - double rank_tol = 1e-9; + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3)); - Pose3Vector poses = {pose_1, pose_2}; - Point2Vector points = {x1_noisy, x2_noisy}; - boost::shared_ptr cal = boost::make_shared(identity_K); boost::optional estimate_dlt = - triangulatePoint3(poses, cal, points, rank_tol, false); + triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. - EXPECT((gt_point - estimate_lost).norm() <= + EXPECT((gt_point - *estimate_lost).norm() <= (gt_point - *estimate_dlt).norm()); } diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9d8325d68..c2c798d8b 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -24,9 +24,9 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { - // number of cameras size_t m = projection_matrices.size(); @@ -52,61 +52,75 @@ Vector4 triangulateHomogeneousDLT( Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); return v; } -Vector3 triangulateLOSTHomogeneous( - const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma) { - size_t m = calibrated_measurements.size(); - assert(m == poses.size()); +Vector4 triangulateHomogeneousDLT( + const std::vector>& + projection_matrices, + const std::vector& measurements, double rank_tol) { + // number of cameras + size_t m = projection_matrices.size(); - // Construct the system matrices. + // Allocate DLT matrix Matrix A = Matrix::Zero(m * 2, 4); for (size_t i = 0; i < m; i++) { - const Pose3& wTi = poses[i]; - // TODO(akshay-krishnan): are there better ways to select j? - const int j = (i + 1) % m; - const Pose3& wTj = poses[j]; + size_t row = i * 2; + const Matrix34& projection = projection_matrices.at(i); + const Point3& p = + measurements.at(i) + .point3(); // to get access to x,y,z of the bearing vector - const Point3 d_ij = wTj.translation() - wTi.translation(); - - const Point3 w_measurement_i = - wTi.rotation().rotate(calibrated_measurements[i]); - const Point3 w_measurement_j = - wTj.rotation().rotate(calibrated_measurements[j]); - - const double q_i = w_measurement_i.cross(w_measurement_j).norm() / - (measurement_sigma * d_ij.cross(w_measurement_j).norm()); - const Matrix23 coefficient_mat = - q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * - wTi.rotation().matrix().transpose(); - - A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation(); + // build system of equations + A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); + A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); } - - const double rank_tol = 1e-6; int rank; double error; Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); + + return v; +} + +Point3 triangulateDLT(const std::vector& poses, + const Point3Vector& homogenousMeasurements, + double rank_tol) { + // number of cameras + size_t m = poses.size(); + + // Allocate DLT matrix + Matrix A = Matrix::Zero(m * 2, 4); + + for (size_t i = 0; i < m; i++) { + size_t row = i * 2; + const Matrix34 projection = poses[i].matrix().block(0, 0, 3, 4); + const Point3& p = homogenousMeasurements[i]; // to get access to x,y,z of + // the bearing vector + + // build system of equations + A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); + A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); + } + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A, rank_tol); + + if (rank < 3) throw(TriangulationUnderconstrainedException()); return Point3(v.head<3>() / v[3]); } -Vector3 triangulateLOSTHomogeneousLS( - const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma) { - size_t m = calibrated_measurements.size(); +Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise) { + size_t m = calibratedMeasurements.size(); assert(m == poses.size()); // Construct the system matrices. @@ -121,70 +135,43 @@ Vector3 triangulateLOSTHomogeneousLS( const Point3 d_ij = wTj.translation() - wTi.translation(); - const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); - const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); + const Point3 w_measurement_i = + wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 w_measurement_j = + wTj.rotation().rotate(calibratedMeasurements[j]); - const double q_i = w_measurement_i.cross(w_measurement_j).norm() / - (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + double q_i = + w_measurement_i.cross(w_measurement_j).norm() / + (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); const Matrix23 coefficient_mat = - q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.block<2, 3>(2*i, 0) = coefficient_mat; - b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation(); + A.block<2, 3>(2 * i, 0) << coefficient_mat; + b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); } return A.colPivHouseholderQr().solve(b); } -Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, - const std::vector& measurements, double rank_tol) { - - // number of cameras - size_t m = projection_matrices.size(); - - // Allocate DLT matrix - Matrix A = Matrix::Zero(m * 2, 4); - - for (size_t i = 0; i < m; i++) { - size_t row = i * 2; - const Matrix34& projection = projection_matrices.at(i); - const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector - - // build system of equations - A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); - A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); - } - int rank; - double error; - Vector v; - boost::tie(rank, error, v) = DLT(A, rank_tol); - - if (rank < 3) - throw(TriangulationUnderconstrainedException()); - - return v; -} - Point3 triangulateDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { - - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } Point3 triangulateDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const std::vector& measurements, double rank_tol) { - // contrary to previous triangulateDLT, this is now taking Unit3 inputs - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); - // Create 3D point from homogeneous coordinates - return Point3(v.head<3>() / v[3]); + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + // Create 3D point from homogeneous coordinates + return Point3(v.head<3>() / v[3]); } /// @@ -215,4 +202,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 5bea4d93c..9e59a92cb 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -62,24 +62,6 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol = 1e-9); -/** - * @brief - * - * @param projection_matrices - * @param measurements - * @param rank_tol - * @return GTSAM_EXPORT - */ -GTSAM_EXPORT Vector3 -triangulateLOSTHomogeneous(const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma); - -GTSAM_EXPORT Vector3 -triangulateLOSTHomogeneousLS(const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma); - /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -112,6 +94,20 @@ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& measurements, double rank_tol = 1e-9); +/** + * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation) + * algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry + * and John Christian. + * @param poses camera poses in world frame + * @param calibratedMeasurements measurements in homogeneous coordinates in each + * camera pose + * @param measurementNoise isotropic noise model for the measurements + * @return triangulated point in world coordinates + */ +GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise); + /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses @@ -320,8 +316,8 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = cameras.size(); - assert(num_meas == measurements.size()); + const size_t num_meas = measurements.size(); + assert(num_meas == cameras.size()); typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); for (size_t ii = 0; ii < num_meas; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that @@ -349,6 +345,65 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( return measurements; } +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using shared camera intrinsics. + * + * @tparam CALIBRATION Calibration type to use. + * @param cal Calibration with which measurements were taken. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurementsShared( + const CALIBRATION& cal, const Point2Vector& measurements) { + Point3Vector calibratedMeasurements; + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), + std::back_inserter(calibratedMeasurements), + [&cal](const Point2& measurement) { + Point3 p; + p << cal.calibrate(measurement), 1.0; + return p; + }); + return calibratedMeasurements; +} + +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using camera intrinsics of each measurement. + * + * @tparam CAMERA Camera type to use. + * @param cameras Cameras corresponding to each measurement. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { + const size_t num_meas = measurements.size(); + assert(num_meas == cameras.size()); + Point3Vector calibratedMeasurements(num_meas); + for (size_t ii = 0; ii < num_meas; ++ii) { + calibratedMeasurements[ii] + << cameras[ii].calibration().calibrate(measurements[ii]), + 1.0; + } + return calibratedMeasurements; +} + +/** Specialize for SphericalCamera to do nothing. */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { + Point3Vector calibratedMeasurements(measurements.size()); + for (size_t ii = 0; ii < measurements.size(); ++ii) { + calibratedMeasurements[ii] << measurements[ii].point3(); + } + return calibratedMeasurements; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -359,6 +414,7 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -366,22 +422,36 @@ Point3 triangulatePoint3(const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = true) { assert(poses.size() == measurements.size()); if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); - - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(*sharedCal, measurements); - // Triangulate linearly - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + Point3 point; + if(use_lost_triangulation) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); + // calibrate the measurements to obtain homogenous coordinates in image plane. + auto calibratedMeasurements = + calibrateMeasurementsShared(*sharedCal, measurements); + + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(*sharedCal, measurements); + + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } // Then refine using non-linear optimization if (optimize) @@ -400,45 +470,6 @@ Point3 triangulatePoint3(const std::vector& poses, return point; } -template -Point3 triangulateLOSTPoint3( - const CameraSet>& cameras, - const Point2Vector& measurements, - const double measurement_sigma = 1e-2, bool use_dlt = false) { - const size_t num_cameras = cameras.size(); - assert(measurements.size() == num_cameras); - - if (num_cameras < 2) throw(TriangulationUnderconstrainedException()); - - // Convert measurements to image plane coordinates. - std::vector calibrated_measurements; - calibrated_measurements.reserve(measurements.size()); - for (int i = 0; i < measurements.size(); ++i) { - Point2 p = cameras[i].calibration().calibrate(measurements[i]); - calibrated_measurements.emplace_back(p.x(), p.y(), 1.0); - } - - std::vector poses; - poses.reserve(cameras.size()); - for (const auto& camera : cameras) poses.push_back(camera.pose()); - - Point3 point = use_dlt - ? triangulateLOSTHomogeneous( - poses, calibrated_measurements, measurement_sigma) - : triangulateLOSTHomogeneousLS( - poses, calibrated_measurements, measurement_sigma); - -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for (const auto& camera : cameras) { - const Point3& p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) throw(TriangulationCheiralityException()); - } -#endif - - return point; -} - /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. This function is similar to the one @@ -449,6 +480,7 @@ Point3 triangulateLOSTPoint3( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -456,7 +488,8 @@ Point3 triangulatePoint3( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation=false) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -464,19 +497,41 @@ Point3 triangulatePoint3( if (m < 2) throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromCameras(cameras); + // Triangulate linearly + Point3 point; + if(use_lost_triangulation) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(cameras, measurements); + // construct poses from cameras. + std::vector poses; + poses.reserve(cameras.size()); + for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + // calibrate the measurements to obtain homogenous coordinates in image + // plane. + auto calibratedMeasurements = + calibrateMeasurements(cameras, measurements); - // The n refine using non-linear optimization - if (optimize) + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromCameras(cameras); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(cameras, measurements); + + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } + + // Then refine using non-linear optimization + if (optimize) { point = triangulateNonlinear(cameras, measurements, point, model); + } #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras @@ -496,9 +551,10 @@ Point3 triangulatePoint3( const CameraSet >& cameras, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model); + (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation); } struct GTSAM_EXPORT TriangulationParameters { From ed5fa923cfe04ddfed1b9e74da9d268ca8009b41 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 16:49:30 -0700 Subject: [PATCH 109/121] Example to run LOST and compare to DLT --- examples/TriangulationLOSTExample.cpp | 161 ++++++++++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 examples/TriangulationLOSTExample.cpp diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp new file mode 100644 index 000000000..a903c625a --- /dev/null +++ b/examples/TriangulationLOSTExample.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * 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 TriangulationLOSTExample.cpp + * @author Akshay Krishnan + * @brief This example runs triangulation several times using 3 different + * approaches: LOST, DLT, and DLT with optimization. It reports the covariance + * and the runtime for each approach. + * + * @date 2022-07-10 + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static std::mt19937 rng(42); + +void PrintCovarianceStats(const Matrix& mat, const std::string& method) { + Matrix centered = mat.rowwise() - mat.colwise().mean(); + Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1); + std::cout << method << " covariance: " << std::endl; + std::cout << cov << std::endl; + std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl; +} + +void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, + const std::string& method) { + double nanoseconds = dur.count() / num_samples; + std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3 + << std::endl; +} + +void GetLargeCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + const double min_xy = -10, max_xy = 10; + const double min_z = -20, max_z = 0; + const int num_cameras = 500; + cameras->reserve(num_cameras); + poses->reserve(num_cameras); + measurements->reserve(num_cameras); + *point = Point3(0.0, 0.0, 10.0); + + std::uniform_real_distribution rand_xy(min_xy, max_xy); + std::uniform_real_distribution rand_z(min_z, max_z); + Cal3_S2 identity_K; + + for (int i = 0; i < num_cameras; ++i) { + Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); + Pose3 wTi(Rot3(), wti); + + poses->push_back(wTi); + cameras->emplace_back(wTi, identity_K); + measurements->push_back(cameras->back().project(*point)); + } +} + +void GetSmallCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + Pose3 pose_1; + Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identity_K; + PinholeCamera camera_1(pose_1, identity_K); + PinholeCamera camera_2(pose_2, identity_K); + + *point = Point3(0, 0, 1); + cameras->push_back(camera_1); + cameras->push_back(camera_2); + *poses = {pose_1, pose_2}; + *measurements = {camera_1.project(*point), camera_2.project(*point)}; +} + +Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, + const double measurement_sigma) { + std::normal_distribution normal(0.0, measurement_sigma); + + Point2Vector noisy_measurements; + noisy_measurements.reserve(measurements.size()); + for (const auto& p : measurements) { + noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + } + return noisy_measurements; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + CameraSet> cameras; + std::vector poses; + Point3 gt_point; + Point2Vector measurements; + GetLargeCamerasDataset(&cameras, &poses, >_point, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, >_point, &measurements); + const double measurement_sigma = 1e-2; + SharedNoiseModel measurement_noise = + noiseModel::Isotropic::Sigma(2, measurement_sigma); + + const long int num_trials = 1000; + Matrix dlt_errors = Matrix::Zero(num_trials, 3); + Matrix lost_errors = Matrix::Zero(num_trials, 3); + Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); + + double rank_tol = 1e-9; + boost::shared_ptr calib = boost::make_shared(); + std::chrono::nanoseconds dlt_duration; + std::chrono::nanoseconds dlt_opt_duration; + std::chrono::nanoseconds lost_duration; + std::chrono::nanoseconds lost_tls_duration; + + for (int i = 0; i < num_trials; i++) { + Point2Vector noisy_measurements = + AddNoiseToMeasurements(measurements, measurement_sigma); + + auto lost_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_lost = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, true); + lost_duration += std::chrono::high_resolution_clock::now() - lost_start; + + auto dlt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, false); + dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; + + auto dlt_opt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt_opt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, true, measurement_noise, false); + dlt_opt_duration += + std::chrono::high_resolution_clock::now() - dlt_opt_start; + + lost_errors.row(i) = *estimate_lost - gt_point; + dlt_errors.row(i) = *estimate_dlt - gt_point; + dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; + } + PrintCovarianceStats(lost_errors, "LOST"); + PrintCovarianceStats(dlt_errors, "DLT"); + PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + + PrintDuration(lost_duration, num_trials, "LOST"); + PrintDuration(dlt_duration, num_trials, "DLT"); + PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); +} \ No newline at end of file From 7d9cf475794e1176d6a31f9b695bd1ade1e27301 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 22:00:18 -0700 Subject: [PATCH 110/121] unit tests pass --- gtsam/geometry/triangulation.cpp | 32 +--------------- gtsam/geometry/triangulation.h | 64 ++++++++++++++++---------------- 2 files changed, 32 insertions(+), 64 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c2c798d8b..c68fffb82 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -88,35 +88,6 @@ Vector4 triangulateHomogeneousDLT( return v; } -Point3 triangulateDLT(const std::vector& poses, - const Point3Vector& homogenousMeasurements, - double rank_tol) { - // number of cameras - size_t m = poses.size(); - - // Allocate DLT matrix - Matrix A = Matrix::Zero(m * 2, 4); - - for (size_t i = 0; i < m; i++) { - size_t row = i * 2; - const Matrix34 projection = poses[i].matrix().block(0, 0, 3, 4); - const Point3& p = homogenousMeasurements[i]; // to get access to x,y,z of - // the bearing vector - - // build system of equations - A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); - A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); - } - int rank; - double error; - Vector v; - boost::tie(rank, error, v) = DLT(A, rank_tol); - - if (rank < 3) throw(TriangulationUnderconstrainedException()); - - return Point3(v.head<3>() / v[3]); -} - Point3 triangulateLOST(const std::vector& poses, const Point3Vector& calibratedMeasurements, const SharedIsotropic& measurementNoise) { @@ -140,7 +111,7 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 w_measurement_j = wTj.rotation().rotate(calibratedMeasurements[j]); - double q_i = + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); const Matrix23 coefficient_mat = @@ -174,7 +145,6 @@ Point3 triangulateDLT( return Point3(v.head<3>() / v[3]); } -/// /** * Optimize for triangulation * @param graph nonlinear factors for projection diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 9e59a92cb..8a709ccd0 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -414,30 +414,30 @@ inline Point3Vector calibrateMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT + * @param use_lost_triangulation whether to use the LOST algorithm instead of + * DLT * @return Returns a Point3 */ -template +template Point3 triangulatePoint3(const std::vector& poses, - boost::shared_ptr sharedCal, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = true) { - + boost::shared_ptr sharedCal, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { assert(poses.size() == measurements.size()); - if (poses.size() < 2) - throw(TriangulationUnderconstrainedException()); + if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if(use_lost_triangulation) { + if (use_lost_triangulation) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma); - // calibrate the measurements to obtain homogenous coordinates in image plane. + // calibrate the measurements to obtain homogenous coordinates in image + // plane. auto calibratedMeasurements = calibrateMeasurementsShared(*sharedCal, measurements); @@ -450,20 +450,20 @@ Point3 triangulatePoint3(const std::vector& poses, auto undistortedMeasurements = undistortMeasurements(*sharedCal, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization if (optimize) - point = triangulateNonlinear // + point = triangulateNonlinear // (poses, sharedCal, measurements, point, model); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const Pose3& pose: poses) { + for (const Pose3& pose : poses) { const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif @@ -480,26 +480,24 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT + * @param use_lost_triangulation whether to use the LOST algorithm instead of + * DLT * @return Returns a Point3 */ -template -Point3 triangulatePoint3( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation=false) { - +template +Point3 triangulatePoint3(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { size_t m = cameras.size(); assert(measurements.size() == m); - if (m < 2) - throw(TriangulationUnderconstrainedException()); + if (m < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if(use_lost_triangulation) { + if (use_lost_triangulation) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -525,7 +523,8 @@ Point3 triangulatePoint3( auto undistortedMeasurements = undistortMeasurements(cameras, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization @@ -535,10 +534,9 @@ Point3 triangulatePoint3( #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const CAMERA& camera: cameras) { + for (const CAMERA& camera : cameras) { const Point3& p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif From 223ea468d6109273ccadbd2ec84ee0c0ed75e294 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 18:45:46 -0700 Subject: [PATCH 111/121] change global variable names in test --- gtsam/geometry/tests/testTriangulation.cpp | 240 ++++++++++----------- 1 file changed, 120 insertions(+), 120 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 731558327..f6408fab9 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -38,24 +38,24 @@ using namespace boost::assign; // Some common constants -static const boost::shared_ptr sharedCal = // +static const boost::shared_ptr kSharedCal = // boost::make_shared(1500, 1200, 0.1, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); -static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -PinholeCamera camera1(pose1, *sharedCal); +static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +static const PinholeCamera kCamera1(kPose1, *kSharedCal); // create second camera 1 meter to the right of first camera -static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); -PinholeCamera camera2(pose2, *sharedCal); +static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0)); +static const PinholeCamera kCamera2(kPose2, *kSharedCal); // landmark ~5 meters infront of camera -static const Point3 landmark(5, 0.5, 1.2); +static const Point3 kLandmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate -Point2 z1 = camera1.project(landmark); -Point2 z2 = camera2.project(landmark); +static const Point2 kZ1 = kCamera1.project(kLandmark); +static const Point2 kZ2 = kCamera2.project(kLandmark); //****************************************************************************** // Simple test with a well-behaved two camera situation @@ -63,22 +63,22 @@ TEST(triangulation, twoPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -86,22 +86,22 @@ TEST(triangulation, twoPoses) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } TEST(triangulation, twoCamerasUsingLOST) { CameraSet> cameras; - cameras.push_back(camera1); - cameras.push_back(camera2); + cameras.push_back(kCamera1); + cameras.push_back(kCamera2); - Point2Vector measurements = {z1, z2}; + Point2Vector measurements = {kZ1, kZ2}; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); double rank_tol = 1e-9; @@ -111,7 +111,7 @@ TEST(triangulation, twoCamerasUsingLOST) { cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, /*use_lost_triangulation=*/true); - EXPECT(assert_equal(landmark, *actual1, 1e-12)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -167,18 +167,18 @@ TEST(triangulation, twoPosesCal3DS2) { boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -188,14 +188,14 @@ TEST(triangulation, twoPosesCal3DS2) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -224,18 +224,18 @@ TEST(triangulation, twoPosesFisheye) { boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -245,14 +245,14 @@ TEST(triangulation, twoPosesFisheye) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -277,17 +277,17 @@ TEST(triangulation, twoPosesFisheye) { TEST(triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // boost::make_shared(1500, 0.1, 0.2, 640, 480); - PinholeCamera camera1(pose1, *bundlerCal); - PinholeCamera camera2(pose2, *bundlerCal); + PinholeCamera camera1(kPose1, *bundlerCal); + PinholeCamera camera2(kPose2, *bundlerCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1, z2; bool optimize = true; @@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) { boost::optional actual = // triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual, 1e-7)); // Add some noise and try again measurements.at(0) += Point2(0.1, 0.5); @@ -313,12 +313,12 @@ TEST(triangulation, fourPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -326,37 +326,37 @@ TEST(triangulation, fourPoses) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); poses += pose3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera camera4(pose4, *sharedCal); + PinholeCamera camera4(pose4, *kSharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); poses += pose4; measurements += Point2(400, 400); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationCheiralityException); #endif } @@ -364,33 +364,33 @@ TEST(triangulation, fourPoses) { //****************************************************************************** TEST(triangulation, threePoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2, pose3; - measurements += z1, z2, z3; + poses += kPose1, kPose2, pose3; + measurements += kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.26m): - EXPECT( (landmark - *actual2).norm() >= 0.2); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.2); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -398,27 +398,27 @@ TEST(triangulation, threePoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1 - measurements += z1, z1, z2, z3; + poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1 + measurements += kZ1, kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! @@ -429,14 +429,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) { // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.17m): - EXPECT( (landmark - *actual2).norm() >= 0.1); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.1); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -444,24 +444,24 @@ TEST(triangulation, fourPoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -471,7 +471,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -480,25 +480,25 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual2 = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = triangulatePoint3(cameras, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { PinholeCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); cameras += camera4; measurements += Point2(400, 400); @@ -519,15 +519,15 @@ TEST(triangulation, fourPoses_distinct_Ks) { TEST(triangulation, fourPoses_distinct_Ks_distortion) { Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -537,22 +537,22 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); } //****************************************************************************** TEST(triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -565,7 +565,7 @@ TEST(triangulation, outliersAndFarLandmarks) { 1.0, false, landmarkDistanceThreshold); // all default except // landmarkDistanceThreshold TriangulationResult actual = triangulateSafe(cameras, measurements, params); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); EXPECT(actual.valid()); landmarkDistanceThreshold = 4; // landmark is farther than that @@ -577,10 +577,10 @@ TEST(triangulation, outliersAndFarLandmarks) { // 3. Add a slightly rotated third camera above with a wrong measurement // (OUTLIER) - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(10, -10); @@ -603,18 +603,18 @@ TEST(triangulation, outliersAndFarLandmarks) { //****************************************************************************** TEST(triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, *sharedCal); + PinholeCamera camera1(kPose1, *kSharedCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); + Point2 z1 = camera1.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1; + poses += kPose1, kPose1; measurements += z1, z1; - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -629,7 +629,7 @@ TEST(triangulation, onePose) { poses += Pose3(); measurements += Point2(0, 0); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -745,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) { std::vector measurements; // Project landmark into two cameras and triangulate - SphericalCamera cam1(pose1); - SphericalCamera cam2(pose2); - Unit3 u1 = cam1.project(landmark); - Unit3 u2 = cam2.project(landmark); + SphericalCamera cam1(kPose1); + SphericalCamera cam2(kPose2); + Unit3 u1 = cam1.project(kLandmark); + Unit3 u2 = cam2.project(kLandmark); - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; @@ -762,25 +762,25 @@ TEST(triangulation, twoPoses_sphericalCamera) { // 1. Test linear triangulation via DLT auto projection_matrices = projectionMatricesFromCameras(cameras); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 2. Test nonlinear triangulation point = triangulateNonlinear(cameras, measurements, point); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 3. Test simple DLT, now within triangulatePoint3 bool optimize = false; boost::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 4. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 5. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -825,7 +825,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, 1e-7)); // behind and to the right of PoseB - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; From da78cc202a33f1eb9ba77aed8edbcb2bb7c37311 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 18:47:37 -0700 Subject: [PATCH 112/121] add author --- gtsam/geometry/tests/testTriangulation.cpp | 1 + gtsam/geometry/triangulation.cpp | 1 + gtsam/geometry/triangulation.h | 1 + 3 files changed, 3 insertions(+) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f6408fab9..0ce67083e 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -15,6 +15,7 @@ * @date July 30th, 2013 * @author Chris Beall (cbeall3) * @author Luca Carlone + * @author Akshay Krishnan */ #include diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c68fffb82..4a8bb279d 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -14,6 +14,7 @@ * @brief Functions for triangulation * @date July 31, 2013 * @author Chris Beall + * @author Akshay Krishnan */ #include diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 8a709ccd0..1b0970fe2 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -15,6 +15,7 @@ * @date July 31, 2013 * @author Chris Beall * @author Luca Carlone + * @author Akshay Krishnan */ #pragma once From b52aaeab22400f9d496611f0a431466d117ad767 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 19:14:26 -0700 Subject: [PATCH 113/121] update to camelcase names in test --- gtsam/geometry/tests/testTriangulation.cpp | 27 +++++++++++----------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0ce67083e..d9e605d55 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -129,36 +129,35 @@ TEST(triangulation, twoCamerasUsingLOST) { TEST(triangulation, twoCamerasLOSTvsDLT) { // LOST has been shown to preform better when the point is much closer to one // camera compared to another. This unit test checks this configuration. - Cal3_S2 identity_K; - Pose3 pose_1; - Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + const Cal3_S2 identityK; + const Pose3 pose1; + const Pose3 pose2(Rot3(), Point3(5., 0., -5.)); CameraSet> cameras; - cameras.emplace_back(pose_1, identity_K); - cameras.emplace_back(pose_2, identity_K); + cameras.emplace_back(pose1, identityK); + cameras.emplace_back(pose2, identityK); - Point3 gt_point(0, 0, 1); - Point2 x1_noisy = cameras[0].project(gt_point) + Point2(0.00817, 0.00977); - Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969); + Point3 landmark(0, 0, 1); + Point2 x1_noisy = cameras[0].project(landmark) + Point2(0.00817, 0.00977); + Point2 x2_noisy = cameras[1].project(landmark) + Point2(-0.00610, 0.01969); Point2Vector measurements = {x1_noisy, x2_noisy}; - double rank_tol = 1e-9; + const double rank_tol = 1e-9; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - boost::optional estimate_lost = // + boost::optional estimateLOST = // triangulatePoint3>( cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, /*use_lost_triangulation=*/true); // These values are from a MATLAB implementation. - EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3)); + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); - boost::optional estimate_dlt = + boost::optional estimateDLT = triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. - EXPECT((gt_point - *estimate_lost).norm() <= - (gt_point - *estimate_dlt).norm()); + EXPECT((landmark - *estimateLOST).norm() <= (landmark - *estimateDLT).norm()); } //****************************************************************************** From f347209d4ce4d36db373519d62ebc47b76c67e2b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 19:30:41 -0700 Subject: [PATCH 114/121] more camelcase changes --- gtsam/geometry/tests/testTriangulation.cpp | 16 ++++++------ gtsam/geometry/triangulation.cpp | 19 +++++++------- gtsam/geometry/triangulation.h | 30 ++++++++++------------ 3 files changed, 31 insertions(+), 34 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index d9e605d55..bc843ad75 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -108,10 +108,10 @@ TEST(triangulation, twoCamerasUsingLOST) { // 1. Test simple triangulation, perfect in no noise situation boost::optional actual1 = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -145,10 +145,10 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); boost::optional estimateLOST = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); // These values are from a MATLAB implementation. EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 4a8bb279d..acb0ddf0e 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -107,20 +107,19 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 d_ij = wTj.translation() - wTi.translation(); - const Point3 w_measurement_i = - wTi.rotation().rotate(calibratedMeasurements[i]); - const Point3 w_measurement_j = - wTj.rotation().rotate(calibratedMeasurements[j]); + const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); - const double q_i = - w_measurement_i.cross(w_measurement_j).norm() / - (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); - const Matrix23 coefficient_mat = + // Note: Setting q_i = 1.0 gives same results as DLT. + const double q_i = wZi.cross(wZj).norm() / + (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + + const Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.block<2, 3>(2 * i, 0) << coefficient_mat; - b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); + A.block<2, 3>(2 * i, 0) << coefficientMat; + b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); } return A.colPivHouseholderQr().solve(b); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1b0970fe2..aa9433484 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -415,8 +415,7 @@ inline Point3Vector calibrateMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of - * DLT + * @param useLOST whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -425,13 +424,13 @@ Point3 triangulatePoint3(const std::vector& poses, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { assert(poses.size() == measurements.size()); if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -481,7 +480,7 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of + * @param useLOST whether to use the LOST algorithm instead of * DLT * @return Returns a Point3 */ @@ -490,7 +489,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -498,7 +497,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -545,15 +544,14 @@ Point3 triangulatePoint3(const CameraSet& cameras, } /// Pinhole-specific version -template -Point3 triangulatePoint3( - const CameraSet >& cameras, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { - return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation); +template +Point3 triangulatePoint3(const CameraSet>& cameras, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool useLOST = false) { + return triangulatePoint3> // + (cameras, measurements, rank_tol, optimize, model, useLOST); } struct GTSAM_EXPORT TriangulationParameters { From 897dae4436e9d8ce5b9518c23cca7abe9e5b7f4b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 20:03:02 -0700 Subject: [PATCH 115/121] more renames to camelcase --- examples/TriangulationLOSTExample.cpp | 130 +++++++++++++------------- gtsam/geometry/triangulation.h | 16 ++-- 2 files changed, 72 insertions(+), 74 deletions(-) diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp index a903c625a..a862026e0 100644 --- a/examples/TriangulationLOSTExample.cpp +++ b/examples/TriangulationLOSTExample.cpp @@ -53,24 +53,24 @@ void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, void GetLargeCamerasDataset(CameraSet>* cameras, std::vector* poses, Point3* point, Point2Vector* measurements) { - const double min_xy = -10, max_xy = 10; - const double min_z = -20, max_z = 0; - const int num_cameras = 500; - cameras->reserve(num_cameras); - poses->reserve(num_cameras); - measurements->reserve(num_cameras); + const double minXY = -10, maxXY = 10; + const double minZ = -20, maxZ = 0; + const int nrCameras = 500; + cameras->reserve(nrCameras); + poses->reserve(nrCameras); + measurements->reserve(nrCameras); *point = Point3(0.0, 0.0, 10.0); - std::uniform_real_distribution rand_xy(min_xy, max_xy); - std::uniform_real_distribution rand_z(min_z, max_z); - Cal3_S2 identity_K; + std::uniform_real_distribution rand_xy(minXY, maxXY); + std::uniform_real_distribution rand_z(minZ, maxZ); + Cal3_S2 identityK; - for (int i = 0; i < num_cameras; ++i) { + for (int i = 0; i < nrCameras; ++i) { Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); Pose3 wTi(Rot3(), wti); poses->push_back(wTi); - cameras->emplace_back(wTi, identity_K); + cameras->emplace_back(wTi, identityK); measurements->push_back(cameras->back().project(*point)); } } @@ -78,84 +78,82 @@ void GetLargeCamerasDataset(CameraSet>* cameras, void GetSmallCamerasDataset(CameraSet>* cameras, std::vector* poses, Point3* point, Point2Vector* measurements) { - Pose3 pose_1; - Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); - Cal3_S2 identity_K; - PinholeCamera camera_1(pose_1, identity_K); - PinholeCamera camera_2(pose_2, identity_K); + Pose3 pose1; + Pose3 pose2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identityK; + PinholeCamera camera1(pose1, identityK); + PinholeCamera camera2(pose2, identityK); *point = Point3(0, 0, 1); - cameras->push_back(camera_1); - cameras->push_back(camera_2); - *poses = {pose_1, pose_2}; - *measurements = {camera_1.project(*point), camera_2.project(*point)}; + cameras->push_back(camera1); + cameras->push_back(camera2); + *poses = {pose1, pose2}; + *measurements = {camera1.project(*point), camera2.project(*point)}; } Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, - const double measurement_sigma) { - std::normal_distribution normal(0.0, measurement_sigma); + const double measurementSigma) { + std::normal_distribution normal(0.0, measurementSigma); - Point2Vector noisy_measurements; - noisy_measurements.reserve(measurements.size()); + Point2Vector noisyMeasurements; + noisyMeasurements.reserve(measurements.size()); for (const auto& p : measurements) { - noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); } - return noisy_measurements; + return noisyMeasurements; } /* ************************************************************************* */ int main(int argc, char* argv[]) { CameraSet> cameras; std::vector poses; - Point3 gt_point; + Point3 landmark; Point2Vector measurements; - GetLargeCamerasDataset(&cameras, &poses, >_point, &measurements); - // GetSmallCamerasDataset(&cameras, &poses, >_point, &measurements); - const double measurement_sigma = 1e-2; - SharedNoiseModel measurement_noise = - noiseModel::Isotropic::Sigma(2, measurement_sigma); + GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements); + const double measurementSigma = 1e-2; + SharedNoiseModel measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - const long int num_trials = 1000; - Matrix dlt_errors = Matrix::Zero(num_trials, 3); - Matrix lost_errors = Matrix::Zero(num_trials, 3); - Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); + const long int nrTrials = 1000; + Matrix errorsDLT = Matrix::Zero(nrTrials, 3); + Matrix errorsLOST = Matrix::Zero(nrTrials, 3); + Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3); double rank_tol = 1e-9; boost::shared_ptr calib = boost::make_shared(); - std::chrono::nanoseconds dlt_duration; - std::chrono::nanoseconds dlt_opt_duration; - std::chrono::nanoseconds lost_duration; - std::chrono::nanoseconds lost_tls_duration; + std::chrono::nanoseconds durationDLT; + std::chrono::nanoseconds durationDLTOpt; + std::chrono::nanoseconds durationLOST; - for (int i = 0; i < num_trials; i++) { - Point2Vector noisy_measurements = - AddNoiseToMeasurements(measurements, measurement_sigma); + for (int i = 0; i < nrTrials; i++) { + Point2Vector noisyMeasurements = + AddNoiseToMeasurements(measurements, measurementSigma); - auto lost_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_lost = triangulatePoint3( - cameras, noisy_measurements, rank_tol, false, measurement_noise, true); - lost_duration += std::chrono::high_resolution_clock::now() - lost_start; + auto lostStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateLOST = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, true); + durationLOST += std::chrono::high_resolution_clock::now() - lostStart; - auto dlt_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_dlt = triangulatePoint3( - cameras, noisy_measurements, rank_tol, false, measurement_noise, false); - dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; + auto dltStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLT = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, false); + durationDLT += std::chrono::high_resolution_clock::now() - dltStart; - auto dlt_opt_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_dlt_opt = triangulatePoint3( - cameras, noisy_measurements, rank_tol, true, measurement_noise, false); - dlt_opt_duration += - std::chrono::high_resolution_clock::now() - dlt_opt_start; + auto dltOptStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLTOpt = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, true, measurementNoise, false); + durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart; - lost_errors.row(i) = *estimate_lost - gt_point; - dlt_errors.row(i) = *estimate_dlt - gt_point; - dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; + errorsLOST.row(i) = *estimateLOST - landmark; + errorsDLT.row(i) = *estimateDLT - landmark; + errorsDLTOpt.row(i) = *estimateDLTOpt - landmark; } - PrintCovarianceStats(lost_errors, "LOST"); - PrintCovarianceStats(dlt_errors, "DLT"); - PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + PrintCovarianceStats(errorsLOST, "LOST"); + PrintCovarianceStats(errorsDLT, "DLT"); + PrintCovarianceStats(errorsDLTOpt, "DLT_OPT"); - PrintDuration(lost_duration, num_trials, "LOST"); - PrintDuration(dlt_duration, num_trials, "DLT"); - PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); -} \ No newline at end of file + PrintDuration(durationLOST, nrTrials, "LOST"); + PrintDuration(durationDLT, nrTrials, "DLT"); + PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT"); +} diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index aa9433484..f6c919b24 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -317,10 +317,10 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. undistortedMeasurements[ii] = @@ -382,10 +382,10 @@ template inline Point3Vector calibrateMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - Point3Vector calibratedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + Point3Vector calibratedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0; From a3ee2660e5809ef40dcd5f9a2d56f32cf7b2efa3 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 22:50:58 -0700 Subject: [PATCH 116/121] LOST in python wrapper --- gtsam/geometry/geometry.i | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 517558265..597f6a169 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1129,7 +1129,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1151,7 +1152,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1173,7 +1175,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, @@ -1195,7 +1198,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Fisheye* sharedCal, const gtsam::Point2Vector& measurements, @@ -1217,7 +1221,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, From b8fb26a0b03f096214c350b57d288d71104aedd7 Mon Sep 17 00:00:00 2001 From: Scott Date: Sat, 16 Jul 2022 15:18:45 -0700 Subject: [PATCH 117/121] Clear ConcurrentMap before loading archive data --- gtsam/base/ConcurrentMap.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 2d7cbd6db..cf4537d4c 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -113,6 +113,7 @@ private: template void load(Archive& ar, const unsigned int /*version*/) { + this->clear(); // Load into STL container and then fill our map FastVector > map; ar & BOOST_SERIALIZATION_NVP(map); From f332d21dd7a1a3cd5ccced3cb5634fb3b1f8b470 Mon Sep 17 00:00:00 2001 From: Scott Date: Sun, 17 Jul 2022 19:00:24 -0700 Subject: [PATCH 118/121] Add unit test for ConcurrentMap serialization --- gtsam/base/tests/testSerializationBase.cpp | 34 ++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index f7aa97b31..d2f4d5f51 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -18,6 +18,7 @@ #include +#include #include #include #include @@ -106,6 +107,39 @@ TEST (Serialization, matrix_vector) { EXPECT(equalityBinary((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished())); } +/* ************************************************************************* */ +TEST (Serialization, ConcurrentMap) { + + ConcurrentMap map; + + map.insert(make_pair(1, "apple")); + map.insert(make_pair(2, "banana")); + + std::string binaryPath = "saved_map.dat"; + try { + std::ofstream outputStream(binaryPath); + boost::archive::binary_oarchive outputArchive(outputStream); + outputArchive << map; + } catch(...) { + EXPECT(false); + } + + // Verify that the existing map contents are replaced by the archive data + ConcurrentMap mapFromDisk; + mapFromDisk.insert(make_pair(3, "clam")); + EXPECT(mapFromDisk.exists(3)); + try { + std::ifstream ifs(binaryPath); + boost::archive::binary_iarchive inputArchive(ifs); + inputArchive >> mapFromDisk; + } catch(...) { + EXPECT(false); + } + EXPECT(mapFromDisk.exists(1)); + EXPECT(mapFromDisk.exists(2)); + EXPECT(!mapFromDisk.exists(3)); +} + /* ************************************************************************* */ From 07c33c46e526c63149f7fad9c8d755bf0fcaf686 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Jul 2022 18:58:41 -0400 Subject: [PATCH 119/121] Set more swap space on linux since gcc runs out of memory --- .github/workflows/build-python.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 3fc2d662f..f42939bc2 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -112,6 +112,11 @@ jobs: run: | echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Set Swap Space + if: runner.os == 'Linux' + uses: pierotofy/set-swap-space@master + with: + swap-size-gb: 6 - name: Build (Linux) if: runner.os == 'Linux' run: | From 31cfce0b8dfc284fb593c2e27f5c0b9c0f374ae7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 05:15:40 -0400 Subject: [PATCH 120/121] update macos CI env --- .github/workflows/build-macos.yml | 10 +++++----- .github/workflows/build-python.yml | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 462723222..7b7646328 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -19,16 +19,16 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - macOS-10.15-xcode-11.3.1, + macos-11-xcode-13.4.1, ] build_type: [Debug, Release] build_unstable: [ON] include: - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macos-11-xcode-13.4.1 + os: macos-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" steps: - name: Checkout @@ -43,7 +43,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f42939bc2..f80b9362c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -22,7 +22,7 @@ jobs: ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, - macOS-10.15-xcode-11.3.1, + macOS-11-xcode-13.4.1, ubuntu-18.04-gcc-5-tbb, ] @@ -52,10 +52,10 @@ jobs: build_type: Debug python_version: "3" - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macOS-11-xcode-13.4.1 + os: macOS-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" - name: ubuntu-18.04-gcc-5-tbb os: ubuntu-18.04 @@ -103,7 +103,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi From 7edd021956e659f1b2ecba5a630682df1d7ea0d2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 14:00:03 -0400 Subject: [PATCH 121/121] fix copying of test files in Cmake --- python/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index cba206d11..0d63c6c54 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -104,7 +104,8 @@ copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" # Hack to get python test and util files copied every time they are modified file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) - configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) + get_filename_component(test_file_name ${test_file} NAME) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY) endforeach() file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py") foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})