diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 851a50698..f799168f2 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -57,12 +57,15 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( // keyFunc(1) to keyFunc(n+1) for (size_t t = 1; t < n; t++) { - hfg.add(HybridGaussianFactor( - {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, + std::vector components = { {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Z_3x1), - std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Vector3::Ones())})); + 0.0}, + {std::make_shared(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)); if (t > 1) { hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 9e64b73da..f7e1a6cd6 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -542,8 +542,10 @@ std::shared_ptr mixedVarianceFactor( double logNormalizationConstant = log(1.0 / noise_tight); double logConstant = -0.5 * d * log2pi + logNormalizationConstant; - auto func = [&](const Assignment& assignment, - const GaussianFactor::shared_ptr& gf) { + auto func = + [&](const Assignment& assignment, + const GaussianFactorValuePair& gfv) -> GaussianFactorValuePair { + auto [gf, val] = gfv; if (assignment.at(mode) != tight_index) { double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose); @@ -555,9 +557,9 @@ std::shared_ptr mixedVarianceFactor( } _gfg.emplace_shared(c); - return std::make_shared(_gfg); + return {std::make_shared(_gfg), 0.0}; } else { - return dynamic_pointer_cast(gf); + return {dynamic_pointer_cast(gf), 0.0}; } }; auto updated_components = gmf->factors().apply(func); diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index bc0ef91d7..8a2e3be3c 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -52,9 +52,9 @@ TEST(HybridFactorGraph, Keys) { // Add a gaussian mixture factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt)); KeySet expected_continuous{X(0), X(1)}; diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index 6156fee96..cfa7e5a5a 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -183,7 +183,7 @@ TEST(HybridGaussianConditional, Likelihood2) { // Check the detailed JacobianFactor calculation for mode==1. { // We have a JacobianFactor - const auto gf1 = (*likelihood)(assignment1); + const auto gf1 = (*likelihood)(assignment1).first; const auto jf1 = std::dynamic_pointer_cast(gf1); CHECK(jf1); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 7e3e2defa..6e41792aa 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -71,9 +71,8 @@ TEST(HybridGaussianFactor, Sum) { auto f20 = std::make_shared(X(1), A1, X(3), A3, b); auto f21 = std::make_shared(X(1), A1, X(3), A3, b); auto f22 = std::make_shared(X(1), A1, X(3), A3, b); - std::vector> factorsA{ - {f10, 0.0}, {f11, 0.0}}; - std::vector> factorsB{ + std::vector factorsA{{f10, 0.0}, {f11, 0.0}}; + std::vector factorsB{ {f20, 0.0}, {f21, 0.0}, {f22, 0.0}}; // TODO(Frank): why specify keys at all? And: keys in factor should be *all* @@ -111,8 +110,7 @@ TEST(HybridGaussianFactor, Printing) { auto b = Matrix::Zero(2, 1); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); - std::vector> factors{ - {f10, 0.0}, {f11, 0.0}}; + std::vector factors{{f10, 0.0}, {f11, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -183,8 +181,7 @@ TEST(HybridGaussianFactor, Error) { auto f0 = std::make_shared(X(1), A01, X(2), A02, b); auto f1 = std::make_shared(X(1), A11, X(2), A12, b); - std::vector> factors{{f0, 0.0}, - {f1, 0.0}}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index acd7b280f..69bc0dd58 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -127,9 +127,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a gaussian mixture factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt)); auto result = hfg.eliminateSequential(); @@ -153,9 +153,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { // Add factor between x0 and x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - std::vector factors = { - std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())}; + std::vector factors = { + {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; hfg.add(HybridGaussianFactor({X(1)}, {m1}, factors)); // Discrete probability table for c1 @@ -178,10 +178,10 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor( - {X(1)}, {{M(1), 2}}, - {std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())})); + std::vector factors = { + {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; + hfg.add(HybridGaussianFactor({X(1)}, {{M(1), 2}}, factors)); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -208,9 +208,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Decision tree with different modes on x1 - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); // Hybrid factor P(x1|c1) hfg.add(HybridGaussianFactor({X(1)}, {m}, dt)); @@ -238,14 +238,14 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); { - hfg.add(HybridGaussianFactor( - {X(0)}, {{M(0), 2}}, - {std::make_shared(X(0), I_3x3, Z_3x1), - std::make_shared(X(0), I_3x3, Vector3::Ones())})); + std::vector factors = { + {std::make_shared(X(0), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(0), I_3x3, Vector3::Ones()), 0.0}}; + hfg.add(HybridGaussianFactor({X(0)}, {{M(0), 2}}, factors)); - DecisionTree dt1( - M(1), std::make_shared(X(2), I_3x3, Z_3x1), - std::make_shared(X(2), I_3x3, Vector3::Ones())); + DecisionTree dt1( + M(1), {std::make_shared(X(2), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(2), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(2)}, {{M(1), 2}}, dt1)); } @@ -256,15 +256,15 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); { - DecisionTree dt( - M(3), std::make_shared(X(3), I_3x3, Z_3x1), - std::make_shared(X(3), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(3), {std::make_shared(X(3), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(3), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(3)}, {{M(3), 2}}, dt)); - DecisionTree dt1( - M(2), std::make_shared(X(5), I_3x3, Z_3x1), - std::make_shared(X(5), I_3x3, Vector3::Ones())); + DecisionTree dt1( + M(2), {std::make_shared(X(5), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(5), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(5)}, {{M(2), 2}}, dt1)); } @@ -552,9 +552,9 @@ TEST(HybridGaussianFactorGraph, optimize) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - DecisionTree dt( - C(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + C(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {c1}, dt)); @@ -734,8 +734,8 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { // Expected decision tree with two factor graphs: // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0) GaussianFactorGraphTree expected{ - M(0), GaussianFactorGraph(std::vector{(*mixture)(d0), prior}), - GaussianFactorGraph(std::vector{(*mixture)(d1), prior})}; + M(0), GaussianFactorGraph(std::vector{(*mixture)(d0).first, prior}), + GaussianFactorGraph(std::vector{(*mixture)(d1).first, prior})}; EXPECT(assert_equal(expected(d0), actual(d0), 1e-5)); EXPECT(assert_equal(expected(d1), actual(d1), 1e-5)); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 947c24677..4f1a6fa46 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -526,6 +526,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf : A[x0] = [ @@ -536,6 +537,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -0 ] No noise model +value: 0 } factor 2: @@ -551,6 +553,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf : A[x1] = [ @@ -561,6 +564,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -0 ] No noise model +value: 0 } factor 3: @@ -609,6 +613,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf: A[x0] = [ @@ -619,6 +624,7 @@ Hybrid [x0 x1; m0]{ ] b = [ -0 ] No noise model +value: 0 } factor 2: @@ -633,6 +639,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model +value: 0 1 Leaf: A[x1] = [ @@ -643,6 +650,7 @@ Hybrid [x1 x2; m1]{ ] b = [ -0 ] No noise model +value: 0 } factor 3: diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 365dfcc9f..74cfa72e6 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -83,7 +83,7 @@ TEST(HybridSerialization, HybridGaussianFactor) { auto b1 = Matrix::Ones(2, 1); auto f0 = std::make_shared(X(0), A, b0); auto f1 = std::make_shared(X(0), A, b1); - std::vector factors{f0, f1}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors);