diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 64025397f..fdf9092b6 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 f972e7cac..9c2580a17 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(); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index e77476e25..dd4128034 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 b804a176b..c5f52e3eb 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)); + 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 if the clique conditional generated from incremental elimination + // 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);