diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index c0815b2d7a..e344675275 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -36,20 +37,17 @@ GaussianMixture::GaussianMixture( conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixture::Conditionals &GaussianMixture::conditionals() { +const GaussianMixture::Conditionals &GaussianMixture::conditionals() const { return conditionals_; } /* *******************************************************************************/ -GaussianMixture GaussianMixture::FromConditionals( +GaussianMixture::GaussianMixture( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const std::vector &conditionalsList) { - Conditionals dt(discreteParents, conditionalsList); - - return GaussianMixture(continuousFrontals, continuousParents, discreteParents, - dt); -} + const std::vector &conditionalsList) + : GaussianMixture(continuousFrontals, continuousParents, discreteParents, + Conditionals(discreteParents, conditionalsList)) {} /* *******************************************************************************/ GaussianMixture::Sum GaussianMixture::add( @@ -128,6 +126,36 @@ void GaussianMixture::print(const std::string &s, }); } +/* ************************************************************************* */ +KeyVector GaussianMixture::continuousParents() const { + // Get all parent keys: + const auto range = parents(); + KeyVector continuousParentKeys(range.begin(), range.end()); + // Loop over all discrete keys: + for (const auto &discreteKey : discreteKeys()) { + const Key key = discreteKey.first; + // remove that key from continuousParentKeys: + continuousParentKeys.erase(std::remove(continuousParentKeys.begin(), + continuousParentKeys.end(), key), + continuousParentKeys.end()); + } + return continuousParentKeys; +} + +/* ************************************************************************* */ +boost::shared_ptr GaussianMixture::likelihood( + const VectorValues &frontals) const { + // TODO(dellaert): check that values has all frontals + const DiscreteKeys discreteParentKeys = discreteKeys(); + const KeyVector continuousParentKeys = continuousParents(); + const GaussianMixtureFactor::Factors likelihoods( + conditionals(), [&](const GaussianConditional::shared_ptr &conditional) { + return conditional->likelihood(frontals); + }); + return boost::make_shared( + continuousParentKeys, discreteParentKeys, likelihoods); +} + /* ************************************************************************* */ std::set DiscreteKeysAsSet(const DiscreteKeys &dkeys) { std::set s; diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 88d5a02c0c..2cdc23b46d 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -29,6 +29,8 @@ namespace gtsam { +class GaussianMixtureFactor; + /** * @brief A conditional of gaussian mixtures indexed by discrete variables, as * part of a Bayes Network. This is the result of the elimination of a @@ -112,21 +114,11 @@ class GTSAM_EXPORT GaussianMixture * @param discreteParents Discrete parents variables * @param conditionals List of conditionals */ - static This FromConditionals( + GaussianMixture( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionals); - /// @} - /// @name Standard API - /// @{ - - GaussianConditional::shared_ptr operator()( - const DiscreteValues &discreteValues) const; - - /// Returns the total number of continuous components - size_t nrComponents() const; - /// @} /// @name Testable /// @{ @@ -140,9 +132,25 @@ class GTSAM_EXPORT GaussianMixture const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} + /// @name Standard API + /// @{ + + GaussianConditional::shared_ptr operator()( + const DiscreteValues &discreteValues) const; + + /// Returns the total number of continuous components + size_t nrComponents() const; + + /// Returns the continuous keys among the parents. + KeyVector continuousParents() const; + + // Create a likelihood factor for a Gaussian mixture, return a Mixture factor + // on the parents. + boost::shared_ptr likelihood( + const VectorValues &frontals) const; /// Getter for the underlying Conditionals DecisionTree - const Conditionals &conditionals(); + const Conditionals &conditionals() const; /** * @brief Compute error of the GaussianMixture as a tree. @@ -181,6 +189,7 @@ class GTSAM_EXPORT GaussianMixture * @return Sum */ Sum add(const Sum &sum) const; + /// @} }; /// Return the DiscreteKey vector as a set. diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index fd437f52c0..32ca1432cd 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -35,16 +35,19 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { const This *e = dynamic_cast(&lf); - return e != nullptr && Base::equals(*e, tol); -} - -/* *******************************************************************************/ -GaussianMixtureFactor GaussianMixtureFactor::FromFactors( - const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const std::vector &factors) { - Factors dt(discreteKeys, factors); - - return GaussianMixtureFactor(continuousKeys, discreteKeys, dt); + if (e == nullptr) return false; + + // This will return false if either factors_ is empty or e->factors_ is empty, + // but not if both are empty or both are not empty: + if (factors_.empty() ^ e->factors_.empty()) return false; + + // Check the base and the factors: + return Base::equals(*e, tol) && + factors_.equals(e->factors_, + [tol](const GaussianFactor::shared_ptr &f1, + const GaussianFactor::shared_ptr &f2) { + return f1->equals(*f2, tol); + }); } /* *******************************************************************************/ @@ -52,18 +55,22 @@ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); std::cout << "{\n"; - factors_.print( - "", [&](Key k) { return formatter(k); }, - [&](const GaussianFactor::shared_ptr &gf) -> std::string { - RedirectCout rd; - std::cout << ":\n"; - if (gf && !gf->empty()) { - gf->print("", formatter); - return rd.str(); - } else { - return "nullptr"; - } - }); + if (factors_.empty()) { + std::cout << " empty" << std::endl; + } else { + factors_.print( + "", [&](Key k) { return formatter(k); }, + [&](const GaussianFactor::shared_ptr &gf) -> std::string { + RedirectCout rd; + std::cout << ":\n"; + if (gf && !gf->empty()) { + gf->print("", formatter); + return rd.str(); + } else { + return "nullptr"; + } + }); + } std::cout << "}" << std::endl; } diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 0b65b5aa93..b8f475de3f 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -93,19 +93,16 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @brief Construct a new GaussianMixtureFactor object using a vector of * GaussianFactor shared pointers. * - * @param keys Vector of keys for continuous factors. + * @param continuousKeys 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, + GaussianMixtureFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys, const std::vector &factors) - : GaussianMixtureFactor(keys, discreteKeys, + : GaussianMixtureFactor(continuousKeys, discreteKeys, Factors(discreteKeys, factors)) {} - static This FromFactors( - const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const std::vector &factors); - /// @} /// @name Testable /// @{ diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 488ee0d14f..a64b3bb4f0 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -69,23 +69,38 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// Add HybridConditional to Bayes Net using Base::add; + /// Add a Gaussian Mixture to the Bayes Net. + void addMixture(const GaussianMixture::shared_ptr &ptr) { + push_back(HybridConditional(ptr)); + } + + /// Add a Gaussian conditional to the Bayes Net. + void addGaussian(const GaussianConditional::shared_ptr &ptr) { + push_back(HybridConditional(ptr)); + } + + /// Add a discrete conditional to the Bayes Net. + void addDiscrete(const DiscreteConditional::shared_ptr &ptr) { + push_back(HybridConditional(ptr)); + } + /// Add a Gaussian Mixture to the Bayes Net. template - void addMixture(T &&...args) { + void emplaceMixture(T &&...args) { push_back(HybridConditional( boost::make_shared(std::forward(args)...))); } /// Add a Gaussian conditional to the Bayes Net. template - void addGaussian(T &&...args) { + void emplaceGaussian(T &&...args) { push_back(HybridConditional( boost::make_shared(std::forward(args)...))); } /// Add a discrete conditional to the Bayes Net. template - void addDiscrete(T &&...args) { + void emplaceDiscrete(T &&...args) { push_back(HybridConditional( boost::make_shared(std::forward(args)...))); } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 32653bdecf..b110f8586a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -468,12 +468,51 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( return error_tree; } +/* ************************************************************************ */ +double HybridGaussianFactorGraph::error( + const VectorValues &continuousValues, + const DiscreteValues &discreteValues) const { + double error = 0.0; + for (size_t idx = 0; idx < size(); idx++) { + auto factor = factors_.at(idx); + + if (factor->isHybrid()) { + if (auto c = boost::dynamic_pointer_cast(factor)) { + error += c->asMixture()->error(continuousValues, discreteValues); + } + if (auto f = boost::dynamic_pointer_cast(factor)) { + error += f->error(continuousValues, discreteValues); + } + + } else if (factor->isContinuous()) { + if (auto f = boost::dynamic_pointer_cast(factor)) { + error += f->inner()->error(continuousValues); + } + if (auto cg = boost::dynamic_pointer_cast(factor)) { + error += cg->asGaussian()->error(continuousValues); + } + } + } + return error; +} + +/* ************************************************************************ */ +double HybridGaussianFactorGraph::probPrime( + const VectorValues &continuousValues, + const DiscreteValues &discreteValues) const { + double error = this->error(continuousValues, discreteValues); + // NOTE: The 0.5 term is handled by each factor + return std::exp(-error); +} + /* ************************************************************************ */ AlgebraicDecisionTree HybridGaussianFactorGraph::probPrime( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree = this->error(continuousValues); - AlgebraicDecisionTree prob_tree = - error_tree.apply([](double error) { return exp(-error); }); + AlgebraicDecisionTree prob_tree = error_tree.apply([](double error) { + // NOTE: The 0.5 term is handled by each factor + return exp(-error); + }); return prob_tree; } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index ac9ae1a462..9de18b6af1 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -182,6 +182,19 @@ class GTSAM_EXPORT HybridGaussianFactorGraph */ AlgebraicDecisionTree error(const VectorValues& continuousValues) const; + /** + * @brief Compute error given a continuous vector values + * and a discrete assignment. + * + * @param continuousValues The continuous VectorValues + * for computing the error. + * @param discreteValues The specific discrete assignment + * whose error we wish to compute. + * @return double + */ + double error(const VectorValues& continuousValues, + const DiscreteValues& discreteValues) const; + /** * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$ * for each discrete assignment, and return as a tree. @@ -193,6 +206,18 @@ class GTSAM_EXPORT HybridGaussianFactorGraph AlgebraicDecisionTree probPrime( const VectorValues& continuousValues) const; + /** + * @brief Compute the unnormalized posterior probability for a continuous + * vector values given a specific assignment. + * + * @param continuousValues The vector values for which to compute the + * posterior probability. + * @param discreteValues The specific assignment to use for the computation. + * @return double + */ + double probPrime(const VectorValues& continuousValues, + const DiscreteValues& discreteValues) const; + /** * @brief Return a Colamd constrained ordering where the discrete keys are * eliminated after the continuous keys. diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index acda946385..3dbf5d5423 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -54,7 +54,7 @@ virtual class HybridDiscreteFactor { #include class GaussianMixtureFactor : gtsam::HybridFactor { - static GaussianMixtureFactor FromFactors( + GaussianMixtureFactor( const gtsam::KeyVector& continuousKeys, const gtsam::DiscreteKeys& discreteKeys, const std::vector& factorsList); @@ -66,12 +66,13 @@ class GaussianMixtureFactor : gtsam::HybridFactor { #include class GaussianMixture : gtsam::HybridFactor { - static GaussianMixture FromConditionals( - const gtsam::KeyVector& continuousFrontals, - const gtsam::KeyVector& continuousParents, - const gtsam::DiscreteKeys& discreteParents, - const std::vector& - conditionalsList); + GaussianMixture(const gtsam::KeyVector& continuousFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKeys& discreteParents, + const std::vector& + conditionalsList); + + gtsam::GaussianMixtureFactor* likelihood(const gtsam::VectorValues &frontals) const; void print(string s = "GaussianMixture\n", const gtsam::KeyFormatter& keyFormatter = @@ -104,18 +105,32 @@ class HybridBayesTree { gtsam::DefaultKeyFormatter) const; }; -#include +#include class HybridBayesNet { HybridBayesNet(); void add(const gtsam::HybridConditional& s); - void addMixture(const gtsam::GaussianMixture& s); - void addGaussian(const gtsam::GaussianConditional& s); - void addDiscrete(const gtsam::DiscreteConditional& s); - void addDiscrete(const gtsam::DiscreteKey& key, string spec); - void addDiscrete(const gtsam::DiscreteKey& key, - const gtsam::DiscreteKeys& parents, string spec); - void addDiscrete(const gtsam::DiscreteKey& key, - const std::vector& parents, string spec); + void addMixture(const gtsam::GaussianMixture* s); + void addGaussian(const gtsam::GaussianConditional* s); + void addDiscrete(const gtsam::DiscreteConditional* s); + + void emplaceMixture(const gtsam::GaussianMixture& s); + void emplaceMixture(const gtsam::KeyVector& continuousFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKeys& discreteParents, + const std::vector& + conditionalsList); + void emplaceGaussian(const gtsam::GaussianConditional& s); + void emplaceDiscrete(const gtsam::DiscreteConditional& s); + void emplaceDiscrete(const gtsam::DiscreteKey& key, string spec); + void emplaceDiscrete(const gtsam::DiscreteKey& key, + const gtsam::DiscreteKeys& parents, string spec); + void emplaceDiscrete(const gtsam::DiscreteKey& key, + const std::vector& parents, + string spec); + + gtsam::GaussianMixture* atMixture(size_t i) const; + gtsam::GaussianConditional* atGaussian(size_t i) const; + gtsam::DiscreteConditional* atDiscrete(size_t i) const; bool empty() const; size_t size() const; @@ -153,9 +168,8 @@ class HybridGaussianFactorGraph { 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); + void push_back(gtsam::DecisionTreeFactor* factor); + void push_back(gtsam::JacobianFactor* factor); bool empty() const; void remove(size_t i); @@ -166,6 +180,12 @@ class HybridGaussianFactorGraph { void print(string s = "") const; bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const; + // evaluation + double error(const gtsam::VectorValues& continuousValues, + const gtsam::DiscreteValues& discreteValues) const; + double probPrime(const gtsam::VectorValues& continuousValues, + const gtsam::DiscreteValues& discreteValues) const; + gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential( gtsam::Ordering::OrderingType type); diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index f9e1916d07..59c57f8a0a 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -57,7 +57,7 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( // keyFunc(1) to keyFunc(n+1) for (size_t t = 1; t < n; t++) { - hfg.add(GaussianMixtureFactor::FromFactors( + hfg.add(GaussianMixtureFactor( {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/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 310081f028..242c9ba412 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include @@ -33,6 +35,7 @@ using namespace gtsam; using noiseModel::Isotropic; using symbol_shorthand::M; using symbol_shorthand::X; +using symbol_shorthand::Z; /* ************************************************************************* */ /* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a @@ -127,7 +130,59 @@ TEST(GaussianMixture, Error) { assignment[M(1)] = 0; EXPECT_DOUBLES_EQUAL(0.5, mixture.error(values, assignment), 1e-8); assignment[M(1)] = 1; - EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment), 1e-8); + EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment), + 1e-8); +} + +/* ************************************************************************* */ +// Create mode key: 0 is low-noise, 1 is high-noise. +static const Key modeKey = M(0); +static const DiscreteKey mode(modeKey, 2); + +// Create a simple GaussianMixture +static GaussianMixture createSimpleGaussianMixture() { + // Create Gaussian mixture Z(0) = X(0) + noise. + // TODO(dellaert): making copies below is not ideal ! + Matrix1 I = Matrix1::Identity(); + const auto conditional0 = boost::make_shared( + GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5)); + const auto conditional1 = boost::make_shared( + GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); + return GaussianMixture({Z(0)}, {X(0)}, {mode}, {conditional0, conditional1}); +} + +/* ************************************************************************* */ +// Create a test for continuousParents. +TEST(GaussianMixture, ContinuousParents) { + const GaussianMixture gm = createSimpleGaussianMixture(); + const KeyVector continuousParentKeys = gm.continuousParents(); + // Check that the continuous parent keys are correct: + EXPECT(continuousParentKeys.size() == 1); + EXPECT(continuousParentKeys[0] == X(0)); +} + +/* ************************************************************************* */ +/// Check that likelihood returns a mixture factor on the parents. +TEST(GaussianMixture, Likelihood) { + const GaussianMixture gm = createSimpleGaussianMixture(); + + // Call the likelihood function: + VectorValues measurements; + measurements.insert(Z(0), Vector1(0)); + const auto factor = gm.likelihood(measurements); + + // Check that the factor is a mixture factor on the parents. + // Loop over all discrete assignments over the discrete parents: + const DiscreteKeys discreteParentKeys = gm.discreteKeys(); + + // Apply the likelihood function to all conditionals: + const GaussianMixtureFactor::Factors factors( + gm.conditionals(), + [measurements](const GaussianConditional::shared_ptr& conditional) { + return conditional->likelihood(measurements); + }); + const GaussianMixtureFactor expected({X(0)}, {mode}, factors); + EXPECT(assert_equal(*factor, expected)); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 8c887a2aa0..627d826945 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -43,7 +43,7 @@ static const DiscreteKey Asia(asiaKey, 2); // Test creation of a pure discrete Bayes net. TEST(HybridBayesNet, Creation) { HybridBayesNet bayesNet; - bayesNet.addDiscrete(Asia, "99/1"); + bayesNet.emplaceDiscrete(Asia, "99/1"); DiscreteConditional expected(Asia, "99/1"); CHECK(bayesNet.atDiscrete(0)); @@ -54,7 +54,7 @@ TEST(HybridBayesNet, Creation) { // Test adding a Bayes net to another one. TEST(HybridBayesNet, Add) { HybridBayesNet bayesNet; - bayesNet.addDiscrete(Asia, "99/1"); + bayesNet.emplaceDiscrete(Asia, "99/1"); HybridBayesNet other; other.push_back(bayesNet); @@ -65,7 +65,7 @@ TEST(HybridBayesNet, Add) { // Test evaluate for a pure discrete Bayes net P(Asia). TEST(HybridBayesNet, evaluatePureDiscrete) { HybridBayesNet bayesNet; - bayesNet.addDiscrete(Asia, "99/1"); + bayesNet.emplaceDiscrete(Asia, "99/1"); HybridValues values; values.insert(asiaKey, 0); EXPECT_DOUBLES_EQUAL(0.99, bayesNet.evaluate(values), 1e-9); @@ -87,10 +87,10 @@ TEST(HybridBayesNet, evaluateHybrid) { // Create hybrid Bayes net. HybridBayesNet bayesNet; - bayesNet.addGaussian(continuousConditional); - bayesNet.addMixture(GaussianMixture::FromConditionals( - {X(1)}, {}, {Asia}, {conditional0, conditional1})); - bayesNet.addDiscrete(Asia, "99/1"); + bayesNet.emplaceGaussian(continuousConditional); + GaussianMixture gm({X(1)}, {}, {Asia}, {conditional0, conditional1}); + bayesNet.emplaceMixture(gm); // copy :-( + bayesNet.emplaceDiscrete(Asia, "99/1"); // Create values at which to evaluate. HybridValues values; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 55e4c28adf..f774e8ef10 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -176,7 +176,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(GaussianMixtureFactor::FromFactors( + hfg.add(GaussianMixtureFactor( {X(1)}, {{M(1), 2}}, {boost::make_shared(X(1), I_3x3, Z_3x1), boost::make_shared(X(1), I_3x3, Vector3::Ones())})); @@ -235,7 +235,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); { - hfg.add(GaussianMixtureFactor::FromFactors( + hfg.add(GaussianMixtureFactor( {X(0)}, {{M(0), 2}}, {boost::make_shared(X(0), I_3x3, Z_3x1), boost::make_shared(X(0), I_3x3, Vector3::Ones())})); diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 5e8a193cf1..7cdff914f2 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -63,13 +63,24 @@ namespace gtsam { : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas), BaseConditional(1) {} + /* ************************************************************************ */ + GaussianConditional GaussianConditional::FromMeanAndStddev(Key key, + const Vector& mu, + double sigma) { + // |Rx - d| = |x-(Ay + b)|/sigma + const Matrix R = Matrix::Identity(mu.size(), mu.size()); + const Vector& d = mu; + return GaussianConditional(key, d, R, + noiseModel::Isotropic::Sigma(mu.size(), sigma)); + } + /* ************************************************************************ */ GaussianConditional GaussianConditional::FromMeanAndStddev( Key key, const Matrix& A, Key parent, const Vector& b, double sigma) { // |Rx + Sy - d| = |x-(Ay + b)|/sigma const Matrix R = Matrix::Identity(b.size(), b.size()); const Matrix S = -A; - const Vector d = b; + const Vector& d = b; return GaussianConditional(key, d, R, parent, S, noiseModel::Isotropic::Sigma(b.size(), sigma)); } @@ -82,7 +93,7 @@ namespace gtsam { const Matrix R = Matrix::Identity(b.size(), b.size()); const Matrix S = -A1; const Matrix T = -A2; - const Vector d = b; + const Vector& d = b; return GaussianConditional(key, d, R, parent1, S, parent2, T, noiseModel::Isotropic::Sigma(b.size(), sigma)); } diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index a72a739737..af1c5d80e5 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -84,12 +84,17 @@ namespace gtsam { const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - /// Construct from mean A1 p1 + b and standard deviation. + /// Construct from mean `mu` and standard deviation `sigma`. + static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu, + double sigma); + + /// Construct from conditional mean `A1 p1 + b` and standard deviation. static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A, Key parent, const Vector& b, double sigma); - /// Construct from mean A1 p1 + A2 p2 + b and standard deviation. + /// Construct from conditional mean `A1 p1 + A2 p2 + b` and standard + /// deviation `sigma`. static GaussianConditional FromMeanAndStddev(Key key, // const Matrix& A1, Key parent1, const Matrix& A2, Key parent2, diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index f5857b0c5a..6f241da55f 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -470,6 +470,10 @@ virtual class GaussianConditional : gtsam::JacobianFactor { size_t name2, Matrix T); // Named constructors + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, + const Vector& mu, + double sigma); + static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, const Matrix& A, gtsam::Key parent, diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 66cddf05e7..af89a4ba7c 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -42,14 +42,13 @@ def test_evaluate(self): conditional1 = GaussianConditional(X(1), [2], I_1x1, model1) dkeys = DiscreteKeys() dkeys.push_back(Asia) - gm = GaussianMixture.FromConditionals([X(1)], [], dkeys, - [conditional0, conditional1]) # + gm = GaussianMixture([X(1)], [], dkeys, [conditional0, conditional1]) # Create hybrid Bayes net. bayesNet = HybridBayesNet() bayesNet.addGaussian(gc) bayesNet.addMixture(gm) - bayesNet.addDiscrete(Asia, "99/1") + bayesNet.emplaceDiscrete(Asia, "99/1") # Create values at which to evaluate. values = HybridValues() diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 37bb5b93c6..2ebc87971a 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -11,75 +11,167 @@ # pylint: disable=invalid-name, no-name-in-module, no-member import unittest +import math import numpy as np -from gtsam.symbol_shorthand import C, X +from gtsam.symbol_shorthand import C, M, X, Z from gtsam.utils.test_case import GtsamTestCase import gtsam +from gtsam import ( + DecisionTreeFactor, + DiscreteConditional, + DiscreteKeys, + GaussianConditional, + GaussianMixture, + GaussianMixtureFactor, + HybridGaussianFactorGraph, + JacobianFactor, + Ordering, + noiseModel, +) class TestHybridGaussianFactorGraph(GtsamTestCase): """Unit tests for HybridGaussianFactorGraph.""" + def test_create(self): """Test construction of hybrid factor graph.""" - noiseModel = gtsam.noiseModel.Unit.Create(3) - dk = gtsam.DiscreteKeys() + model = noiseModel.Unit.Create(3) + dk = DiscreteKeys() dk.push_back((C(0), 2)) - 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) + jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) + jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) + gmf = GaussianMixtureFactor([X(0)], dk, [jf1, jf2]) - hfg = gtsam.HybridGaussianFactorGraph() - hfg.add(jf1) - hfg.add(jf2) + hfg = HybridGaussianFactorGraph() + hfg.push_back(jf1) + hfg.push_back(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( - hfg, [C(0)])) + Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) + ) self.assertEqual(hbn.size(), 2) mixture = hbn.at(0).inner() - self.assertIsInstance(mixture, gtsam.GaussianMixture) + self.assertIsInstance(mixture, GaussianMixture) self.assertEqual(len(mixture.keys()), 2) discrete_conditional = hbn.at(hbn.size() - 1).inner() - self.assertIsInstance(discrete_conditional, gtsam.DiscreteConditional) + self.assertIsInstance(discrete_conditional, DiscreteConditional) def test_optimize(self): """Test construction of hybrid factor graph.""" - noiseModel = gtsam.noiseModel.Unit.Create(3) - dk = gtsam.DiscreteKeys() + model = noiseModel.Unit.Create(3) + dk = DiscreteKeys() dk.push_back((C(0), 2)) - 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) + jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) + jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) + gmf = GaussianMixtureFactor([X(0)], dk, [jf1, jf2]) - hfg = gtsam.HybridGaussianFactorGraph() - hfg.add(jf1) - hfg.add(jf2) + hfg = HybridGaussianFactorGraph() + hfg.push_back(jf1) + hfg.push_back(jf2) hfg.push_back(gmf) dtf = gtsam.DecisionTreeFactor([(C(0), 2)], "0 1") - hfg.add(dtf) + hfg.push_back(dtf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( - hfg, [C(0)])) + Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) + ) hv = hbn.optimize() self.assertEqual(hv.atDiscrete(C(0)), 1) + @staticmethod + def tiny(num_measurements: int = 1): + """Create a tiny two variable hybrid model.""" + # Create hybrid Bayes net. + bayesNet = gtsam.HybridBayesNet() + + # Create mode key: 0 is low-noise, 1 is high-noise. + modeKey = M(0) + mode = (modeKey, 2) + + # Create Gaussian mixture Z(0) = X(0) + noise for each measurement. + I = np.eye(1) + keys = DiscreteKeys() + keys.push_back(mode) + for i in range(num_measurements): + conditional0 = GaussianConditional.FromMeanAndStddev( + Z(i), I, X(0), [0], sigma=0.5 + ) + conditional1 = GaussianConditional.FromMeanAndStddev( + Z(i), I, X(0), [0], sigma=3 + ) + bayesNet.emplaceMixture( + [Z(i)], [X(0)], keys, [conditional0, conditional1] + ) + + # Create prior on X(0). + prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0) + bayesNet.addGaussian(prior_on_x0) + + # Add prior on mode. + bayesNet.emplaceDiscrete(mode, "1/1") + + return bayesNet + + def test_tiny(self): + """Test a tiny two variable hybrid model.""" + bayesNet = self.tiny() + sample = bayesNet.sample() + # print(sample) + + # Create a factor graph from the Bayes net with sampled measurements. + fg = HybridGaussianFactorGraph() + conditional = bayesNet.atMixture(0) + measurement = gtsam.VectorValues() + measurement.insert(Z(0), sample.at(Z(0))) + factor = conditional.likelihood(measurement) + fg.push_back(factor) + fg.push_back(bayesNet.atGaussian(1)) + fg.push_back(bayesNet.atDiscrete(2)) + + self.assertEqual(fg.size(), 3) + + def test_tiny2(self): + """Test a tiny two variable hybrid model, with 2 measurements.""" + # Create the Bayes net and sample from it. + bayesNet = self.tiny(num_measurements=2) + sample = bayesNet.sample() + # print(sample) + + # Create a factor graph from the Bayes net with sampled measurements. + fg = HybridGaussianFactorGraph() + for i in range(2): + conditional = bayesNet.atMixture(i) + measurement = gtsam.VectorValues() + measurement.insert(Z(i), sample.at(Z(i))) + factor = conditional.likelihood(measurement) + fg.push_back(factor) + fg.push_back(bayesNet.atGaussian(2)) + fg.push_back(bayesNet.atDiscrete(3)) + + self.assertEqual(fg.size(), 4) + # Calculate ratio between Bayes net probability and the factor graph: + continuousValues = gtsam.VectorValues() + continuousValues.insert(X(0), sample.at(X(0))) + discreteValues = sample.discrete() + expected_ratio = bayesNet.evaluate(sample) / fg.probPrime( + continuousValues, discreteValues + ) + print(expected_ratio) + + # TODO(dellaert): Change the mode to 0 and calculate the ratio again. + if __name__ == "__main__": unittest.main()