tree based constructors

borglab · Sep 16, 2024 · 64d9fc6 · 64d9fc6
1 parent 1c74da2
commit 64d9fc6
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 14 deletions.
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
@@ -57,15 +57,16 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
 
   // keyFunc(1) to keyFunc(n+1)
   for (size_t t = 1; t < n; t++) {
-    std::vector<GaussianFactorValuePair> components = {
-        {std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
-                                          I_3x3, Z_3x1),
-         0.0},
-        {std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
-                                          I_3x3, Vector3::Ones()),
-         0.0}};
-    hfg.add(HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)},
-                                 {{dKeyFunc(t), 2}}, components));
+    DiscreteKeys dKeys{{dKeyFunc(t), 2}};
+    HybridGaussianFactor::FactorValuePairs components(
+        dKeys, {{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3,
+                                                  keyFunc(t + 1), I_3x3, Z_3x1),
+                 0.0},
+                {std::make_shared<JacobianFactor>(
+                     keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()),
+                 0.0}});
+    hfg.add(
+        HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components));
 
     if (t > 1) {
       hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}},

diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -383,15 +383,17 @@ TEST(HybridBayesNet, Sampling) {
   HybridNonlinearFactorGraph nfg;
 
   auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0));
+  nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
+
   auto zero_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  std::vector<NonlinearFactorValuePair> factors = {{zero_motion, 0.0},
-                                                   {one_motion, 0.0}};
-  nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
-  nfg.emplace_shared<HybridNonlinearFactor>(
-      KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);
+  DiscreteKeys discreteKeys{DiscreteKey(M(0), 2)};
+  HybridNonlinearFactor::Factors factors(
+      discreteKeys, {{zero_motion, 0.0}, {one_motion, 0.0}});
+  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, discreteKeys,
+                                            factors);
 
   DiscreteKey mode(M(0), 2);
   nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");