Add conditional constructors as well

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -33,14 +33,14 @@
 
 namespace gtsam {
 /* *******************************************************************************/
-struct HybridGaussianConditional::ConstructorHelper {
+struct HybridGaussianConditional::Helper {
   std::optional<size_t> nrFrontals;
   FactorValuePairs pairs;
   Conditionals conditionals;
   double minNegLogConstant;
 
   /// Construct from tree of GaussianConditionals.
-  ConstructorHelper(const Conditionals &conditionals)
+  Helper(const Conditionals &conditionals)
       : conditionals(conditionals),
         minNegLogConstant(std::numeric_limits<double>::infinity()) {
     auto func = [this](const GaussianConditional::shared_ptr &c)
@@ -63,14 +63,54 @@ struct HybridGaussianConditional::ConstructorHelper {
   }
 
   /// Construct from means and a sigmas.
-  ConstructorHelper(Key x, const DiscreteKey mode,
+  Helper(const DiscreteKey mode, Key key,
-                    const std::vector<std::pair<Vector, double>> &parameters)
+         const std::vector<std::pair<Vector, double>> &parameters)
       : nrFrontals(1),
         minNegLogConstant(std::numeric_limits<double>::infinity()) {
     std::vector<GaussianConditional::shared_ptr> gcs;
     std::vector<GaussianFactorValuePair> fvs;
     for (const auto &[mean, sigma] : parameters) {
-      auto c = GaussianConditional::sharedMeanAndStddev(x, mean, sigma);
+      auto c = GaussianConditional::sharedMeanAndStddev(key, mean, sigma);
+      double value = c->negLogConstant();
+      minNegLogConstant = std::min(minNegLogConstant, value);
+      gcs.push_back(c);
+      fvs.push_back({c, value});
+    }
+    conditionals = Conditionals({mode}, gcs);
+    pairs = FactorValuePairs({mode}, fvs);
+  }
+
+  /// Construct from means and a sigmas.
+  Helper(const DiscreteKey mode, Key key,  //
+         const Matrix &A, Key parent,
+         const std::vector<std::pair<Vector, double>> &parameters)
+      : nrFrontals(1),
+        minNegLogConstant(std::numeric_limits<double>::infinity()) {
+    std::vector<GaussianConditional::shared_ptr> gcs;
+    std::vector<GaussianFactorValuePair> fvs;
+    for (const auto &[mean, sigma] : parameters) {
+      auto c =
+          GaussianConditional::sharedMeanAndStddev(key, A, parent, mean, sigma);
+      double value = c->negLogConstant();
+      minNegLogConstant = std::min(minNegLogConstant, value);
+      gcs.push_back(c);
+      fvs.push_back({c, value});
+    }
+    conditionals = Conditionals({mode}, gcs);
+    pairs = FactorValuePairs({mode}, fvs);
+  }
+
+  /// Construct from means and a sigmas.
+  Helper(const DiscreteKey mode, Key key,  //
+         const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
+         const std::vector<std::pair<Vector, double>> &parameters)
+      : nrFrontals(1),
+        minNegLogConstant(std::numeric_limits<double>::infinity()) {
+    std::vector<GaussianConditional::shared_ptr> gcs;
+    std::vector<GaussianFactorValuePair> fvs;
+    for (const auto &[mean, sigma] : parameters) {
+      auto c = GaussianConditional::sharedMeanAndStddev(key, A1, parent1, A2,
+                                                        parent2, mean, sigma);
       double value = c->negLogConstant();
       minNegLogConstant = std::min(minNegLogConstant, value);
       gcs.push_back(c);
@@ -83,7 +123,7 @@ struct HybridGaussianConditional::ConstructorHelper {
 
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const DiscreteKeys &discreteParents, const ConstructorHelper &helper)
+    const DiscreteKeys &discreteParents, const Helper &helper)
     : BaseFactor(discreteParents, helper.pairs),
       BaseConditional(*helper.nrFrontals),
       conditionals_(helper.conditionals),
@@ -96,16 +136,30 @@ HybridGaussianConditional::HybridGaussianConditional(
                                 Conditionals({mode}, conditionals)) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
-    Key x, const DiscreteKey mode,
+    const DiscreteKey mode, Key key,  //
     const std::vector<std::pair<Vector, double>> &parameters)
     : HybridGaussianConditional(DiscreteKeys{mode},
-                                ConstructorHelper(x, mode, parameters)) {}
+                                Helper(mode, key, parameters)) {}
+
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKey mode, Key key,  //
+    const Matrix &A, Key parent,
+    const std::vector<std::pair<Vector, double>> &parameters)
+    : HybridGaussianConditional(DiscreteKeys{mode},
+                                Helper(mode, key, A, parent, parameters)) {}
+
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKey mode, Key key,  //
+    const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
+    const std::vector<std::pair<Vector, double>> &parameters)
+    : HybridGaussianConditional(
+          DiscreteKeys{mode},
+          Helper(mode, key, A1, parent1, A2, parent2, parameters)) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKeys &discreteParents,
     const HybridGaussianConditional::Conditionals &conditionals)
-    : HybridGaussianConditional(discreteParents,
+    : HybridGaussianConditional(discreteParents, Helper(conditionals)) {}
-                                ConstructorHelper(conditionals)) {}
 
 /* *******************************************************************************/
 const HybridGaussianConditional::Conditionals &

gtsam/hybrid/HybridGaussianConditional.h

@@ -87,15 +87,21 @@ class GTSAM_EXPORT HybridGaussianConditional
       const DiscreteKey &mode,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
-  /**
+  /// Construct from mean `mu_i` and `sigma_i`.
-   * @brief Construct from vector of means and sigmas.
-   *
-   * @param x The continuous key.
-   * @param mode The discrete key.
-   * @param parameters The means and sigmas for the Gaussian conditionals.
-   */
   HybridGaussianConditional(
-      Key x, const DiscreteKey mode,
+      const DiscreteKey mode, Key key,  //
+      const std::vector<std::pair<Vector, double>> &parameters);
+
+  /// Construct from conditional mean `A1 p1 + b_i` and `sigma_i`.
+  HybridGaussianConditional(
+      const DiscreteKey mode, Key key,  //
+      const Matrix &A, Key parent,
+      const std::vector<std::pair<Vector, double>> &parameters);
+
+  /// Construct from conditional mean `A1 p1 + A2 p2 + b_i` and `sigma_i`.
+  HybridGaussianConditional(
+      const DiscreteKey mode, Key key,  //
+      const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
       const std::vector<std::pair<Vector, double>> &parameters);
 
   /**
@@ -194,11 +200,11 @@ class GTSAM_EXPORT HybridGaussianConditional
 
  private:
   /// Helper struct for private constructor.
-  struct ConstructorHelper;
+  struct Helper;
 
   /// Private constructor that uses helper struct above.
   HybridGaussianConditional(const DiscreteKeys &discreteParents,
-                            const ConstructorHelper &helper);
+                            const Helper &helper);
 
   /// Convert to a DecisionTree of Gaussian factor graphs.
   GaussianFactorGraphTree asGaussianFactorGraphTree() const;

gtsam/hybrid/tests/TinyHybridExample.h

@@ -41,12 +41,12 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
   HybridBayesNet bayesNet;
 
   // Create hybrid Gaussian factor z_i = x0 + noise for each measurement.
+  std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 0.5},
+                                                           {Z_1x1, 3.0}};
   for (size_t i = 0; i < num_measurements; i++) {
     const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
-    std::vector<GaussianConditional::shared_ptr> conditionals{
+    bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, Z(i), I_1x1,
-        GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
+                                                       X(0), measurementModels);
-        GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
-    bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, conditionals);
   }
 
   // Create prior on X(0).

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -82,7 +82,7 @@ TEST(GaussianMixture, GaussianMixtureModel) {
   HybridBayesNet hbn;
   std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma},
                                                     {Vector1(mu1), sigma}};
-  hbn.emplace_shared<HybridGaussianConditional>(Z(0), m, parameters);
+  hbn.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
   hbn.push_back(mixing);
 
   // At the halfway point between the means, we should get P(m|z)=0.5
@@ -120,7 +120,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
   HybridBayesNet hbn;
   std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma0},
                                                     {Vector1(mu1), sigma1}};
-  hbn.emplace_shared<HybridGaussianConditional>(Z(0), m, parameters);
+  hbn.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
   hbn.push_back(mixing);
 
   // We get zMax=3.1333 by finding the maximum value of the function, at which

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -99,7 +99,7 @@ const auto gc = GaussianConditional::sharedMeanAndStddev(X(0), 2 * I_1x1, X(1),
 
 const std::vector<std::pair<Vector, double>> parms{{Vector1(5), 2.0},
                                                    {Vector1(2), 3.0}};
-const auto hgc = std::make_shared<HybridGaussianConditional>(X(1), Asia, parms);
+const auto hgc = std::make_shared<HybridGaussianConditional>(Asia, X(1), parms);
 
 const auto prior = std::make_shared<DiscreteConditional>(Asia, "99/1");
 auto wrap = [](const auto& c) {

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -573,12 +573,10 @@ TEST(HybridEstimation, ModeSelection) {
   bn.push_back(
       GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1));
 
-  std::vector<GaussianConditional::shared_ptr> conditionals{
+  std::vector<std::pair<Vector, double>> parameters{{Z_1x1, noise_loose},
-      GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
+                                                    {Z_1x1, noise_tight}};
-                                               Z_1x1, noise_loose),
+  bn.emplace_shared<HybridGaussianConditional>(mode, Z(0), I_1x1, X(0), -I_1x1,
-      GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
+                                               X(1), parameters);
-                                               Z_1x1, noise_tight)};
-  bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
 
   VectorValues vv;
   vv.insert(Z(0), Z_1x1);
@@ -605,12 +603,10 @@ TEST(HybridEstimation, ModeSelection2) {
   bn.push_back(
       GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1));
 
-  std::vector<GaussianConditional::shared_ptr> conditionals{
+  std::vector<std::pair<Vector, double>> parameters{{Z_3x1, noise_loose},
-      GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
+                                                    {Z_3x1, noise_tight}};
-                                               Z_3x1, noise_loose),
+  bn.emplace_shared<HybridGaussianConditional>(mode, Z(0), I_3x3, X(0), -I_3x3,
-      GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
+                                               X(1), parameters);
-                                               Z_3x1, noise_tight)};
-  bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
 
   VectorValues vv;
   vv.insert(Z(0), Z_3x1);

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -803,11 +803,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
 
   // mode-dependent: 1 is low-noise, 0 is high-noise.
   // Create hybrid Gaussian factor z_0 = x0 + noise for each measurement.
-  std::vector<GaussianConditional::shared_ptr> conditionals{
+  std::vector<std::pair<Vector, double>> parms{{Z_1x1, 3}, {Z_1x1, 0.5}};
-      GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3),
+  bn.emplace_shared<HybridGaussianConditional>(m1, Z(0), I_1x1, X(0), parms);
-      GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)};
-  auto gm = std::make_shared<HybridGaussianConditional>(m1, conditionals);
-  bn.push_back(gm);
 
   // Create prior on X(0).
   bn.push_back(
@@ -912,32 +909,27 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
   // NOTE: we add reverse topological so we can sample from the Bayes net.:
 
   // Add measurements:
+  std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 3},
+                                                           {Z_1x1, 0.5}};
   for (size_t t : {0, 1, 2}) {
     // Create hybrid Gaussian factor on Z(t) conditioned on X(t) and mode N(t):
     const auto noise_mode_t = DiscreteKey{N(t), 2};
-    std::vector<GaussianConditional::shared_ptr> conditionals{
+    bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, Z(t), I_1x1,
-        GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5),
+                                                 X(t), measurementModels);
-        GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1,
-                                                 3.0)};
-    bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, conditionals);
 
     // Create prior on discrete mode N(t):
     bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80");
   }
 
-  // Add motion models:
+  // Add motion models. TODO(frank): why are they exactly the same?
+  std::vector<std::pair<Vector, double>> motionModels{{Z_1x1, 0.2},
+                                                      {Z_1x1, 0.2}};
   for (size_t t : {2, 1}) {
     // Create hybrid Gaussian factor on X(t) conditioned on X(t-1)
     // and mode M(t-1):
     const auto motion_model_t = DiscreteKey{M(t), 2};
-    std::vector<GaussianConditional::shared_ptr> conditionals{
+    bn.emplace_shared<HybridGaussianConditional>(motion_model_t, X(t), I_1x1,
-        GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), Z_1x1,
+                                                 X(t - 1), motionModels);
-                                                 0.2),
-        GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1,
-                                                 0.2)};
-    auto gm = std::make_shared<HybridGaussianConditional>(motion_model_t,
-                                                          conditionals);
-    bn.push_back(gm);
 
     // Create prior on motion model M(t):
     bn.emplace_shared<DiscreteConditional>(motion_model_t, "40/60");

gtsam/hybrid/tests/testHybridMotionModel.cpp

@@ -55,13 +55,10 @@ void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) {
 /// Create hybrid motion model p(x1 | x0, m1)
 static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
     double mu0, double mu1, double sigma0, double sigma1) {
-  auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
+  std::vector<std::pair<Vector, double>> motionModels{{Vector1(mu0), sigma0},
-  auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
+                                                      {Vector1(mu1), sigma1}};
-  auto c0 = make_shared<GaussianConditional>(X(1), Vector1(mu0), I_1x1, X(0),
+  return std::make_shared<HybridGaussianConditional>(m1, X(1), I_1x1, X(0),
-                                             -I_1x1, model0),
+                                                     motionModels);
-       c1 = make_shared<GaussianConditional>(X(1), Vector1(mu1), I_1x1, X(0),
-                                             -I_1x1, model1);
-  return std::make_shared<HybridGaussianConditional>(m1, std::vector{c0, c1});
 }
 
 /// Create two state Bayes network with 1 or two measurement models