12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1356 from borglab/hybrid/elimination

release/4.3a0
Varun Agrawal 2022-12-29 22:44:17 -05:00 committed by GitHub
parent 04ba19337c a4659f01c7
commit 90c2f2e29f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 423 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
gtsam/hybrid/GaussianMixture.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/base/utilities.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/linear/GaussianFactorGraph.h>
@ -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<GaussianConditional::shared_ptr> &conditionalsList) {
Conditionals dt(discreteParents, conditionalsList);
return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
dt);
}
const std::vector<GaussianConditional::shared_ptr> &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<GaussianMixtureFactor> 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<GaussianMixtureFactor>(
continuousParentKeys, discreteParentKeys, likelihoods);
}
/* ************************************************************************* */
std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
std::set<DiscreteKey> s;

33
gtsam/hybrid/GaussianMixture.h
View File

@ -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<GaussianConditional::shared_ptr> &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<GaussianMixtureFactor> 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.

47
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -35,16 +35,19 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
/* *******************************************************************************/
bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
const This *e = dynamic_cast<const This *>(&lf);
return e != nullptr && Base::equals(*e, tol);
}
if (e == nullptr) return false;
/* *******************************************************************************/
GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const std::vector<GaussianFactor::shared_ptr> &factors) {
Factors dt(discreteKeys, factors);
// 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;
return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
// 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;
}

11
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -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<GaussianFactor::shared_ptr> &factors)
: GaussianMixtureFactor(keys, discreteKeys,
: GaussianMixtureFactor(continuousKeys, discreteKeys,
Factors(discreteKeys, factors)) {}
static This FromFactors(
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const std::vector<GaussianFactor::shared_ptr> &factors);
/// @}
/// @name Testable
/// @{

21
gtsam/hybrid/HybridBayesNet.h
View File

@ -69,23 +69,38 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
/// 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 <typename... T>
void addMixture(T &&...args) {
void emplaceMixture(T &&...args) {
push_back(HybridConditional(
boost::make_shared<GaussianMixture>(std::forward<T>(args)...)));
}
/// Add a Gaussian conditional to the Bayes Net.
template <typename... T>
void addGaussian(T &&...args) {
void emplaceGaussian(T &&...args) {
push_back(HybridConditional(
boost::make_shared<GaussianConditional>(std::forward<T>(args)...)));
}
/// Add a discrete conditional to the Bayes Net.
template <typename... T>
void addDiscrete(T &&...args) {
void emplaceDiscrete(T &&...args) {
push_back(HybridConditional(
boost::make_shared<DiscreteConditional>(std::forward<T>(args)...)));
}

43
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -468,12 +468,51 @@ AlgebraicDecisionTree<Key> 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<HybridConditional>(factor)) {
error += c->asMixture()->error(continuousValues, discreteValues);
}
if (auto f = boost::dynamic_pointer_cast<GaussianMixtureFactor>(factor)) {
error += f->error(continuousValues, discreteValues);
}
} else if (factor->isContinuous()) {
if (auto f = boost::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
error += f->inner()->error(continuousValues);
}
if (auto cg = boost::dynamic_pointer_cast<HybridConditional>(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<Key> HybridGaussianFactorGraph::probPrime(
const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree = this->error(continuousValues);
AlgebraicDecisionTree<Key> prob_tree =
error_tree.apply([](double error) { return exp(-error); });
AlgebraicDecisionTree<Key> prob_tree = error_tree.apply([](double error) {
// NOTE: The 0.5 term is handled by each factor
return exp(-error);
});
return prob_tree;
}

25
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -182,6 +182,19 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
*/
AlgebraicDecisionTree<Key> 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<Key> 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.

58
gtsam/hybrid/hybrid.i
View File

@ -55,7 +55,7 @@ virtual class HybridDiscreteFactor {
#include <gtsam/hybrid/GaussianMixtureFactor.h>
class GaussianMixtureFactor : gtsam::HybridFactor {
static GaussianMixtureFactor FromFactors(
GaussianMixtureFactor(
const gtsam::KeyVector& continuousKeys,
const gtsam::DiscreteKeys& discreteKeys,
const std::vector<gtsam::GaussianFactor::shared_ptr>& factorsList);
@ -67,12 +67,13 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
#include <gtsam/hybrid/GaussianMixture.h>
class GaussianMixture : gtsam::HybridFactor {
static GaussianMixture FromConditionals(
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>&
conditionalsList);
GaussianMixture(const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>&
conditionalsList);
gtsam::GaussianMixtureFactor* likelihood(const gtsam::VectorValues &frontals) const;
void print(string s = "GaussianMixture\n",
const gtsam::KeyFormatter& keyFormatter =
@ -105,18 +106,32 @@ class HybridBayesTree {
gtsam::DefaultKeyFormatter) const;
};
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridBayesNet.h>
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<gtsam::DiscreteKey>& 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<gtsam::GaussianConditional::shared_ptr>&
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<gtsam::DiscreteKey>& 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;
@ -154,9 +169,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);
@ -167,6 +181,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);

2
gtsam/hybrid/tests/Switching.h
View File

@ -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<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
I_3x3, Z_3x1),

57
gtsam/hybrid/tests/testGaussianMixture.cpp
View File

@ -20,6 +20,8 @@
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianConditional.h>
@ -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>(
GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5));
const auto conditional1 = boost::make_shared<GaussianConditional>(
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));
}
/* ************************************************************************* */

14
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -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;

4
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -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<JacobianFactor>(X(1), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(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<JacobianFactor>(X(0), I_3x3, Z_3x1),
boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones())}));

15
gtsam/linear/GaussianConditional.cpp
View File

@ -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));
}

9
gtsam/linear/GaussianConditional.h
View File

@ -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,

4
gtsam/linear/linear.i
View File

@ -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,

5
python/gtsam/tests/test_HybridBayesNet.py
View File

@ -42,14 +42,13 @@ class TestHybridBayesNet(GtsamTestCase):
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()

148
python/gtsam/tests/test_HybridFactorGraph.py
View File

@ -11,75 +11,167 @@ Author: Fan Jiang
# 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()