Merge branch 'hybrid-error-scalars' into hybrid-enum

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<GaussianConditional::shared_ptr> &conditionals)
+    : HybridGaussianConditional(continuousFrontals, continuousParents,
+                                discreteParents,
+                                Conditionals(discreteParents, conditionals)) {}
+
 /* *******************************************************************************/
 // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be
 // derived from HybridGaussianFactor, no?

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<GaussianConditional::shared_ptr> &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<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys);
 
 // traits

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<std::pair<gtsam::GaussianFactor::shared_ptr, double>>&
           factorsList);
 
@@ -91,8 +91,12 @@ class HybridGaussianConditional : gtsam::HybridFactor {
       const gtsam::KeyVector& continuousFrontals,
       const gtsam::KeyVector& continuousParents,
       const gtsam::DiscreteKeys& discreteParents,
-      const std::vector<gtsam::GaussianConditional::shared_ptr>&
+      const gtsam::HybridGaussianConditional::Conditionals& conditionals);
-          conditionalsList);
+  HybridGaussianConditional(
+      const gtsam::KeyVector& continuousFrontals,
+      const gtsam::KeyVector& continuousParents,
+      const gtsam::DiscreteKeys& discreteParents,
+      const std::vector<gtsam::GaussianConditional::shared_ptr>& 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<std::pair<gtsam::NonlinearFactor*, double>>& factors,
       bool normalized = false);
 

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).

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))