From 356b89a165c274c50cf6aae4d9fd9cd48ec1ed4d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Dec 2022 16:51:24 -0500 Subject: [PATCH 01/11] Fix to const & --- gtsam/linear/GaussianBayesNet.cpp | 3 ++- gtsam/linear/GaussianBayesNet.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 4c13384356..52dc49347a 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 426d3bd833..c8a28e0757 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 From 0495f81104ad32d8c97a32364779812e6c031102 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Dec 2022 16:51:40 -0500 Subject: [PATCH 02/11] Test for GBN::sample --- gtsam/linear/linear.i | 11 ++++++++--- python/gtsam/tests/test_GaussianBayesNet.py | 19 +++++++++++++------ 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index fdf156ff99..f5857b0c5a 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/tests/test_GaussianBayesNet.py b/python/gtsam/tests/test_GaussianBayesNet.py index e4d396cfe8..022de8c3fd 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() From d9511d6dc2fe746e661ca5b9050cf737ad2d7004 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Dec 2022 17:47:41 -0500 Subject: [PATCH 03/11] Convenience constructors --- gtsam/hybrid/HybridBayesNet.h | 21 ++++++++++++++++++--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 19 +++++++------------ 2 files changed, 25 insertions(+), 15 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 4e41cb11df..488ee0d14f 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/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index d22087f47b..8c887a2aa0 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; From 1de49598afd398fc061f095bd8f7a6848b75e741 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Dec 2022 18:05:46 -0500 Subject: [PATCH 04/11] Add methods in HybridBayesNet --- gtsam/hybrid/hybrid.i | 16 +++++++++++++++- python/gtsam/preamble/hybrid.h | 1 + python/gtsam/specializations/hybrid.h | 4 +++- 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 899c129e00..acda946385 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/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index bae636d6a5..7008a1f076 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -19,3 +19,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); #endif PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/specializations/hybrid.h b/python/gtsam/specializations/hybrid.h index bede6d86c4..db5240117e 100644 --- a/python/gtsam/specializations/hybrid.h +++ b/python/gtsam/specializations/hybrid.h @@ -1,4 +1,6 @@ py::bind_vector >(m_, "GaussianFactorVector"); - py::implicitly_convertible >(); + +py::bind_vector >(m_, "GaussianConditionalVector"); +py::implicitly_convertible >(); From fd12181ebe41d5243febb09af1f77c23dc4596f6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Dec 2022 18:10:00 -0500 Subject: [PATCH 05/11] Cleanup --- python/gtsam/tests/test_HybridValues.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_HybridValues.py b/python/gtsam/tests/test_HybridValues.py index 63e7c8e7dd..8a54d518ce 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.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() From 7c91fe82b4963389bd8cb23f224ab6617af3a5a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Dec 2022 18:10:07 -0500 Subject: [PATCH 06/11] Add evaluate test --- python/gtsam/tests/test_HybridBayesNet.py | 66 +++++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 python/gtsam/tests/test_HybridBayesNet.py diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py new file mode 100644 index 0000000000..cfe080dcb0 --- /dev/null +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -0,0 +1,66 @@ +""" +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 + +from __future__ import print_function + +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 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) + # gm = GaussianMixture.FromConditionals([X(1)], [], [Asia], [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()) + assert self.assertAlmostEqual( + conditionalProbability * mixtureProbability * 0.99, bayesNet.evaluate(values), places=5 + ) + + +if __name__ == "__main__": + unittest.main() From 873f5baf562eb6d789635091593090621d5f5d12 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Dec 2022 08:32:43 +0530 Subject: [PATCH 07/11] remove unnecessary preamble and specializations for hybrid wrapping --- python/gtsam/preamble/hybrid.h | 9 --------- python/gtsam/specializations/hybrid.h | 5 ----- 2 files changed, 14 deletions(-) diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index 7008a1f076..661215e599 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -11,12 +11,3 @@ * mutations on Python side will not be reflected on C++. */ #include - -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif - -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/specializations/hybrid.h b/python/gtsam/specializations/hybrid.h index db5240117e..8b13789179 100644 --- a/python/gtsam/specializations/hybrid.h +++ b/python/gtsam/specializations/hybrid.h @@ -1,6 +1 @@ -py::bind_vector >(m_, "GaussianFactorVector"); -py::implicitly_convertible >(); - -py::bind_vector >(m_, "GaussianConditionalVector"); -py::implicitly_convertible >(); From 03baf8f75ed0aba3c971b2da5ad58460666d000d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Dec 2022 08:33:14 +0530 Subject: [PATCH 08/11] formatting and fixes to test --- python/gtsam/tests/test_HybridBayesNet.py | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index cfe080dcb0..13ac3a3e2d 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -10,8 +10,6 @@ """ # pylint: disable=invalid-name, no-name-in-module, no-member -from __future__ import print_function - import unittest import numpy as np @@ -19,12 +17,12 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import GaussianConditional, GaussianMixture, HybridBayesNet, HybridValues, noiseModel +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) @@ -32,7 +30,8 @@ def test_evaluate(self): # Create the continuous conditional I_1x1 = np.eye(1) - gc = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], 5.0) + gc = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], + 5.0) # Create the noise models model0 = noiseModel.Diagonal.Sigmas([2.0]) @@ -41,7 +40,10 @@ def test_evaluate(self): # Create the conditionals conditional0 = GaussianConditional(X(1), [5], I_1x1, model0) conditional1 = GaussianConditional(X(1), [2], I_1x1, model1) - # gm = GaussianMixture.FromConditionals([X(1)], [], [Asia], [conditional0, conditional1]) # + dkeys = DiscreteKeys() + dkeys.push_back(Asia) + gm = GaussianMixture.FromConditionals([X(1)], [], dkeys, + [conditional0, conditional1]) # # Create hybrid Bayes net. bayesNet = HybridBayesNet() @@ -57,9 +59,10 @@ def test_evaluate(self): conditionalProbability = gc.evaluate(values.continuous()) mixtureProbability = conditional0.evaluate(values.continuous()) - assert self.assertAlmostEqual( - conditionalProbability * mixtureProbability * 0.99, bayesNet.evaluate(values), places=5 - ) + assert self.assertAlmostEqual(conditionalProbability * + mixtureProbability * 0.99, + bayesNet.evaluate(values), + places=5) if __name__ == "__main__": From f4420f2c8dbed17efe506105485a1a5572a67290 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Dec 2022 08:40:46 +0530 Subject: [PATCH 09/11] add mixture to bayesnet and fix double assert bug --- python/gtsam/tests/test_HybridBayesNet.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 13ac3a3e2d..66cddf05e7 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -48,7 +48,7 @@ def test_evaluate(self): # Create hybrid Bayes net. bayesNet = HybridBayesNet() bayesNet.addGaussian(gc) - # bayesNet.addMixture(gm) + bayesNet.addMixture(gm) bayesNet.addDiscrete(Asia, "99/1") # Create values at which to evaluate. @@ -59,10 +59,10 @@ def test_evaluate(self): conditionalProbability = gc.evaluate(values.continuous()) mixtureProbability = conditional0.evaluate(values.continuous()) - assert self.assertAlmostEqual(conditionalProbability * - mixtureProbability * 0.99, - bayesNet.evaluate(values), - places=5) + self.assertAlmostEqual(conditionalProbability * mixtureProbability * + 0.99, + bayesNet.evaluate(values), + places=5) if __name__ == "__main__": From cc2183adb32fc0c10a9fc98895dba782dea603bd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Dec 2022 10:32:46 +0530 Subject: [PATCH 10/11] fix wrap preamble --- python/gtsam/preamble/hybrid.h | 7 +++++++ python/gtsam/specializations/hybrid.h | 1 - 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index 661215e599..90a062d666 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -11,3 +11,10 @@ * mutations on Python side will not be reflected on C++. */ #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 diff --git a/python/gtsam/specializations/hybrid.h b/python/gtsam/specializations/hybrid.h index 8b13789179..e69de29bb2 100644 --- a/python/gtsam/specializations/hybrid.h +++ b/python/gtsam/specializations/hybrid.h @@ -1 +0,0 @@ - From 1eb6fc77a0e22d004c4cf0288d0e3eddd8fed192 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Dec 2022 10:33:33 +0530 Subject: [PATCH 11/11] fix formatting and other issues --- python/gtsam/tests/test_HybridFactorGraph.py | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 576efa82fd..37bb5b93c6 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -10,21 +10,19 @@ """ # 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 @@ def test_create(self): gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( hfg, [C(0)])) - # print("hbn = ", hbn) self.assertEqual(hbn.size(), 2) mixture = hbn.at(0).inner() @@ -56,7 +53,7 @@ def test_create(self): 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 @@ def test_optimize(self): 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()