diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 8fce251cc..a8ed7eed1 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -55,6 +55,15 @@ HybridGaussianConditional::conditionals() const { return conditionals_; } +/* *******************************************************************************/ +HybridGaussianConditional::HybridGaussianConditional( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const std::vector &conditionals) + : HybridGaussianConditional(continuousFrontals, continuousParents, + discreteParents, + Conditionals(discreteParents, conditionals)) {} + /* *******************************************************************************/ // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be // derived from HybridGaussianFactor, no? diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 2c132d3bf..9cd13cbec 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -106,6 +106,20 @@ class GTSAM_EXPORT HybridGaussianConditional const DiscreteKeys &discreteParents, const Conditionals &conditionals); + /** + * @brief Make a Gaussian Mixture from a vector of Gaussian conditionals. + * The DecisionTree-based constructor is preferred over this one. + * + * @param continuousFrontals The continuous frontal variables + * @param continuousParents The continuous parent variables + * @param discreteParents Discrete parents variables + * @param conditionals Vector of conditionals + */ + HybridGaussianConditional( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const std::vector &conditionals); + /// @} /// @name Testable /// @{ @@ -247,7 +261,7 @@ class GTSAM_EXPORT HybridGaussianConditional #endif }; -/// Return the DiscreteKeys vector as a set. +/// Return the DiscreteKey vector as a set. std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys); // traits diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index dcf0fce04..485d10848 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -76,7 +76,7 @@ virtual class HybridConditional { class HybridGaussianFactor : gtsam::HybridFactor { HybridGaussianFactor( const gtsam::KeyVector& continuousKeys, - const gtsam::DiscreteKeys& discreteKeys, + const gtsam::DiscreteKey& discreteKey, const std::vector>& factorsList); @@ -91,8 +91,12 @@ class HybridGaussianConditional : gtsam::HybridFactor { const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, - const std::vector& - conditionalsList); + const gtsam::HybridGaussianConditional::Conditionals& conditionals); + HybridGaussianConditional( + const gtsam::KeyVector& continuousFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKeys& discreteParents, + const std::vector& conditionals); gtsam::HybridGaussianFactor* likelihood( const gtsam::VectorValues& frontals) const; @@ -248,7 +252,7 @@ class HybridNonlinearFactor : gtsam::HybridFactor { bool normalized = false); HybridNonlinearFactor( - const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, const std::vector>& factors, bool normalized = false); diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index adbda59ce..ce71d75ed 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,7 +20,7 @@ import gtsam from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, HybridBayesNet, HybridGaussianConditional, HybridGaussianFactor, HybridGaussianFactorGraph, - HybridValues, JacobianFactor, Ordering, noiseModel) + HybridValues, JacobianFactor, noiseModel) DEBUG_MARGINALS = False @@ -31,13 +31,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): def test_create(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) - dk = DiscreteKeys() - dk.push_back((C(0), 2)) jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) + gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -58,13 +56,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): def test_optimize(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) - dk = DiscreteKeys() - dk.push_back((C(0), 2)) jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) + gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -96,8 +92,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): # Create Gaussian mixture Z(0) = X(0) + noise for each measurement. I_1x1 = np.eye(1) - keys = DiscreteKeys() - keys.push_back(mode) for i in range(num_measurements): conditional0 = GaussianConditional.FromMeanAndStddev(Z(i), I_1x1, @@ -107,8 +101,10 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): I_1x1, X(0), [0], sigma=3) + discreteParents = DiscreteKeys() + discreteParents.push_back(mode) bayesNet.push_back( - HybridGaussianConditional([Z(i)], [X(0)], keys, + HybridGaussianConditional([Z(i)], [X(0)], discreteParents, [conditional0, conditional1])) # Create prior on X(0). diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index 247f83c19..4a1abdcf3 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -27,8 +27,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): def test_nonlinear_hybrid(self): nlfg = gtsam.HybridNonlinearFactorGraph() - dk = gtsam.DiscreteKeys() - dk.push_back((10, 2)) nlfg.push_back( BetweenFactorPoint3(1, 2, Point3(1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1]))) @@ -40,7 +38,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): noiseModel.Unit.Create(3)), 0.0), (PriorFactorPoint3(1, Point3(1, 2, 1), noiseModel.Unit.Create(3)), 0.0)] - nlfg.push_back(gtsam.HybridNonlinearFactor([1], dk, factors)) + nlfg.push_back(gtsam.HybridNonlinearFactor([1], (10, 2), factors)) nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3")) values = gtsam.Values() values.insert_point3(1, Point3(0, 0, 0))