From b99039ed011dda8ea87a973d3061cfb16c5669c0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 25 Feb 2022 15:28:24 -0500 Subject: [PATCH 1/6] GaussianMixture printing improvement --- gtsam/hybrid/GaussianMixture.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 0431ba584d..f5867503be 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -65,19 +65,19 @@ GaussianConditional::shared_ptr GaussianMixture::operator()( void GaussianMixture::print(const std::string &s, const KeyFormatter &keyFormatter) const { std::cout << (s.empty() ? "" : s + " "); - std::cout << "GaussianMixture ["; + std::cout << "GaussianMixture [ "; for (Key key : frontals()) std::cout << keyFormatter(key) << " "; - std::cout << "| "; + if (parents().size()) std::cout << "| "; for (Key key : parents()) std::cout << keyFormatter(key) << " "; std::cout << "]"; std::cout << "{\n"; - auto valueFormatter = [](const GaussianFactor::shared_ptr &v) { - auto printCapture = [](const GaussianFactor::shared_ptr &p) { + auto valueFormatter = [&](const GaussianFactor::shared_ptr &v) { + auto printCapture = [&](const GaussianFactor::shared_ptr &p) { RedirectCout rd; - p->print(); + p->print("", keyFormatter); std::string s = rd.str(); return s; }; From 20f946d97102a50c30a5ca5baf758ad0577db493 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 25 Feb 2022 15:29:10 -0500 Subject: [PATCH 2/6] fix loop in IncrementalHybrid --- gtsam/hybrid/IncrementalHybrid.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/IncrementalHybrid.cpp b/gtsam/hybrid/IncrementalHybrid.cpp index 28eb9659a0..bdad7c2e13 100644 --- a/gtsam/hybrid/IncrementalHybrid.cpp +++ b/gtsam/hybrid/IncrementalHybrid.cpp @@ -33,7 +33,11 @@ void IncrementalHybrid::update(GaussianHybridFactorGraph graph, // We add all relevant conditional mixtures on the last continuous variable // in the previous `hybridBayesNet` to the graph std::unordered_set allVars(ordering.begin(), ordering.end()); - for (auto &&conditional : hybridBayesNet_) { + + // TODO(Varun) Using a for-range loop doesn't work since some of the + // conditionals are invalid pointers + for (size_t i = 0; i < hybridBayesNet_.size(); i++) { + auto conditional = hybridBayesNet_.at(i); // Flag indicating if a conditional will be updated due to factors in // `graph` bool marked_for_update = false; @@ -53,7 +57,8 @@ void IncrementalHybrid::update(GaussianHybridFactorGraph graph, // If a conditional is due to be updated, we remove if from the // previous bayes net. if (marked_for_update) { - auto it = find(hybridBayesNet_.begin(), hybridBayesNet_.end(), conditional); + auto it = find(hybridBayesNet_.begin(), hybridBayesNet_.end(), + conditional); hybridBayesNet_.erase(it); } break; From c483f98ec0e7f2bf90d222723835a720bcd289a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 25 Feb 2022 15:29:27 -0500 Subject: [PATCH 3/6] more wrapping --- gtsam/hybrid/hybrid.i | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 1968ee9121..5abd86f00c 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -48,6 +48,16 @@ virtual class GaussianMixture : gtsam::DCGaussianMixtureFactor { class DCFactorGraph { DCFactorGraph(); gtsam::DiscreteKeys discreteKeys() const; + + size_t size() const; + bool empty() const; + void remove(size_t i); + void resize(size_t size); + size_t nrFactors() const; + + void print(const std::string& str = "DCFactorGraph", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; }; #include @@ -84,6 +94,11 @@ class NonlinearHybridFactorGraph { const gtsam::NonlinearFactorGraph& nonlinearGraph() const; + //TODO(Varun) issues with templated inheritance + const gtsam::DiscreteFactorGraph& discreteGraph() const; + const gtsam::DCFactorGraph& dcGraph() const; + gtsam::DiscreteKeys discreteKeys() const; + gtsam::GaussianHybridFactorGraph linearize( const gtsam::Values& continuousValues) const; @@ -113,13 +128,27 @@ class GaussianHybridFactorGraph { void print(const std::string& str = "GaussianHybridFactorGraph", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + //TODO(Varun) issues with templated inheritance + const gtsam::DiscreteFactorGraph& discreteGraph() const; + const gtsam::DCFactorGraph& dcGraph() const; + gtsam::DiscreteKeys discreteKeys() const; + }; #include class IncrementalHybrid { + IncrementalHybrid(); + void update(gtsam::GaussianHybridFactorGraph graph, - const gtsam::Ordering& ordering); + const gtsam::Ordering& ordering, + boost::optional maxNrLeaves = nullptr); + + GaussianMixture* gaussianMixture(size_t index) const; + const DiscreteFactorGraph& remainingDiscreteGraph() const; + const HybridBayesNet& hybridBayesNet() const; + const GaussianHybridFactorGraph& remainingFactorGraph() const; }; } // namespace gtsam From d3e0401834419f33c15d908c1eee88bcd1ba1d6f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 25 Feb 2022 15:29:55 -0500 Subject: [PATCH 4/6] add optional support in wrapping for hybrid --- python/gtsam/preamble/hybrid.h | 9 +++++++++ python/gtsam/tests/test_Hybrid.py | 8 +++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index d07a75f6fb..a1346da3a4 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -10,3 +10,12 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +#include + +// Support for binding boost::optional types in C++11. +// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html +namespace pybind11 { namespace detail { + template + struct type_caster> : optional_caster> {}; +}} diff --git a/python/gtsam/tests/test_Hybrid.py b/python/gtsam/tests/test_Hybrid.py index 354b778d49..9df0f7aa8b 100644 --- a/python/gtsam/tests/test_Hybrid.py +++ b/python/gtsam/tests/test_Hybrid.py @@ -1,6 +1,6 @@ import gtsam import numpy as np -from gtsam import GaussianHybridFactorGraph +from gtsam import GaussianHybridFactorGraph, IncrementalHybrid from gtsam.utils.test_case import GtsamTestCase @@ -11,3 +11,9 @@ def setUp(self) -> None: def test_elimination(self): # Check if constructed correctly self.assertIsInstance(self.ghfg, GaussianHybridFactorGraph) + + def test_incremental(self): + ordering = gtsam.Ordering() + inc = IncrementalHybrid() + inc.update(self.ghfg, ordering) + # inc.update(self.ghfg, ordering, 4) From da17d11c3711a1f8c4a4881cde9d5cee6bdb9ee6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 25 Feb 2022 15:30:06 -0500 Subject: [PATCH 5/6] Add todo for AbstractConditional --- gtsam/inference/AbstractConditional.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/inference/AbstractConditional.h b/gtsam/inference/AbstractConditional.h index 6099db0e9d..247863057d 100644 --- a/gtsam/inference/AbstractConditional.h +++ b/gtsam/inference/AbstractConditional.h @@ -83,6 +83,8 @@ class GTSAM_EXPORT AbstractConditional { virtual Parents parents() const = 0; /// @} + + //TODO(Varun) add iterators so we can call hybridBayesNet.keyVector() }; /// traits From 8619c7c2c161c717f113a495ed3f14c32bc778b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 25 Feb 2022 20:38:10 -0500 Subject: [PATCH 6/6] fix test --- gtsam/hybrid/tests/testHybridFactorGraph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index 47c1c172ab..357babe55f 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -553,7 +553,7 @@ TEST(HybridFactorGraph, Printing) { // Expected output for hybridBayesNet. string expected_hybridBayesNet = R"( size: 3 -factor 0: GaussianMixture [x1 | x2 m1 ]{ +factor 0: GaussianMixture [ x1 | x2 m1 ]{ Choice(m1) 0 Leaf Jacobian factor on 2 keys: Conditional density [x1] @@ -572,7 +572,7 @@ factor 0: GaussianMixture [x1 | x2 m1 ]{ } -factor 1: GaussianMixture [x2 | x3 m2 m1 ]{ +factor 1: GaussianMixture [ x2 | x3 m2 m1 ]{ Choice(m2) 0 Choice(m1) 0 0 Leaf Jacobian factor on 2 keys: @@ -609,7 +609,7 @@ factor 1: GaussianMixture [x2 | x3 m2 m1 ]{ } -factor 2: GaussianMixture [x3 | m2 m1 ]{ +factor 2: GaussianMixture [ x3 | m2 m1 ]{ Choice(m2) 0 Choice(m1) 0 0 Leaf Jacobian factor on 1 keys: