diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 4d7490e49..ba0c0bf1a 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -16,20 +16,30 @@ */ #include +#include +#include #include namespace gtsam { /* ************************************************************************* */ -HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) - : Base(other->keys()), inner_(other) {} +HybridGaussianFactor::HybridGaussianFactor( + const boost::shared_ptr &ptr) + : Base(ptr->keys()), inner_(ptr) {} + +HybridGaussianFactor::HybridGaussianFactor( + boost::shared_ptr &&ptr) + : Base(ptr->keys()), inner_(std::move(ptr)) {} -/* ************************************************************************* */ HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner_(boost::make_shared(std::move(jf))) {} +HybridGaussianFactor::HybridGaussianFactor(HessianFactor &&hf) + : Base(hf.keys()), + inner_(boost::make_shared(std::move(hf))) {} + /* ************************************************************************* */ bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { const This *e = dynamic_cast(&other); diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6ca62921c..966524b81 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -19,10 +19,13 @@ #include #include -#include namespace gtsam { +// Forward declarations +class JacobianFactor; +class HessianFactor; + /** * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have * a diamond inheritance i.e. an extra factor type that inherits from both @@ -41,12 +44,41 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { HybridGaussianFactor() = default; - // Explicit conversion from a shared ptr of GF - explicit HybridGaussianFactor(GaussianFactor::shared_ptr other); + /** + * Constructor from shared_ptr of GaussianFactor. + * Example: + * boost::shared_ptr ptr = + * boost::make_shared(...); + * + */ + explicit HybridGaussianFactor(const boost::shared_ptr &ptr); - // Forwarding constructor from concrete JacobianFactor + /** + * Forwarding constructor from shared_ptr of GaussianFactor. + * Examples: + * HybridGaussianFactor factor = boost::make_shared(...); + * HybridGaussianFactor factor(boost::make_shared(...)); + */ + explicit HybridGaussianFactor(boost::shared_ptr &&ptr); + + /** + * Forwarding constructor from rvalue reference of JacobianFactor. + * + * Examples: + * HybridGaussianFactor factor = JacobianFactor(...); + * HybridGaussianFactor factor(JacobianFactor(...)); + */ explicit HybridGaussianFactor(JacobianFactor &&jf); + /** + * Forwarding constructor from rvalue reference of JacobianFactor. + * + * Examples: + * HybridGaussianFactor factor = HessianFactor(...); + * HybridGaussianFactor factor(HessianFactor(...)); + */ + explicit HybridGaussianFactor(HessianFactor &&hf); + public: /// @name Testable /// @{ diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 5d614efff..9d010d2a1 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -418,7 +418,7 @@ void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { } /* ************************************************************************ */ -void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(boost::shared_ptr &factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 6bb1290f6..4e22bed7c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -37,7 +37,6 @@ class HybridEliminationTree; class HybridBayesTree; class HybridJunctionTree; class DecisionTreeFactor; - class JacobianFactor; /** @@ -131,7 +130,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph void add(JacobianFactor&& factor); /// Add a Jacobian factor as a shared ptr. - void add(JacobianFactor::shared_ptr factor); + void add(boost::shared_ptr& factor); /// Add a DecisionTreeFactor to the factor graph. void add(DecisionTreeFactor&& factor); diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index adeee5963..ff896041e 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -118,6 +118,12 @@ class GTSAM_EXPORT HybridValues { */ Vector& at(Key j) { return continuous_.at(j); }; + /** For all key/value pairs in \c values, replace values with corresponding keys in this class + * with those in \c values. Throws std::out_of_range if any keys in \c values are not present + * in this class. */ + void update(const VectorValues& values) { continuous_.update(values); } + + /// @} /// @name Wrapper support /// @{ diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 84f3377de..15687d11b 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -17,6 +17,7 @@ class HybridValues { bool equals(const gtsam::HybridValues& other, double tol) const; void insert(gtsam::Key j, int value); void insert(gtsam::Key j, const gtsam::Vector& value); + void update(const gtsam::VectorValues& values); size_t& atDiscrete(gtsam::Key j); gtsam::Vector& at(gtsam::Key j); }; diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 40016c8bf..481617db1 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -135,6 +135,14 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): self.assertEqual(fg.size(), 3) + @staticmethod + def calculate_ratio(bayesNet, fg, sample): + """Calculate ratio between Bayes net probability and the factor graph.""" + continuous = gtsam.VectorValues() + continuous.insert(X(0), sample.at(X(0))) + return bayesNet.evaluate(sample) / fg.probPrime( + continuous, sample.discrete()) + def test_ratio(self): """ Given a tiny two variable hybrid model, with 2 measurements, @@ -161,18 +169,26 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): fg.push_back(bayesNet.atGaussian(2)) fg.push_back(bayesNet.atDiscrete(3)) + # print(fg) 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) - #TODO(Varun) This should be 1. Adding the normalizing factor should fix fg.probPrime - print(expected_ratio) + expected_ratio = self.calculate_ratio(bayesNet, fg, sample) + # print(f"expected_ratio: {expected_ratio}\n") - # TODO(dellaert): Change the mode to 0 and calculate the ratio again. + # Create measurements from the sample. + measurements = gtsam.VectorValues() + for i in range(2): + measurements.insert(Z(i), sample.at(Z(i))) + + # Check with a number of other samples. + for i in range(10): + other = bayesNet.sample() + other.update(measurements) + # print(other) + # ratio = self.calculate_ratio(bayesNet, fg, other) + # print(f"Ratio: {ratio}\n") + # self.assertAlmostEqual(ratio, expected_ratio) if __name__ == "__main__":