diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index c503118eb3..048fd2701e 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -132,6 +132,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph explicit HybridGaussianFactorGraph(const CONTAINER& factors) : Base(factors) {} + /** + * Construct from an initializer lists of GaussianFactor shared pointers. + * Example: + * HybridGaussianFactorGraph graph = { factor1, factor2, factor3 }; + */ + HybridGaussianFactorGraph(std::initializer_list factors) + : Base(factors) {} + /** * Implicit copy/downcast constructor to override explicit template container * constructor. In BayesTree this is used for: diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 6f8b7b9ffe..28116df45d 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridGaussianISAM.h + * @file HybridGaussianISAM.cpp * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index ec6ebf4d0a..503afaa723 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -39,8 +39,8 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { - // TODO(Varun) Relinearization doesn't take into account pruning - reorder_relinearize(); + // TODO(Varun) Re-linearization doesn't take into account pruning + reorderRelinearize(); reorderCounter_ = 0; } @@ -60,7 +60,7 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridNonlinearISAM::reorder_relinearize() { +void HybridNonlinearISAM::reorderRelinearize() { if (factors_.size() > 0) { // Obtain the new linearization point const Values newLinPoint = estimate(); @@ -69,7 +69,7 @@ void HybridNonlinearISAM::reorder_relinearize() { // Just recreate the whole BayesTree // TODO: allow for constrained ordering here - // TODO: decouple relinearization and reordering to avoid + // TODO: decouple re-linearization and reordering to avoid isam_.update(*factors_.linearize(newLinPoint), {}, {}, eliminationFunction_); diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index e41b4ebe1b..686a083276 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -37,7 +37,7 @@ class GTSAM_EXPORT HybridNonlinearISAM { /// The discrete assignment DiscreteValues assignment_; - /** The original factors, used when relinearizing */ + /** The original factors, used when re-linearizing */ HybridNonlinearFactorGraph factors_; /** The reordering interval and counter */ @@ -52,8 +52,8 @@ class GTSAM_EXPORT HybridNonlinearISAM { /// @{ /** - * Periodically reorder and relinearize - * @param reorderInterval is the number of updates between reorderings, + * Periodically reorder and re-linearize + * @param reorderInterval is the number of updates between re-orderings, * 0 never reorders (and is dangerous for memory consumption) * 1 (default) reorders every time, in worse case is batch every update * typical values are 50 or 100 @@ -124,8 +124,8 @@ class GTSAM_EXPORT HybridNonlinearISAM { const std::optional& maxNrLeaves = {}, const std::optional& ordering = {}); - /** Relinearization and reordering of variables */ - void reorder_relinearize(); + /** Re-linearization and reordering of variables */ + void reorderRelinearize(); /// @} }; diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 64025397ff..fdf9092b6c 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -120,12 +120,21 @@ 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 nonlinearFactorGraph; + HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain; HybridGaussianFactorGraph linearizedFactorGraph; Values linearizationPoint; + // Access the flat nonlinear factor graph. + const HybridNonlinearFactorGraph &nonlinearFactorGraph() const { + return nonlinearFactorGraph_; + } + /** * @brief Create with given number of time steps. * @@ -136,7 +145,7 @@ struct Switching { */ Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1, std::vector measurements = {}, - std::string discrete_transition_prob = "1/2 3/2") + std::string transitionProbabilityTable = "1/2 3/2") : K(K) { using noiseModel::Isotropic; @@ -155,32 +164,36 @@ struct Switching { // Create hybrid factor graph. // Add a prior ϕ(X(0)) on X(0). - nonlinearFactorGraph.emplace_shared>( + nonlinearFactorGraph_.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], + nonlinearFactorGraph_.emplace_shared(modes[k], motion_models); + binaryFactors.push_back(nonlinearFactorGraph_.back()); } // 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>( + nonlinearFactorGraph_.emplace_shared>( X(k), measurements.at(k), measurement_noise); + unaryFactors.push_back(nonlinearFactorGraph_.back()); } // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2)) - addModeChain(&nonlinearFactorGraph, discrete_transition_prob); + 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); + linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint); } // Create motion models for a given time step @@ -200,15 +213,16 @@ struct Switching { * * @param fg The factor graph to which the mode chain is added. */ - template - void addModeChain(FACTORGRAPH *fg, - std::string discrete_transition_prob = "1/2 3/2") { - fg->template emplace_shared(modes[0], "1/1"); + HybridNonlinearFactorGraph createModeChain( + std::string transitionProbabilityTable = "1/2 3/2") { + HybridNonlinearFactorGraph chain; + chain.emplace_shared(modes[0], "1/1"); for (size_t k = 0; k < K - 2; k++) { auto parents = {modes[k]}; - fg->template emplace_shared( - modes[k + 1], parents, discrete_transition_prob); + chain.emplace_shared(modes[k + 1], parents, + transitionProbabilityTable); } + return chain; } }; diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 6206e5f459..9c2580a17a 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -37,6 +37,8 @@ // Include for test suite #include +#include + #include "Switching.h" using namespace std; @@ -55,13 +57,16 @@ std::vector discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0, Switching InitializeEstimationProblem( const size_t K, const double between_sigma, const double measurement_sigma, const std::vector& measurements, - const std::string& discrete_transition_prob, + const std::string& transitionProbabilityTable, HybridNonlinearFactorGraph& graph, Values& initial) { Switching switching(K, between_sigma, measurement_sigma, measurements, - discrete_transition_prob); + transitionProbabilityTable); + + // Add prior on M(0) + graph.push_back(switching.modeChain.at(0)); // Add the X(0) prior - graph.push_back(switching.nonlinearFactorGraph.at(0)); + graph.push_back(switching.unaryFactors.at(0)); initial.insert(X(0), switching.linearizationPoint.at(X(0))); return switching; @@ -128,10 +133,9 @@ TEST(HybridEstimation, IncrementalSmoother) { constexpr size_t maxNrLeaves = 3; for (size_t k = 1; k < K; k++) { - // Motion Model - graph.push_back(switching.nonlinearFactorGraph.at(k)); - // Measurement - graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1)); + if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain + graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model + graph.push_back(switching.unaryFactors.at(k)); // Measurement initial.insert(X(k), switching.linearizationPoint.at(X(k))); @@ -176,10 +180,9 @@ TEST(HybridEstimation, ValidPruningError) { constexpr size_t maxNrLeaves = 3; for (size_t k = 1; k < K; k++) { - // Motion Model - graph.push_back(switching.nonlinearFactorGraph.at(k)); - // Measurement - graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1)); + if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain + graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model + graph.push_back(switching.unaryFactors.at(k)); // Measurement initial.insert(X(k), switching.linearizationPoint.at(X(k))); @@ -225,15 +228,17 @@ TEST(HybridEstimation, ISAM) { HybridGaussianFactorGraph linearized; + const size_t maxNrLeaves = 3; for (size_t k = 1; k < K; k++) { - // Motion Model - graph.push_back(switching.nonlinearFactorGraph.at(k)); - // Measurement - graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1)); + if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain + graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model + graph.push_back(switching.unaryFactors.at(k)); // Measurement initial.insert(X(k), switching.linearizationPoint.at(X(k))); - isam.update(graph, initial, 3); + isam.update(graph, initial, maxNrLeaves); + // isam.saveGraph("NLiSAM" + std::to_string(k) + ".dot"); + // GTSAM_PRINT(isam); graph.resize(0); initial.clear(); @@ -339,12 +344,8 @@ TEST(HybridEstimation, Probability) { HybridValues hybrid_values = bayesNet->optimize(); // This is the correct sequence as designed - DiscreteValues discrete_seq; - discrete_seq[M(0)] = 1; - discrete_seq[M(1)] = 1; - discrete_seq[M(2)] = 0; - - EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); + DiscreteValues expectedSequence{{M(0), 1}, {M(1), 1}, {M(2), 0}}; + EXPECT(assert_equal(expectedSequence, hybrid_values.discrete())); } /****************************************************************************/ @@ -411,12 +412,8 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { HybridValues hybrid_values = discreteBayesTree->optimize(); // This is the correct sequence as designed - DiscreteValues discrete_seq; - discrete_seq[M(0)] = 1; - discrete_seq[M(1)] = 1; - discrete_seq[M(2)] = 0; - - EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); + DiscreteValues expectedSequence{{M(0), 1}, {M(1), 1}, {M(2), 0}}; + EXPECT(assert_equal(expectedSequence, hybrid_values.discrete())); } /********************************************************************************* diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 70fa321ada..11e3194f26 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testHybridIncremental.cpp + * @file testHybridGaussianISAM.cpp * @brief Unit tests for incremental inference * @author Fan Jiang, Varun Agrawal, Frank Dellaert * @date Jan 2021 @@ -27,8 +27,6 @@ #include #include -#include - #include "Switching.h" // Include for test suite @@ -36,77 +34,63 @@ using namespace std; using namespace gtsam; -using noiseModel::Isotropic; -using symbol_shorthand::L; using symbol_shorthand::M; using symbol_shorthand::W; using symbol_shorthand::X; using symbol_shorthand::Y; using symbol_shorthand::Z; +/* ****************************************************************************/ +namespace switching3 { +// ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1) +const Switching switching(3); +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)}; + +// Second update graph: ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0,m1) +const HybridGaussianFactorGraph graph2{lfg.at(2), lfg.at(3), lfg.at(4), + lfg.at(6)}; +} // namespace switching3 + /* ****************************************************************************/ // Test if we can perform elimination incrementally. TEST(HybridGaussianElimination, IncrementalElimination) { - Switching switching(3); + using namespace switching3; HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; - - // Create initial factor graph - // * * * - // | | | - // X0 -*- X1 -*- X2 - // \*-M0-*/ - graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X0) - graph1.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0) - graph1.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1) - graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M0) - // Run update step + // Run first update step isam.update(graph1); // Check that after update we have 2 hybrid Bayes net nodes: - // P(X0 | X1, M0) and P(X1, X2 | M0, M1), P(M0, M1) - EXPECT_LONGS_EQUAL(3, isam.size()); - EXPECT(isam[X(0)]->conditional()->frontals() == KeyVector{X(0)}); - EXPECT(isam[X(0)]->conditional()->parents() == KeyVector({X(1), M(0)})); - EXPECT(isam[X(1)]->conditional()->frontals() == KeyVector({X(1), X(2)})); - EXPECT(isam[X(1)]->conditional()->parents() == KeyVector({M(0), M(1)})); + // P(M0) and P(X0, X1 | M0) + EXPECT_LONGS_EQUAL(2, isam.size()); + EXPECT(isam[M(0)]->conditional()->frontals() == KeyVector({M(0)})); + EXPECT(isam[M(0)]->conditional()->parents() == KeyVector()); + EXPECT(isam[X(0)]->conditional()->frontals() == KeyVector({X(0), X(1)})); + EXPECT(isam[X(0)]->conditional()->parents() == KeyVector({M(0)})); /********************************************************/ - // New factor graph for incremental update. - HybridGaussianFactorGraph graph2; - - graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X1) - graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X2) - graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M0, M1) - + // Run second update step isam.update(graph2); - // Check that after the second update we have - // 1 additional hybrid Bayes net node: - // P(X1, X2 | M0, M1) + // Check that after update we have 3 hybrid Bayes net nodes: + // P(X1, X2 | M0, M1) P(X1, X2 | M0, M1) EXPECT_LONGS_EQUAL(3, isam.size()); - EXPECT(isam[X(2)]->conditional()->frontals() == KeyVector({X(1), X(2)})); - EXPECT(isam[X(2)]->conditional()->parents() == KeyVector({M(0), M(1)})); + EXPECT(isam[M(0)]->conditional()->frontals() == KeyVector({M(0), M(1)})); + EXPECT(isam[M(0)]->conditional()->parents() == KeyVector()); + EXPECT(isam[X(1)]->conditional()->frontals() == KeyVector({X(1), X(2)})); + EXPECT(isam[X(1)]->conditional()->parents() == KeyVector({M(0), M(1)})); + EXPECT(isam[X(0)]->conditional()->frontals() == KeyVector{X(0)}); + EXPECT(isam[X(0)]->conditional()->parents() == KeyVector({X(1), M(0)})); } /* ****************************************************************************/ // Test if we can incrementally do the inference TEST(HybridGaussianElimination, IncrementalInference) { - Switching switching(3); + using namespace switching3; HybridGaussianISAM isam; - HybridGaussianFactorGraph graph1; - - // Create initial factor graph - // * * * - // | | | - // X0 -*- X1 -*- X2 - // | | - // *-M0 - * - M1 - graph1.push_back(switching.linearizedFactorGraph.at(0)); // P(X0) - graph1.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0) - graph1.push_back(switching.linearizedFactorGraph.at(3)); // P(X1) - graph1.push_back(switching.linearizedFactorGraph.at(5)); // P(M0) // Run update step isam.update(graph1); @@ -115,13 +99,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { EXPECT(discreteConditional_m0->keys() == KeyVector({M(0)})); /********************************************************/ - // New factor graph for incremental update. - HybridGaussianFactorGraph graph2; - - graph2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1) - graph2.push_back(switching.linearizedFactorGraph.at(4)); // P(X2) - graph2.push_back(switching.linearizedFactorGraph.at(6)); // P(M0, M1) - + // Second incremental update. isam.update(graph2); /********************************************************/ @@ -160,44 +138,19 @@ TEST(HybridGaussianElimination, IncrementalInference) { // The other discrete probabilities on M(2) are calculated the same way const Ordering discreteOrdering{M(0), M(1)}; HybridBayesTree::shared_ptr discreteBayesTree = - expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( - discreteOrdering); - - DiscreteValues m00; - m00[M(0)] = 0, m00[M(1)] = 0; - DiscreteConditional decisionTree = - *(*discreteBayesTree)[M(1)]->conditional()->asDiscrete(); - double m00_prob = decisionTree(m00); - - auto discreteConditional = isam[M(1)]->conditional()->asDiscrete(); + expectedRemainingGraph->eliminateMultifrontal(discreteOrdering); // Test the probability values with regression tests. - DiscreteValues assignment; - EXPECT(assert_equal(0.0952922, m00_prob, 1e-5)); - assignment[M(0)] = 0; - assignment[M(1)] = 0; - EXPECT(assert_equal(0.0952922, (*discreteConditional)(assignment), 1e-5)); - assignment[M(0)] = 1; - assignment[M(1)] = 0; - EXPECT(assert_equal(0.282758, (*discreteConditional)(assignment), 1e-5)); - assignment[M(0)] = 0; - assignment[M(1)] = 1; - EXPECT(assert_equal(0.314175, (*discreteConditional)(assignment), 1e-5)); - assignment[M(0)] = 1; - assignment[M(1)] = 1; - EXPECT(assert_equal(0.307775, (*discreteConditional)(assignment), 1e-5)); - - // Check if the clique conditional generated from incremental elimination + auto discrete = isam[M(1)]->conditional()->asDiscrete(); + EXPECT(assert_equal(0.095292, (*discrete)({{M(0), 0}, {M(1), 0}}), 1e-5)); + EXPECT(assert_equal(0.282758, (*discrete)({{M(0), 1}, {M(1), 0}}), 1e-5)); + EXPECT(assert_equal(0.314175, (*discrete)({{M(0), 0}, {M(1), 1}}), 1e-5)); + EXPECT(assert_equal(0.307775, (*discrete)({{M(0), 1}, {M(1), 1}}), 1e-5)); + + // Check that the clique conditional generated from incremental elimination // matches that of batch elimination. - auto expectedChordal = - expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(); - auto actualConditional = dynamic_pointer_cast( - isam[M(1)]->conditional()->inner()); - // Account for the probability terms from evaluating continuous FGs - DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}}; - vector probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485}; - auto expectedConditional = - std::make_shared(discrete_keys, probs); + auto expectedConditional = (*discreteBayesTree)[M(1)]->conditional(); + auto actualConditional = isam[M(1)]->conditional(); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); } @@ -227,7 +180,7 @@ TEST(HybridGaussianElimination, Approx_inference) { } // Now we calculate the actual factors using full elimination - const auto [unprunedHybridBayesTree, unprunedRemainingGraph] = + const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] = switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; @@ -236,7 +189,7 @@ TEST(HybridGaussianElimination, Approx_inference) { incrementalHybrid.prune(maxNrLeaves); /* - unpruned factor is: + unPruned factor is: Choice(m3) 0 Choice(m2) 0 0 Choice(m1) @@ -282,8 +235,8 @@ TEST(HybridGaussianElimination, Approx_inference) { // Check that the hybrid nodes of the bayes net match those of the pre-pruning // bayes net, at the same positions. - auto &unprunedLastDensity = *dynamic_pointer_cast( - unprunedHybridBayesTree->clique(X(3))->conditional()->inner()); + auto &unPrunedLastDensity = *dynamic_pointer_cast( + unPrunedHybridBayesTree->clique(X(3))->conditional()->inner()); auto &lastDensity = *dynamic_pointer_cast( incrementalHybrid[X(3)]->conditional()->inner()); @@ -298,7 +251,7 @@ TEST(HybridGaussianElimination, Approx_inference) { EXPECT(lastDensity(assignment) == nullptr); } else { CHECK(lastDensity(assignment)); - EXPECT(assert_equal(*unprunedLastDensity(assignment), + EXPECT(assert_equal(*unPrunedLastDensity(assignment), *lastDensity(assignment))); } } @@ -306,7 +259,7 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridGaussianElimination, Incremental_approximate) { +TEST(HybridGaussianElimination, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph1; @@ -330,7 +283,7 @@ TEST(HybridGaussianElimination, Incremental_approximate) { incrementalHybrid.prune(maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, - // each with 2, 4, 8, and 5 (pruned) leaves respetively. + // each with 2, 4, 8, and 5 (pruned) leaves respectively. EXPECT_LONGS_EQUAL(4, incrementalHybrid.size()); EXPECT_LONGS_EQUAL( 2, incrementalHybrid[X(0)]->conditional()->asHybrid()->nrComponents()); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index e77476e258..dd41280349 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -216,8 +216,8 @@ TEST(HybridNonlinearFactorGraph, PushBack) { TEST(HybridNonlinearFactorGraph, ErrorTree) { Switching s(3); - HybridNonlinearFactorGraph graph = s.nonlinearFactorGraph; - Values values = s.linearizationPoint; + const HybridNonlinearFactorGraph &graph = s.nonlinearFactorGraph(); + const Values &values = s.linearizationPoint; auto error_tree = graph.errorTree(s.linearizationPoint); @@ -248,7 +248,7 @@ TEST(HybridNonlinearFactorGraph, ErrorTree) { TEST(HybridNonlinearFactorGraph, Switching) { Switching self(3); - EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph.size()); + EXPECT_LONGS_EQUAL(7, self.nonlinearFactorGraph().size()); EXPECT_LONGS_EQUAL(7, self.linearizedFactorGraph.size()); } @@ -260,7 +260,7 @@ TEST(HybridNonlinearFactorGraph, Linearization) { // Linearize here: HybridGaussianFactorGraph actualLinearized = - *self.nonlinearFactorGraph.linearize(self.linearizationPoint); + *self.nonlinearFactorGraph().linearize(self.linearizationPoint); EXPECT_LONGS_EQUAL(7, actualLinearized.size()); } @@ -409,7 +409,7 @@ TEST(HybridNonlinearFactorGraph, Partial_Elimination) { /* ****************************************************************************/ TEST(HybridNonlinearFactorGraph, Error) { Switching self(3); - HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph; + HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph(); { HybridValues values(VectorValues(), DiscreteValues{{M(0), 0}, {M(1), 0}}, @@ -441,8 +441,9 @@ TEST(HybridNonlinearFactorGraph, Error) { TEST(HybridNonlinearFactorGraph, PrintErrors) { Switching self(3); - // Get nonlinear factor graph and add linear factors to be holistic - HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph; + // Get nonlinear factor graph and add linear factors to be holistic. + // TODO(Frank): ??? + HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph(); fg.add(self.linearizedFactorGraph); // Optimize to get HybridValues diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index b804a176ba..c5f52e3ebf 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -57,10 +57,10 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { // | | | // X0 -*- X1 -*- X2 // \*-M0-*/ - graph1.push_back(switching.nonlinearFactorGraph.at(0)); // P(X0) - graph1.push_back(switching.nonlinearFactorGraph.at(1)); // P(X0, X1 | M0) - graph1.push_back(switching.nonlinearFactorGraph.at(2)); // P(X1, X2 | M1) - graph1.push_back(switching.nonlinearFactorGraph.at(5)); // P(M0) + graph1.push_back(switching.unaryFactors.at(0)); // P(X0) + graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph1.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph1.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); @@ -83,9 +83,9 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { HybridNonlinearFactorGraph graph2; initial = Values(); - graph1.push_back(switching.nonlinearFactorGraph.at(3)); // P(X1) - graph2.push_back(switching.nonlinearFactorGraph.at(4)); // P(X2) - graph2.push_back(switching.nonlinearFactorGraph.at(6)); // P(M0, M1) + graph1.push_back(switching.unaryFactors.at(1)); // P(X1) + graph2.push_back(switching.unaryFactors.at(2)); // P(X2) + graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) isam.update(graph2, initial); @@ -112,10 +112,10 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // X0 -*- X1 -*- X2 // | | // *-M0 - * - M1 - graph1.push_back(switching.nonlinearFactorGraph.at(0)); // P(X0) - graph1.push_back(switching.nonlinearFactorGraph.at(1)); // P(X0, X1 | M0) - graph1.push_back(switching.nonlinearFactorGraph.at(3)); // P(X1) - graph1.push_back(switching.nonlinearFactorGraph.at(5)); // P(M0) + graph1.push_back(switching.unaryFactors.at(0)); // P(X0) + graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph1.push_back(switching.unaryFactors.at(1)); // P(X1) + graph1.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); @@ -134,9 +134,9 @@ TEST(HybridNonlinearISAM, IncrementalInference) { initial.insert(X(2), 3); - graph2.push_back(switching.nonlinearFactorGraph.at(2)); // P(X1, X2 | M1) - graph2.push_back(switching.nonlinearFactorGraph.at(4)); // P(X2) - graph2.push_back(switching.nonlinearFactorGraph.at(6)); // P(M0, M1) + graph2.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph2.push_back(switching.unaryFactors.at(2)); // P(X2) + graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) isam.update(graph2, initial); bayesTree = isam.bayesTree(); @@ -175,46 +175,22 @@ TEST(HybridNonlinearISAM, IncrementalInference) { EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); // We only perform manual continuous elimination for 0,0. - // The other discrete probabilities on M(1) are calculated the same way + // The other discrete probabilities on M(2) are calculated the same way const Ordering discreteOrdering{M(0), M(1)}; HybridBayesTree::shared_ptr discreteBayesTree = - expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( - discreteOrdering); - - DiscreteValues m00; - m00[M(0)] = 0, m00[M(1)] = 0; - DiscreteConditional decisionTree = - *(*discreteBayesTree)[M(1)]->conditional()->asDiscrete(); - double m00_prob = decisionTree(m00); - - auto discreteConditional = bayesTree[M(1)]->conditional()->asDiscrete(); + expectedRemainingGraph->eliminateMultifrontal(discreteOrdering); // Test the probability values with regression tests. - DiscreteValues assignment; - EXPECT(assert_equal(0.0952922, m00_prob, 1e-5)); - assignment[M(0)] = 0; - assignment[M(1)] = 0; - EXPECT(assert_equal(0.0952922, (*discreteConditional)(assignment), 1e-5)); - assignment[M(0)] = 1; - assignment[M(1)] = 0; - EXPECT(assert_equal(0.282758, (*discreteConditional)(assignment), 1e-5)); - assignment[M(0)] = 0; - assignment[M(1)] = 1; - EXPECT(assert_equal(0.314175, (*discreteConditional)(assignment), 1e-5)); - assignment[M(0)] = 1; - assignment[M(1)] = 1; - EXPECT(assert_equal(0.307775, (*discreteConditional)(assignment), 1e-5)); - - // Check if the clique conditional generated from incremental elimination + auto discrete = bayesTree[M(1)]->conditional()->asDiscrete(); + EXPECT(assert_equal(0.095292, (*discrete)({{M(0), 0}, {M(1), 0}}), 1e-5)); + EXPECT(assert_equal(0.282758, (*discrete)({{M(0), 1}, {M(1), 0}}), 1e-5)); + EXPECT(assert_equal(0.314175, (*discrete)({{M(0), 0}, {M(1), 1}}), 1e-5)); + EXPECT(assert_equal(0.307775, (*discrete)({{M(0), 1}, {M(1), 1}}), 1e-5)); + + // Check that the clique conditional generated from incremental elimination // matches that of batch elimination. - auto expectedChordal = expectedRemainingGraph->eliminateMultifrontal(); - auto actualConditional = dynamic_pointer_cast( - bayesTree[M(1)]->conditional()->inner()); - // Account for the probability terms from evaluating continuous FGs - DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}}; - vector probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485}; - auto expectedConditional = - std::make_shared(discrete_keys, probs); + auto expectedConditional = (*discreteBayesTree)[M(1)]->conditional(); + auto actualConditional = bayesTree[M(1)]->conditional(); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); } @@ -227,18 +203,19 @@ TEST(HybridNonlinearISAM, Approx_inference) { Values initial; // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 - for (size_t i = 1; i < 4; i++) { - graph1.push_back(switching.nonlinearFactorGraph.at(i)); + for (size_t i = 0; i < 3; i++) { + graph1.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.nonlinearFactorGraph.at(0)); - for (size_t i = 4; i <= 7; i++) { - graph1.push_back(switching.nonlinearFactorGraph.at(i)); - initial.insert(X(i - 4), i - 3); + for (size_t i = 0; i < 4; i++) { + graph1.push_back(switching.unaryFactors.at(i)); + initial.insert(X(i), i + 1); } + // TODO(Frank): no mode chain? + // Create ordering. Ordering ordering; for (size_t j = 0; j < 4; j++) { @@ -246,7 +223,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { } // Now we calculate the actual factors using full elimination - const auto [unprunedHybridBayesTree, unprunedRemainingGraph] = + const auto [unPrunedHybridBayesTree, unPrunedRemainingGraph] = switching.linearizedFactorGraph .BaseEliminateable::eliminatePartialMultifrontal(ordering); @@ -257,7 +234,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { bayesTree.prune(maxNrLeaves); /* - unpruned factor is: + unPruned factor is: Choice(m3) 0 Choice(m2) 0 0 Choice(m1) @@ -303,8 +280,8 @@ TEST(HybridNonlinearISAM, Approx_inference) { // Check that the hybrid nodes of the bayes net match those of the pre-pruning // bayes net, at the same positions. - auto &unprunedLastDensity = *dynamic_pointer_cast( - unprunedHybridBayesTree->clique(X(3))->conditional()->inner()); + auto &unPrunedLastDensity = *dynamic_pointer_cast( + unPrunedHybridBayesTree->clique(X(3))->conditional()->inner()); auto &lastDensity = *dynamic_pointer_cast( bayesTree[X(3)]->conditional()->inner()); @@ -319,7 +296,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { EXPECT(lastDensity(assignment) == nullptr); } else { CHECK(lastDensity(assignment)); - EXPECT(assert_equal(*unprunedLastDensity(assignment), + EXPECT(assert_equal(*unPrunedLastDensity(assignment), *lastDensity(assignment))); } } @@ -335,19 +312,20 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { /***** 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.nonlinearFactorGraph.at(i)); + for (size_t i = 0; i < 3; i++) { + graph1.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.nonlinearFactorGraph.at(0)); - initial.insert(X(0), 1); - for (size_t i = 5; i <= 7; i++) { - graph1.push_back(switching.nonlinearFactorGraph.at(i)); - initial.insert(X(i - 4), i - 3); + for (size_t i = 0; i < 4; i++) { + graph1.push_back(switching.unaryFactors.at(i)); + initial.insert(X(i), i + 1); } + // TODO(Frank): no mode chain? + + // Run update with pruning size_t maxComponents = 5; incrementalHybrid.update(graph1, initial); @@ -368,8 +346,8 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { /***** Run Round 2 *****/ HybridGaussianFactorGraph graph2; - graph2.push_back(switching.nonlinearFactorGraph.at(4)); // x3-x4 - graph2.push_back(switching.nonlinearFactorGraph.at(8)); // x4 measurement + graph2.push_back(switching.binaryFactors.at(3)); // x3-x4 + graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5);