diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index f18c19559..2de8e7fc0 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -31,6 +31,9 @@ #include +#include "gtsam/linear/GaussianFactor.h" +#include "gtsam/linear/GaussianFactorGraph.h" + #pragma once namespace gtsam { @@ -44,33 +47,28 @@ using symbol_shorthand::X; * system which depends on a discrete mode at each time step of the chain. * * @param n The number of chain elements. - * @param keyFunc The functional to help specify the continuous key. - * @param dKeyFunc The functional to help specify the discrete key. + * @param x The functional to help specify the continuous key. + * @param m The functional to help specify the discrete key. * @return HybridGaussianFactorGraph::shared_ptr */ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( - size_t n, std::function keyFunc = X, - std::function dKeyFunc = M) { + size_t n, std::function x = X, std::function m = M) { HybridGaussianFactorGraph hfg; - hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); + hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1)); - // keyFunc(1) to keyFunc(n+1) + // x(1) to x(n+1) for (size_t t = 1; t < n; t++) { - DiscreteKeys dKeys{{dKeyFunc(t), 2}}; - HybridGaussianFactor::FactorValuePairs components( - dKeys, {{std::make_shared(keyFunc(t), I_3x3, - keyFunc(t + 1), I_3x3, Z_3x1), - 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)}, dKeys, components)); + DiscreteKeys dKeys{{m(t), 2}}; + std::vector components; + components.emplace_back( + new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Z_3x1)); + components.emplace_back( + new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Vector3::Ones())); + hfg.add(HybridGaussianFactor({x(t), x(t + 1)}, {m(t), 2}, components)); if (t > 1) { - hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, - "0 1 1 3")); + hfg.add(DecisionTreeFactor({{m(t - 1), 2}, {m(t), 2}}, "0 1 1 3")); } } diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index 20719b739..d76eaf08d 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -52,10 +52,10 @@ TEST(HybridFactorGraph, Keys) { // Add a hybrid Gaussian factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - 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)); + std::vector components{ + std::make_shared(X(1), I_3x3, Z_3x1), + std::make_shared(X(1), I_3x3, Vector3::Ones())}; + hfg.add(HybridGaussianFactor({X(1)}, {m1}, components)); KeySet expected_continuous{X(0), X(1)}; EXPECT( @@ -65,9 +65,11 @@ TEST(HybridFactorGraph, Keys) { EXPECT(assert_container_equality(expected_discrete, hfg.discreteKeySet())); } -/* ************************************************************************* */ +/* ************************************************************************* + */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +/* ************************************************************************* + */ diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index bfa283983..34341c999 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -54,32 +54,49 @@ TEST(HybridGaussianFactor, Constructor) { CHECK(it == factor.end()); } +/* ************************************************************************* */ +namespace testA { +DiscreteKey m1(1, 2); + +auto A1 = Matrix::Zero(2, 1); +auto A2 = Matrix::Zero(2, 2); +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); +} // namespace testA + +/* ************************************************************************* */ +// Test simple to complex constructors... +TEST(HybridGaussianFactor, ConstructorVariants) { + using namespace testA; + HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11}); + + std::vector pairs{{f10, 0.0}, {f11, 0.0}}; + HybridGaussianFactor fromPairs({X(1), X(2)}, {m1}, pairs); + assert_equal(fromFactors, fromPairs); + + HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs); + HybridGaussianFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree); + assert_equal(fromDecisionTree, fromPairs); +} + /* ************************************************************************* */ // "Add" two hybrid factors together. TEST(HybridGaussianFactor, Sum) { - DiscreteKey m1(1, 2), m2(2, 3); + using namespace testA; + DiscreteKey m2(2, 3); - auto A1 = Matrix::Zero(2, 1); - auto A2 = Matrix::Zero(2, 2); auto A3 = Matrix::Zero(2, 3); - auto b = Matrix::Zero(2, 1); - Vector2 sigmas; - sigmas << 1, 2; - - auto f10 = std::make_shared(X(1), A1, X(2), A2, b); - auto f11 = std::make_shared(X(1), A1, X(2), A2, b); 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{ - {f20, 0.0}, {f21, 0.0}, {f22, 0.0}}; // TODO(Frank): why specify keys at all? And: keys in factor should be *all* // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? // Design review! - HybridGaussianFactor hybridFactorA({X(1), X(2)}, {m1}, factorsA); - HybridGaussianFactor hybridFactorB({X(1), X(3)}, {m2}, factorsB); + HybridGaussianFactor hybridFactorA({X(1), X(2)}, m1, {f10, f11}); + HybridGaussianFactor hybridFactorB({X(1), X(3)}, m2, {f20, f21, f22}); // Check that number of keys is 3 EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size()); @@ -104,15 +121,8 @@ TEST(HybridGaussianFactor, Sum) { /* ************************************************************************* */ TEST(HybridGaussianFactor, Printing) { - DiscreteKey m1(1, 2); - auto A1 = Matrix::Zero(2, 1); - auto A2 = Matrix::Zero(2, 2); - 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}}; - - HybridGaussianFactor hybridFactor({X(1), X(2)}, {m1}, factors); + using namespace testA; + HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f10, f11}); std::string expected = R"(HybridGaussianFactor @@ -179,9 +189,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}}; - - HybridGaussianFactor hybridFactor({X(1), X(2)}, {m1}, factors); + HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f0, f1}); VectorValues continuousValues; continuousValues.insert(X(1), Vector2(0, 0)); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 1530069aa..36e2887c8 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -114,6 +114,14 @@ TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { EXPECT_LONGS_EQUAL(result.first->size(), 1); EXPECT_LONGS_EQUAL(result.second->size(), 1); } +/* ************************************************************************* */ + +namespace two { +std::vector components(Key key) { + return {std::make_shared(key, I_3x3, Z_3x1), + std::make_shared(key, I_3x3, Vector3::Ones())}; +} +} // namespace two /* ************************************************************************* */ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { @@ -127,10 +135,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a hybrid gaussian factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - 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)); + hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1)))); auto result = hfg.eliminateSequential(); @@ -153,10 +158,7 @@ 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), 0.0}, - {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; - hfg.add(HybridGaussianFactor({X(1)}, {m1}, factors)); + hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1)))); // Discrete probability table for c1 hfg.add(DecisionTreeFactor(m1, {2, 8})); @@ -178,10 +180,7 @@ 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)); - 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(HybridGaussianFactor({X(1)}, {M(1), 2}, two::components(X(1)))); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -207,13 +206,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { // Factor between x0-x1 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), 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)); + hfg.add(HybridGaussianFactor({X(1)}, m, two::components(X(1)))); // Prior factor on c1 hfg.add(DecisionTreeFactor(m, {2, 8})); @@ -241,13 +235,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { 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), 0.0}, - {std::make_shared(X(2), I_3x3, Vector3::Ones()), 0.0}); - - hfg.add(HybridGaussianFactor({X(2)}, {{M(1), 2}}, dt1)); + hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, two::components(X(0)))); + hfg.add(HybridGaussianFactor({X(2)}, {M(1), 2}, two::components(X(2)))); } hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); @@ -256,17 +245,8 @@ 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), 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), 0.0}, - {std::make_shared(X(5), I_3x3, Vector3::Ones()), 0.0}); - - hfg.add(HybridGaussianFactor({X(5)}, {{M(2), 2}}, dt1)); + hfg.add(HybridGaussianFactor({X(3)}, {M(3), 2}, two::components(X(3)))); + hfg.add(HybridGaussianFactor({X(5)}, {M(2), 2}, two::components(X(5)))); } auto ordering_full = @@ -551,12 +531,7 @@ 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), 0.0}, - {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); - - hfg.add(HybridGaussianFactor({X(1)}, {c1}, dt)); + hfg.add(HybridGaussianFactor({X(1)}, c1, two::components(X(1)))); auto result = hfg.eliminateSequential(); @@ -642,13 +617,13 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { // regression EXPECT(assert_equal(expected_error, error_tree, 1e-7)); - auto probs = graph.probPrime(delta.continuous()); + auto probabilities = graph.probPrime(delta.continuous()); std::vector prob_leaves = {0.36793249, 0.61247742, 0.59489556, 0.99029064}; - AlgebraicDecisionTree expected_probs(discrete_keys, prob_leaves); + AlgebraicDecisionTree expected_probabilities(discrete_keys, prob_leaves); // regression - EXPECT(assert_equal(expected_probs, probs, 1e-7)); + EXPECT(assert_equal(expected_probabilities, probabilities, 1e-7)); } /* ****************************************************************************/