diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index fdf9092b6c..49ced968ed 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -120,21 +120,13 @@ using MotionModel = BetweenFactor; // Test fixture with switching network. /// ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2)) struct Switching { - private: - HybridNonlinearFactorGraph nonlinearFactorGraph_; - public: size_t K; DiscreteKeys modes; HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain; - HybridGaussianFactorGraph linearizedFactorGraph; + HybridGaussianFactorGraph linearUnaryFactors, linearBinaryFactors; Values linearizationPoint; - // Access the flat nonlinear factor graph. - const HybridNonlinearFactorGraph &nonlinearFactorGraph() const { - return nonlinearFactorGraph_; - } - /** * @brief Create with given number of time steps. * @@ -164,36 +156,33 @@ struct Switching { // Create hybrid factor graph. // Add a prior ϕ(X(0)) on X(0). - nonlinearFactorGraph_.emplace_shared>( + unaryFactors.emplace_shared>( X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma)); - unaryFactors.push_back(nonlinearFactorGraph_.back()); // Add "motion models" ϕ(X(k),X(k+1),M(k)). for (size_t k = 0; k < K - 1; k++) { auto motion_models = motionModels(k, between_sigma); - nonlinearFactorGraph_.emplace_shared(modes[k], - motion_models); - binaryFactors.push_back(nonlinearFactorGraph_.back()); + binaryFactors.emplace_shared(modes[k], + motion_models); } // Add measurement factors ϕ(X(k);z_k). auto measurement_noise = Isotropic::Sigma(1, prior_sigma); for (size_t k = 1; k < K; k++) { - nonlinearFactorGraph_.emplace_shared>( - X(k), measurements.at(k), measurement_noise); - unaryFactors.push_back(nonlinearFactorGraph_.back()); + unaryFactors.emplace_shared>(X(k), measurements.at(k), + measurement_noise); } // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2)) modeChain = createModeChain(transitionProbabilityTable); - nonlinearFactorGraph_ += modeChain; // Create the linearization point. for (size_t k = 0; k < K; k++) { linearizationPoint.insert(X(k), static_cast(k + 1)); } - linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint); + linearUnaryFactors = *unaryFactors.linearize(linearizationPoint); + linearBinaryFactors = *binaryFactors.linearize(linearizationPoint); } // Create motion models for a given time step @@ -224,6 +213,24 @@ struct Switching { } return chain; } + + /// Get the full linear factor graph. + HybridGaussianFactorGraph linearizedFactorGraph() const { + HybridGaussianFactorGraph graph; + graph.push_back(linearUnaryFactors); + graph.push_back(linearBinaryFactors); + graph.push_back(modeChain); + return graph; + } + + /// Get all the nonlinear factors. + HybridNonlinearFactorGraph nonlinearFactorGraph() const { + HybridNonlinearFactorGraph graph; + graph.push_back(unaryFactors); + graph.push_back(binaryFactors); + graph.push_back(modeChain); + return graph; + } }; } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index ad4e4e7a8d..16d0ae1a12 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -31,6 +31,7 @@ // Include for test suite #include + #include using namespace std; @@ -263,7 +264,7 @@ TEST(HybridBayesNet, Choose) { const Ordering ordering(s.linearizationPoint.keys()); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteValues assignment; assignment[M(0)] = 1; @@ -292,7 +293,7 @@ TEST(HybridBayesNet, OptimizeAssignment) { const Ordering ordering(s.linearizationPoint.keys()); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteValues assignment; assignment[M(0)] = 1; @@ -319,7 +320,7 @@ TEST(HybridBayesNet, Optimize) { Switching s(4, 1.0, 0.1, {0, 1, 2, 3}, "1/1 1/1"); HybridBayesNet::shared_ptr hybridBayesNet = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); HybridValues delta = hybridBayesNet->optimize(); @@ -347,7 +348,7 @@ TEST(HybridBayesNet, Pruning) { Switching s(3); HybridBayesNet::shared_ptr posterior = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); EXPECT_LONGS_EQUAL(5, posterior->size()); // Optimize @@ -400,7 +401,7 @@ TEST(HybridBayesNet, Prune) { Switching s(4); HybridBayesNet::shared_ptr posterior = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); EXPECT_LONGS_EQUAL(7, posterior->size()); HybridValues delta = posterior->optimize(); @@ -418,7 +419,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) { Switching s(4); HybridBayesNet::shared_ptr posterior = - s.linearizedFactorGraph.eliminateSequential(); + s.linearizedFactorGraph().eliminateSequential(); EXPECT_LONGS_EQUAL(7, posterior->size()); DiscreteConditional joint; diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 6da991173a..9e886200a1 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -352,7 +352,7 @@ TEST(HybridBayesTree, OptimizeMultifrontal) { Switching s(4); HybridBayesTree::shared_ptr hybridBayesTree = - s.linearizedFactorGraph.eliminateMultifrontal(); + s.linearizedFactorGraph().eliminateMultifrontal(); HybridValues delta = hybridBayesTree->optimize(); VectorValues expectedValues; @@ -364,30 +364,40 @@ TEST(HybridBayesTree, OptimizeMultifrontal) { EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } +namespace optimize_fixture { +HybridGaussianFactorGraph GetGaussianFactorGraph(size_t N) { + Switching s(N); + HybridGaussianFactorGraph graph; + + for (size_t i = 0; i < N - 1; i++) { + graph.push_back(s.linearBinaryFactors.at(i)); + } + for (size_t i = 0; i < N; i++) { + graph.push_back(s.linearUnaryFactors.at(i)); + } + for (size_t i = 0; i < N - 1; i++) { + graph.push_back(s.modeChain.at(i)); + } + + return graph; +} +} // namespace optimize_fixture + /* ****************************************************************************/ // Test for optimizing a HybridBayesTree with a given assignment. TEST(HybridBayesTree, OptimizeAssignment) { - Switching s(4); + using namespace optimize_fixture; - HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; - - // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + size_t N = 4; - // Add the Gaussian factors, 1 prior on X(1), - // 3 measurements on X(2), X(3), X(4) - graph1.push_back(s.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 7; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + HybridGaussianISAM isam; - // Add the discrete factors - for (size_t i = 7; i <= 9; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 + // Then add the Gaussian factors, 1 prior on X(0), + // 3 measurements on X(1), X(2), X(3) + // Finally add the discrete factors + // m0, m1-m0, m2-m1 + HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N); isam.update(graph1); @@ -409,12 +419,13 @@ TEST(HybridBayesTree, OptimizeAssignment) { EXPECT(assert_equal(expected_delta, delta)); + Switching s(N); // Create ordering. Ordering ordering; for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); GaussianBayesNet gbn = hybridBayesNet->choose(assignment); VectorValues expected = gbn.optimize(); @@ -425,38 +436,29 @@ TEST(HybridBayesTree, OptimizeAssignment) { /* ****************************************************************************/ // Test for optimizing a HybridBayesTree. TEST(HybridBayesTree, Optimize) { - Switching s(4); + using namespace optimize_fixture; - HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; - - // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + size_t N = 4; - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(2), X(3), X(4) - graph1.push_back(s.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 6; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } - - // Add the discrete factors - for (size_t i = 7; i <= 9; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + HybridGaussianISAM isam; + // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 + // Then add the Gaussian factors, 1 prior on X(0), + // 3 measurements on X(1), X(2), X(3) + // Finally add the discrete factors + // m0, m1-m0, m2-m1 + HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N); isam.update(graph1); HybridValues delta = isam.optimize(); + Switching s(N); // Create ordering. Ordering ordering; for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); const auto [hybridBayesNet, remainingFactorGraph] = - s.linearizedFactorGraph.eliminatePartialSequential(ordering); + s.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteFactorGraph dfg; for (auto&& f : *remainingFactorGraph) { @@ -481,27 +483,18 @@ TEST(HybridBayesTree, Optimize) { /* ****************************************************************************/ // Test for choosing a GaussianBayesTree from a HybridBayesTree. TEST(HybridBayesTree, Choose) { - Switching s(4); - - HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; + using namespace optimize_fixture; - // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + size_t N = 4; - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(2), X(3), X(4) - graph1.push_back(s.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 6; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + HybridGaussianISAM isam; - // Add the discrete factors - for (size_t i = 7; i <= 9; i++) { - graph1.push_back(s.linearizedFactorGraph.at(i)); - } + // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 + // Then add the Gaussian factors, 1 prior on X(0), + // 3 measurements on X(1), X(2), X(3) + // Finally add the discrete factors + // m0, m1-m0, m2-m1 + HybridGaussianFactorGraph graph1 = GetGaussianFactorGraph(N); isam.update(graph1); @@ -513,8 +506,9 @@ TEST(HybridBayesTree, Choose) { GaussianBayesTree gbt = isam.choose(assignment); // Specify ordering so it matches that of HybridGaussianISAM. + Switching s(N); Ordering ordering(KeyVector{X(0), X(1), X(2), X(3), M(0), M(1), M(2)}); - auto bayesTree = s.linearizedFactorGraph.eliminateMultifrontal(ordering); + auto bayesTree = s.linearizedFactorGraph().eliminateMultifrontal(ordering); auto expected_gbt = bayesTree->choose(assignment); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 9c2580a17a..5b27e2b417 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -82,7 +82,7 @@ TEST(HybridEstimation, Full) { // Switching example of robot moving in 1D // with given measurements and equal mode priors. Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1"); - HybridGaussianFactorGraph graph = switching.linearizedFactorGraph; + HybridGaussianFactorGraph graph = switching.linearizedFactorGraph(); Ordering hybridOrdering; for (size_t k = 0; k < K; k++) { @@ -325,7 +325,7 @@ TEST(HybridEstimation, Probability) { // given measurements and equal mode priors. Switching switching(K, between_sigma, measurement_sigma, measurements, "1/1 1/1"); - auto graph = switching.linearizedFactorGraph; + auto graph = switching.linearizedFactorGraph(); // Continuous elimination Ordering continuous_ordering(graph.continuousKeySet()); @@ -365,7 +365,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { // mode priors. Switching switching(K, between_sigma, measurement_sigma, measurements, "1/1 1/1"); - auto graph = switching.linearizedFactorGraph; + auto graph = switching.linearizedFactorGraph(); // Get the tree of unnormalized probabilities for each mode sequence. AlgebraicDecisionTree expected_probPrimeTree = GetProbPrimeTree(graph); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 4b91d091d8..13a6cd614f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -175,7 +175,7 @@ TEST(HybridBayesNet, Switching) { Switching s(2, betweenSigma, priorSigma); // Check size of linearized factor graph - const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; + const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph(); EXPECT_LONGS_EQUAL(4, graph.size()); // Create some continuous and discrete values @@ -184,7 +184,7 @@ TEST(HybridBayesNet, Switching) { const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}}; // Get the hybrid gaussian factor and check it is as expected - auto hgf = std::dynamic_pointer_cast(graph.at(1)); + auto hgf = std::dynamic_pointer_cast(graph.at(2)); CHECK(hgf); // Get factors and scalars for both modes @@ -298,7 +298,7 @@ TEST(HybridBayesNet, Switching) { factors_x1.push_back( factor); // Use the remaining factor from previous elimination factors_x1.push_back( - graph.at(2)); // Add the factor for x1 from the original graph + graph.at(1)); // Add the factor for x1 from the original graph // Test collectProductFactor for x1 clique auto productFactor_x1 = factors_x1.collectProductFactor(); @@ -356,7 +356,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) { Switching s(3); // Check size of linearized factor graph - const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; + const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph(); EXPECT_LONGS_EQUAL(7, graph.size()); // Eliminate the graph @@ -383,16 +383,16 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) { TEST(HybridGaussianFactorGraph, DiscreteSelection) { Switching s(3); - HybridGaussianFactorGraph graph = s.linearizedFactorGraph; + HybridGaussianFactorGraph graph = s.linearizedFactorGraph(); DiscreteValues dv00{{M(0), 0}, {M(1), 0}}; GaussianFactorGraph continuous_00 = graph(dv00); GaussianFactorGraph expected_00; expected_00.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); - expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); expected_00.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_00.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_00.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); + expected_00.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); EXPECT(assert_equal(expected_00, continuous_00)); @@ -400,10 +400,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) { GaussianFactorGraph continuous_01 = graph(dv01); GaussianFactorGraph expected_01; expected_01.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); - expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); expected_01.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_01.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_01.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-1))); + expected_01.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); EXPECT(assert_equal(expected_01, continuous_01)); @@ -411,10 +411,10 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) { GaussianFactorGraph continuous_10 = graph(dv10); GaussianFactorGraph expected_10; expected_10.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); - expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); expected_10.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_10.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_10.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); + expected_10.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-1))); EXPECT(assert_equal(expected_10, continuous_10)); @@ -422,16 +422,16 @@ TEST(HybridGaussianFactorGraph, DiscreteSelection) { GaussianFactorGraph continuous_11 = graph(dv11); GaussianFactorGraph expected_11; expected_11.push_back(JacobianFactor(X(0), I_1x1 * 10, Vector1(-10))); - expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); - expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); expected_11.push_back(JacobianFactor(X(1), I_1x1 * 10, Vector1(-10))); expected_11.push_back(JacobianFactor(X(2), I_1x1 * 10, Vector1(-10))); + expected_11.push_back(JacobianFactor(X(0), -I_1x1, X(1), I_1x1, Vector1(-0))); + expected_11.push_back(JacobianFactor(X(1), -I_1x1, X(2), I_1x1, Vector1(-0))); EXPECT(assert_equal(expected_11, continuous_11)); } /* ************************************************************************* */ -TEST(HybridGaussianFactorGraph, optimize) { +TEST(HybridGaussianFactorGraph, Optimize) { HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); @@ -451,16 +451,16 @@ TEST(HybridGaussianFactorGraph, Conditionals) { Switching switching(4); HybridGaussianFactorGraph hfg; - hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X0) + hfg.push_back(switching.linearUnaryFactors.at(0)); // P(X0) Ordering ordering; ordering.push_back(X(0)); HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering); HybridGaussianFactorGraph hfg2; - hfg2.push_back(*bayes_net); // P(X0) - hfg2.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0) - hfg2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1) - hfg2.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + hfg2.push_back(*bayes_net); // P(X0) + hfg2.push_back(switching.linearBinaryFactors.at(0)); // P(X0, X1 | M0) + hfg2.push_back(switching.linearBinaryFactors.at(1)); // P(X1, X2 | M1) + hfg2.push_back(switching.linearUnaryFactors.at(2)); // P(X2) ordering += X(1), X(2), M(0), M(1); // Created product of first two factors and check eliminate: @@ -510,13 +510,13 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { Switching s(4); HybridGaussianFactorGraph graph; - graph.push_back(s.linearizedFactorGraph.at(0)); // f(X0) - graph.push_back(s.linearizedFactorGraph.at(1)); // f(X0, X1, M0) - graph.push_back(s.linearizedFactorGraph.at(2)); // f(X1, X2, M1) - graph.push_back(s.linearizedFactorGraph.at(4)); // f(X1) - graph.push_back(s.linearizedFactorGraph.at(5)); // f(X2) - graph.push_back(s.linearizedFactorGraph.at(7)); // f(M0) - graph.push_back(s.linearizedFactorGraph.at(8)); // f(M0, M1) + graph.push_back(s.linearUnaryFactors.at(0)); // f(X0) + graph.push_back(s.linearBinaryFactors.at(0)); // f(X0, X1, M0) + graph.push_back(s.linearBinaryFactors.at(1)); // f(X1, X2, M1) + graph.push_back(s.linearUnaryFactors.at(1)); // f(X1) + graph.push_back(s.linearUnaryFactors.at(2)); // f(X2) + graph.push_back(s.modeChain.at(0)); // f(M0) + graph.push_back(s.modeChain.at(1)); // f(M0, M1) HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); @@ -531,8 +531,8 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { graph = HybridGaussianFactorGraph(); graph.push_back(*hybridBayesNet); - graph.push_back(s.linearizedFactorGraph.at(3)); // f(X2, X3, M2) - graph.push_back(s.linearizedFactorGraph.at(6)); // f(X3) + graph.push_back(s.linearBinaryFactors.at(2)); // f(X2, X3, M2) + graph.push_back(s.linearUnaryFactors.at(3)); // f(X3) hybridBayesNet = graph.eliminateSequential(); EXPECT_LONGS_EQUAL(7, hybridBayesNet->size()); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 11e3194f26..b2e388529f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -42,15 +42,15 @@ using symbol_shorthand::Z; /* ****************************************************************************/ namespace switching3 { -// ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1) +// ϕ(x0) ϕ(x1;z1) ϕ(x2;z2) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(m0) ϕ(m0,m1) const Switching switching(3); -const HybridGaussianFactorGraph &lfg = switching.linearizedFactorGraph; +const HybridGaussianFactorGraph &lfg = switching.linearizedFactorGraph(); // First update graph: ϕ(x0) ϕ(x0,x1,m0) ϕ(m0) -const HybridGaussianFactorGraph graph1{lfg.at(0), lfg.at(1), lfg.at(5)}; +const HybridGaussianFactorGraph graph1{lfg.at(0), lfg.at(3), lfg.at(5)}; // Second update graph: ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0,m1) -const HybridGaussianFactorGraph graph2{lfg.at(2), lfg.at(3), lfg.at(4), +const HybridGaussianFactorGraph graph2{lfg.at(4), lfg.at(1), lfg.at(2), lfg.at(6)}; } // namespace switching3 @@ -108,7 +108,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { // Now we calculate the expected factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = - switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + switching.linearizedFactorGraph().eliminatePartialMultifrontal(ordering); // The densities on X(0) should be the same auto x0_conditional = dynamic_pointer_cast( @@ -162,15 +162,14 @@ TEST(HybridGaussianElimination, Approx_inference) { HybridGaussianFactorGraph graph1; // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + for (size_t i = 0; i < 3; i++) { + graph1.push_back(switching.linearBinaryFactors.at(i)); } - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.linearizedFactorGraph.at(0)); - for (size_t i = 4; i <= 7; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + // Add the Gaussian factors, 1 prior on x0, + // 3 measurements on x1, x2, x3 + for (size_t i = 0; i <= 3; i++) { + graph1.push_back(switching.linearUnaryFactors.at(i)); } // Create ordering. @@ -181,7 +180,7 @@ TEST(HybridGaussianElimination, Approx_inference) { // Now we calculate the actual factors using full elimination const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] = - switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + switching.linearizedFactorGraph().eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; incrementalHybrid.update(graph1); @@ -266,15 +265,14 @@ TEST(HybridGaussianElimination, IncrementalApproximate) { /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + for (size_t i = 0; i < 3; i++) { + graph1.push_back(switching.linearBinaryFactors.at(i)); } - // Add the Gaussian factors, 1 prior on X(0), - // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.linearizedFactorGraph.at(0)); - for (size_t i = 5; i <= 7; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + // Add the Gaussian factors, 1 prior on x0, + // 3 measurements on x1, x2, x3 + for (size_t i = 0; i <= 3; i++) { + graph1.push_back(switching.linearUnaryFactors.at(i)); } // Run update with pruning @@ -296,8 +294,8 @@ TEST(HybridGaussianElimination, IncrementalApproximate) { /***** Run Round 2 *****/ HybridGaussianFactorGraph graph2; - graph2.push_back(switching.linearizedFactorGraph.at(4)); - graph2.push_back(switching.linearizedFactorGraph.at(8)); + graph2.push_back(switching.linearBinaryFactors.at(3)); // x3-x4 + graph2.push_back(switching.linearUnaryFactors.at(4)); // x4 // Run update with pruning a second time. incrementalHybrid.update(graph2); diff --git a/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp index 3a4a6c1f41..f1920fa060 100644 --- a/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp @@ -189,8 +189,10 @@ TEST(HybridGaussianProductFactor, AddPruned) { product += prunedFactorB; EXPECT_LONGS_EQUAL(6, product.nrLeaves()); +#ifdef GTSAM_DT_MERGING auto pruned = product.removeEmpty(); EXPECT_LONGS_EQUAL(5, pruned.nrLeaves()); +#endif } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index dd41280349..b22847e894 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -249,7 +249,7 @@ TEST(HybridNonlinearFactorGraph, Switching) { Switching self(3); EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph().size()); - EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph.size()); + EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph().size()); } /**************************************************************************** @@ -276,7 +276,7 @@ TEST(HybridNonlinearFactorGraph, EliminationTree) { for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Create elimination tree. - HybridEliminationTree etree(self.linearizedFactorGraph, ordering); + HybridEliminationTree etree(self.linearizedFactorGraph(), ordering); EXPECT_LONGS_EQUAL(1, etree.roots().size()) } @@ -286,12 +286,12 @@ TEST(HybridNonlinearFactorGraph, EliminationTree) { TEST(GaussianElimination, Eliminate_x0) { Switching self(3); - // Gather factors on x1, has a simple Gaussian and a hybrid factor. + // Gather factors on x0, has a simple Gaussian and a hybrid factor. HybridGaussianFactorGraph factors; // Add gaussian prior - factors.push_back(self.linearizedFactorGraph[0]); + factors.push_back(self.linearUnaryFactors[0]); // Add first hybrid factor - factors.push_back(self.linearizedFactorGraph[1]); + factors.push_back(self.linearBinaryFactors[0]); // Eliminate x0 const Ordering ordering{X(0)}; @@ -313,8 +313,8 @@ TEST(HybridsGaussianElimination, Eliminate_x1) { // Gather factors on x1, will be two hybrid factors (with x0 and x2, resp.). HybridGaussianFactorGraph factors; - factors.push_back(self.linearizedFactorGraph[1]); // involves m0 - factors.push_back(self.linearizedFactorGraph[2]); // involves m1 + factors.push_back(self.linearBinaryFactors[0]); // involves m0 + factors.push_back(self.linearBinaryFactors[1]); // involves m1 // Eliminate x1 const Ordering ordering{X(1)}; @@ -349,7 +349,7 @@ GaussianFactorGraph::shared_ptr batchGFG(double between, TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { Switching self(2, 1.0, 0.1); - auto factors = self.linearizedFactorGraph; + auto factors = self.linearizedFactorGraph(); // Eliminate x0 const Ordering ordering{X(0), X(1)}; @@ -380,15 +380,13 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { TEST(HybridNonlinearFactorGraph, Partial_Elimination) { Switching self(3); - auto linearizedFactorGraph = self.linearizedFactorGraph; - // Create ordering of only continuous variables. Ordering ordering; for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Eliminate partially i.e. only continuous part. const auto [hybridBayesNet, remainingFactorGraph] = - linearizedFactorGraph.eliminatePartialSequential(ordering); + self.linearizedFactorGraph().eliminatePartialSequential(ordering); CHECK(hybridBayesNet); EXPECT_LONGS_EQUAL(3, hybridBayesNet->size()); @@ -444,11 +442,11 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) { // Get nonlinear factor graph and add linear factors to be holistic. // TODO(Frank): ??? HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph(); - fg.add(self.linearizedFactorGraph); + fg.add(self.linearizedFactorGraph()); // Optimize to get HybridValues HybridBayesNet::shared_ptr bn = - self.linearizedFactorGraph.eliminateSequential(); + self.linearizedFactorGraph().eliminateSequential(); HybridValues hv = bn->optimize(); // Print and verify @@ -465,8 +463,6 @@ TEST(HybridNonlinearFactorGraph, PrintErrors) { TEST(HybridNonlinearFactorGraph, Full_Elimination) { Switching self(3); - auto linearizedFactorGraph = self.linearizedFactorGraph; - // We first do a partial elimination DiscreteBayesNet discreteBayesNet; @@ -477,7 +473,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { // Eliminate partially. const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = - linearizedFactorGraph.eliminatePartialSequential(ordering); + self.linearizedFactorGraph().eliminatePartialSequential(ordering); DiscreteFactorGraph discrete_fg; // TODO(Varun) Make this a function of HybridGaussianFactorGraph? @@ -500,7 +496,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { // Eliminate partially. HybridBayesNet::shared_ptr hybridBayesNet = - linearizedFactorGraph.eliminateSequential(ordering); + self.linearizedFactorGraph().eliminateSequential(ordering); CHECK(hybridBayesNet); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); @@ -533,7 +529,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { TEST(HybridNonlinearFactorGraph, Printing) { Switching self(3); - auto linearizedFactorGraph = self.linearizedFactorGraph; + auto linearizedFactorGraph = self.linearizedFactorGraph(); // Create ordering. Ordering ordering; @@ -556,6 +552,24 @@ Factor 0 No noise model Factor 1 +GaussianFactor: + + A[x1] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 2 +GaussianFactor: + + A[x2] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 3 HybridGaussianFactor: Hybrid [x0 x1; m0]{ Choice(m0) @@ -583,7 +597,7 @@ scalar: 0.918939 } -Factor 2 +Factor 4 HybridGaussianFactor: Hybrid [x1 x2; m1]{ Choice(m1) @@ -611,24 +625,6 @@ scalar: 0.918939 } -Factor 3 -GaussianFactor: - - A[x1] = [ - 10 -] - b = [ -10 ] - No noise model - -Factor 4 -GaussianFactor: - - A[x2] = [ - 10 -] - b = [ -10 ] - No noise model - Factor 5 DiscreteFactor: P( m0 ): @@ -651,16 +647,38 @@ Factor 6 #else string expected_hybridFactorGraph = R"( size: 7 -factor 0: +Factor 0 +GaussianFactor: + A[x0] = [ 10 ] b = [ -10 ] No noise model -factor 1: + +Factor 1 +GaussianFactor: + + A[x1] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 2 +GaussianFactor: + + A[x2] = [ + 10 +] + b = [ -10 ] + No noise model + +Factor 3 +HybridGaussianFactor: Hybrid [x0 x1; m0]{ Choice(m0) - 0 Leaf: + 0 Leaf : A[x0] = [ -1 ] @@ -669,8 +687,9 @@ Hybrid [x0 x1; m0]{ ] b = [ -1 ] No noise model +scalar: 0.918939 - 1 Leaf: + 1 Leaf : A[x0] = [ -1 ] @@ -679,12 +698,15 @@ Hybrid [x0 x1; m0]{ ] b = [ -0 ] No noise model +scalar: 0.918939 } -factor 2: + +Factor 4 +HybridGaussianFactor: Hybrid [x1 x2; m1]{ Choice(m1) - 0 Leaf: + 0 Leaf : A[x1] = [ -1 ] @@ -693,8 +715,9 @@ Hybrid [x1 x2; m1]{ ] b = [ -1 ] No noise model +scalar: 0.918939 - 1 Leaf: + 1 Leaf : A[x1] = [ -1 ] @@ -703,26 +726,21 @@ Hybrid [x1 x2; m1]{ ] b = [ -0 ] No noise model +scalar: 0.918939 } -factor 3: - A[x1] = [ - 10 -] - b = [ -10 ] - No noise model -factor 4: - A[x2] = [ - 10 -] - b = [ -10 ] - No noise model -factor 5: P( m0 ): + +Factor 5 +DiscreteFactor: + P( m0 ): Choice(m0) - 0 Leaf 0.5 - 1 Leaf 0.5 + 0 Leaf 0.5 + 1 Leaf 0.5 -factor 6: P( m1 | m0 ): + +Factor 6 +DiscreteFactor: + P( m1 | m0 ): Choice(m1) 0 Choice(m0) 0 0 Leaf 0.33333333 @@ -731,6 +749,7 @@ factor 6: P( m1 | m0 ): 1 0 Leaf 0.66666667 1 1 Leaf 0.4 + )"; #endif diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index c5f52e3ebf..6516ef1fba 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -147,7 +147,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // Now we calculate the actual factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = - switching.linearizedFactorGraph + switching.linearizedFactorGraph() .BaseEliminateable::eliminatePartialMultifrontal(ordering); // The densities on X(1) should be the same @@ -214,8 +214,6 @@ TEST(HybridNonlinearISAM, Approx_inference) { initial.insert(X(i), i + 1); } - // TODO(Frank): no mode chain? - // Create ordering. Ordering ordering; for (size_t j = 0; j < 4; j++) { @@ -224,7 +222,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { // Now we calculate the actual factors using full elimination const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] = - switching.linearizedFactorGraph + switching.linearizedFactorGraph() .BaseEliminateable::eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; @@ -325,7 +323,6 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { // TODO(Frank): no mode chain? - // Run update with pruning size_t maxComponents = 5; incrementalHybrid.update(graph1, initial); @@ -347,7 +344,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { /***** Run Round 2 *****/ HybridGaussianFactorGraph graph2; graph2.push_back(switching.binaryFactors.at(3)); // x3-x4 - graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement + graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5); diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 5b06312ba4..3be96b7512 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -130,7 +130,7 @@ TEST(HybridSerialization, HybridGaussianConditional) { // Test HybridBayesNet serialization. TEST(HybridSerialization, HybridBayesNet) { Switching s(2); - HybridBayesNet hbn = *(s.linearizedFactorGraph.eliminateSequential()); + HybridBayesNet hbn = *(s.linearizedFactorGraph().eliminateSequential()); EXPECT(equalsObj(hbn)); EXPECT(equalsXML(hbn)); @@ -141,7 +141,7 @@ TEST(HybridSerialization, HybridBayesNet) { // Test HybridBayesTree serialization. TEST(HybridSerialization, HybridBayesTree) { Switching s(2); - HybridBayesTree hbt = *(s.linearizedFactorGraph.eliminateMultifrontal()); + HybridBayesTree hbt = *(s.linearizedFactorGraph().eliminateMultifrontal()); EXPECT(equalsObj(hbt)); EXPECT(equalsXML(hbt));