diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 4e41cb11d..488ee0d14 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -69,10 +69,25 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// Add HybridConditional to Bayes Net using Base::add; + /// Add a Gaussian Mixture to the Bayes Net. + template + void addMixture(T &&...args) { + push_back(HybridConditional( + boost::make_shared(std::forward(args)...))); + } + + /// Add a Gaussian conditional to the Bayes Net. + template + void addGaussian(T &&...args) { + push_back(HybridConditional( + boost::make_shared(std::forward(args)...))); + } + /// Add a discrete conditional to the Bayes Net. - void add(const DiscreteKey &key, const std::string &table) { - push_back( - HybridConditional(boost::make_shared(key, table))); + template + void addDiscrete(T &&...args) { + push_back(HybridConditional( + boost::make_shared(std::forward(args)...))); } using Base::push_back; diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 899c129e0..acda94638 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -87,7 +87,6 @@ class HybridBayesTreeClique { // double evaluate(const gtsam::HybridValues& values) const; }; -#include class HybridBayesTree { HybridBayesTree(); void print(string s = "HybridBayesTree\n", @@ -105,14 +104,29 @@ class HybridBayesTree { gtsam::DefaultKeyFormatter) const; }; +#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); + bool empty() const; size_t size() const; gtsam::KeySet keys() const; const gtsam::HybridConditional* at(size_t i) const; + + double evaluate(const gtsam::HybridValues& x) const; gtsam::HybridValues optimize() const; + gtsam::HybridValues sample(const gtsam::HybridValues &given) const; + gtsam::HybridValues sample() const; + void print(string s = "HybridBayesNet\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index d22087f47..8c887a2aa 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.add(Asia, "99/1"); + bayesNet.addDiscrete(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.add(Asia, "99/1"); + bayesNet.addDiscrete(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.add(Asia, "99/1"); + bayesNet.addDiscrete(Asia, "99/1"); HybridValues values; values.insert(asiaKey, 0); EXPECT_DOUBLES_EQUAL(0.99, bayesNet.evaluate(values), 1e-9); @@ -85,17 +85,12 @@ TEST(HybridBayesNet, evaluateHybrid) { conditional1 = boost::make_shared( X(1), Vector1::Constant(2), I_1x1, model1); - // TODO(dellaert): creating and adding mixture is clumsy. - const auto mixture = GaussianMixture::FromConditionals( - {X(1)}, {}, {Asia}, {conditional0, conditional1}); - // Create hybrid Bayes net. HybridBayesNet bayesNet; - bayesNet.push_back(HybridConditional( - boost::make_shared(continuousConditional))); - bayesNet.push_back( - HybridConditional(boost::make_shared(mixture))); - bayesNet.add(Asia, "99/1"); + bayesNet.addGaussian(continuousConditional); + bayesNet.addMixture(GaussianMixture::FromConditionals( + {X(1)}, {}, {Asia}, {conditional0, conditional1})); + bayesNet.addDiscrete(Asia, "99/1"); // Create values at which to evaluate. HybridValues values; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 4c1338435..52dc49347 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -46,7 +46,8 @@ namespace gtsam { return optimize(solution); } - VectorValues GaussianBayesNet::optimize(VectorValues solution) const { + VectorValues GaussianBayesNet::optimize(const VectorValues& given) const { + VectorValues solution = given; // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) // solve each node in reverse topological sort order (parents first) for (auto cg : boost::adaptors::reverse(*this)) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 426d3bd83..c8a28e075 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -113,7 +113,7 @@ namespace gtsam { VectorValues optimize() const; /// Version of optimize for incomplete BayesNet, given missing variables - VectorValues optimize(const VectorValues given) const; + VectorValues optimize(const VectorValues& given) const; /** * Sample using ancestral sampling diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index fdf156ff9..f5857b0c5 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -490,6 +490,8 @@ virtual class GaussianConditional : gtsam::JacobianFactor { bool equals(const gtsam::GaussianConditional& cg, double tol) const; // Standard Interface + double evaluate(const gtsam::VectorValues& x) const; + double logDensity(const gtsam::VectorValues& x) const; gtsam::Key firstFrontalKey() const; gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; gtsam::JacobianFactor* likelihood( @@ -543,17 +545,20 @@ virtual class GaussianBayesNet { bool equals(const gtsam::GaussianBayesNet& other, double tol) const; size_t size() const; - // Standard interface void push_back(gtsam::GaussianConditional* conditional); void push_back(const gtsam::GaussianBayesNet& bayesNet); gtsam::GaussianConditional* front() const; gtsam::GaussianConditional* back() const; + // Standard interface + double evaluate(const gtsam::VectorValues& x) const; + double logDensity(const gtsam::VectorValues& x) const; + gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues given) const; + gtsam::VectorValues optimize(const gtsam::VectorValues& given) const; gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues sample(gtsam::VectorValues given) const; + gtsam::VectorValues sample(const gtsam::VectorValues& given) const; gtsam::VectorValues sample() const; gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index bae636d6a..90a062d66 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -12,10 +12,9 @@ */ #include +// NOTE: Needed since we are including pybind11/stl.h. #ifdef GTSAM_ALLOCATOR_TBB PYBIND11_MAKE_OPAQUE(std::vector>); #else PYBIND11_MAKE_OPAQUE(std::vector); #endif - -PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/specializations/hybrid.h b/python/gtsam/specializations/hybrid.h index bede6d86c..e69de29bb 100644 --- a/python/gtsam/specializations/hybrid.h +++ b/python/gtsam/specializations/hybrid.h @@ -1,4 +0,0 @@ - -py::bind_vector >(m_, "GaussianFactorVector"); - -py::implicitly_convertible >(); diff --git a/python/gtsam/tests/test_GaussianBayesNet.py b/python/gtsam/tests/test_GaussianBayesNet.py index e4d396cfe..022de8c3f 100644 --- a/python/gtsam/tests/test_GaussianBayesNet.py +++ b/python/gtsam/tests/test_GaussianBayesNet.py @@ -29,8 +29,7 @@ def smallBayesNet(): """Create a small Bayes Net for testing""" bayesNet = GaussianBayesNet() I_1x1 = np.eye(1, dtype=float) - bayesNet.push_back(GaussianConditional( - _x_, [9.0], I_1x1, _y_, I_1x1)) + bayesNet.push_back(GaussianConditional(_x_, [9.0], I_1x1, _y_, I_1x1)) bayesNet.push_back(GaussianConditional(_y_, [5.0], I_1x1)) return bayesNet @@ -41,13 +40,21 @@ class TestGaussianBayesNet(GtsamTestCase): def test_matrix(self): """Test matrix method""" R, d = smallBayesNet().matrix() # get matrix and RHS - R1 = np.array([ - [1.0, 1.0], - [0.0, 1.0]]) + R1 = np.array([[1.0, 1.0], [0.0, 1.0]]) d1 = np.array([9.0, 5.0]) np.testing.assert_equal(R, R1) np.testing.assert_equal(d, d1) + def test_sample(self): + """Test sample method""" + bayesNet = smallBayesNet() + sample = bayesNet.sample() + self.assertIsInstance(sample, gtsam.VectorValues) -if __name__ == '__main__': + # standard deviation is 1.0 for both, so we set tolerance to 3*sigma + mean = bayesNet.optimize() + self.gtsamAssertEquals(sample, mean, tol=3.0) + + +if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py new file mode 100644 index 000000000..66cddf05e --- /dev/null +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -0,0 +1,69 @@ +""" +GTSAM Copyright 2010-2022, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Hybrid Values. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +import unittest + +import numpy as np +from gtsam.symbol_shorthand import A, X +from gtsam.utils.test_case import GtsamTestCase + +import gtsam +from gtsam import (DiscreteKeys, GaussianConditional, GaussianMixture, + HybridBayesNet, HybridValues, noiseModel) + + +class TestHybridBayesNet(GtsamTestCase): + """Unit tests for HybridValues.""" + def test_evaluate(self): + """Test evaluate for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia).""" + asiaKey = A(0) + Asia = (asiaKey, 2) + + # Create the continuous conditional + I_1x1 = np.eye(1) + gc = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], + 5.0) + + # Create the noise models + model0 = noiseModel.Diagonal.Sigmas([2.0]) + model1 = noiseModel.Diagonal.Sigmas([3.0]) + + # Create the conditionals + conditional0 = GaussianConditional(X(1), [5], I_1x1, model0) + conditional1 = GaussianConditional(X(1), [2], I_1x1, model1) + dkeys = DiscreteKeys() + dkeys.push_back(Asia) + gm = GaussianMixture.FromConditionals([X(1)], [], dkeys, + [conditional0, conditional1]) # + + # Create hybrid Bayes net. + bayesNet = HybridBayesNet() + bayesNet.addGaussian(gc) + bayesNet.addMixture(gm) + bayesNet.addDiscrete(Asia, "99/1") + + # Create values at which to evaluate. + values = HybridValues() + values.insert(asiaKey, 0) + values.insert(X(0), [-6]) + values.insert(X(1), [1]) + + conditionalProbability = gc.evaluate(values.continuous()) + mixtureProbability = conditional0.evaluate(values.continuous()) + self.assertAlmostEqual(conditionalProbability * mixtureProbability * + 0.99, + bayesNet.evaluate(values), + places=5) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 576efa82f..37bb5b93c 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -10,21 +10,19 @@ 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 C, X from gtsam.utils.test_case import GtsamTestCase +import gtsam + class TestHybridGaussianFactorGraph(GtsamTestCase): """Unit tests for HybridGaussianFactorGraph.""" - def test_create(self): - """Test contruction of hybrid factor graph.""" + """Test construction of hybrid factor graph.""" noiseModel = gtsam.noiseModel.Unit.Create(3) dk = gtsam.DiscreteKeys() dk.push_back((C(0), 2)) @@ -45,7 +43,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( hfg, [C(0)])) - # print("hbn = ", hbn) self.assertEqual(hbn.size(), 2) mixture = hbn.at(0).inner() @@ -56,7 +53,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): self.assertIsInstance(discrete_conditional, gtsam.DiscreteConditional) def test_optimize(self): - """Test contruction of hybrid factor graph.""" + """Test construction of hybrid factor graph.""" noiseModel = gtsam.noiseModel.Unit.Create(3) dk = gtsam.DiscreteKeys() dk.push_back((C(0), 2)) @@ -73,16 +70,16 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): hfg.add(jf2) hfg.push_back(gmf) - dtf = gtsam.DecisionTreeFactor([(C(0), 2)],"0 1") + dtf = gtsam.DecisionTreeFactor([(C(0), 2)], "0 1") hfg.add(dtf) hbn = hfg.eliminateSequential( gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( hfg, [C(0)])) - # print("hbn = ", hbn) hv = hbn.optimize() self.assertEqual(hv.atDiscrete(C(0)), 1) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_HybridValues.py b/python/gtsam/tests/test_HybridValues.py index 63e7c8e7d..8a54d518c 100644 --- a/python/gtsam/tests/test_HybridValues.py +++ b/python/gtsam/tests/test_HybridValues.py @@ -1,5 +1,5 @@ """ -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +GTSAM Copyright 2010-2022, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved @@ -20,22 +20,23 @@ from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase -class TestHybridGaussianFactorGraph(GtsamTestCase): +class TestHybridValues(GtsamTestCase): """Unit tests for HybridValues.""" def test_basic(self): - """Test contruction and basic methods of hybrid values.""" - + """Test construction and basic methods of hybrid values.""" + hv1 = gtsam.HybridValues() - hv1.insert(X(0), np.ones((3,1))) + hv1.insert(X(0), np.ones((3, 1))) hv1.insert(C(0), 2) hv2 = gtsam.HybridValues() hv2.insert(C(0), 2) - hv2.insert(X(0), np.ones((3,1))) + hv2.insert(X(0), np.ones((3, 1))) self.assertEqual(hv1.atDiscrete(C(0)), 2) - self.assertEqual(hv1.at(X(0))[0], np.ones((3,1))[0]) + self.assertEqual(hv1.at(X(0))[0], np.ones((3, 1))[0]) + if __name__ == "__main__": unittest.main()