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 a28f9dea0..1a62e7cb8 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -417,7 +417,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 5d8deee80..2f4590d8a 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 80c942a83..90decf769 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 2ebc87971..53ff6354e 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -11,30 +11,20 @@ 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, 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, -) +from gtsam import (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.""" model = noiseModel.Unit.Create(3) @@ -52,8 +42,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): hfg.push_back(gmf) hbn = hfg.eliminateSequential( - Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) - ) + Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( + hfg, [C(0)])) self.assertEqual(hbn.size(), 2) @@ -84,8 +74,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): hfg.push_back(dtf) hbn = hfg.eliminateSequential( - Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) - ) + Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( + hfg, [C(0)])) hv = hbn.optimize() self.assertEqual(hv.atDiscrete(C(0)), 1) @@ -105,15 +95,16 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): 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] - ) + 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) @@ -142,6 +133,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_tiny2(self): """Test a tiny two variable hybrid model, with 2 measurements.""" # Create the Bayes net and sample from it. @@ -160,17 +159,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 - ) - print(expected_ratio) - # TODO(dellaert): Change the mode to 0 and calculate the ratio again. + # Calculate ratio between Bayes net probability and the factor graph: + expected_ratio = self.calculate_ratio(bayesNet, fg, sample) + # print(f"expected_ratio: {expected_ratio}\n") + + # 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__":