diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6c06c1c0a7..6d1064647e 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -148,6 +148,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// Getter for GaussianFactor decision tree const FactorValuePairs &factors() const { return factors_; } + /** * @brief Helper function to return factors and functional to create a * DecisionTree of Gaussian Factor Graphs. diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 9ca7a3938e..8832cbb34f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -581,4 +581,15 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose( return gfg; } +/* ************************************************************************ */ +DiscreteFactorGraph HybridGaussianFactorGraph::discreteFactors() const { + DiscreteFactorGraph dfg; + for (auto &&f : factors_) { + if (auto discreteFactor = std::dynamic_pointer_cast(f)) { + dfg.push_back(discreteFactor); + } + } + return dfg; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 048fd2701e..e3c1e2d557 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -254,6 +255,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph GaussianFactorGraph operator()(const DiscreteValues& assignment) const { return choose(assignment); } + + /** + * @brief Helper method to get all the discrete factors + * as a DiscreteFactorGraph. + * + * @return DiscreteFactorGraph + */ + DiscreteFactorGraph discreteFactors() const; }; // traits diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 6ffb955117..376bc66f1f 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -16,6 +16,7 @@ * @date Sep 12, 2024 */ +#include #include #include #include @@ -184,6 +185,11 @@ std::shared_ptr HybridNonlinearFactor::linearize( [continuousValues]( const std::pair& f) -> GaussianFactorValuePair { auto [factor, val] = f; + // Check if valid factor. If not, return null and infinite error. + if (!factor) { + return {nullptr, std::numeric_limits::infinity()}; + } + if (auto gaussian = std::dynamic_pointer_cast( factor->noiseModel())) { return {factor->linearize(continuousValues), @@ -202,4 +208,34 @@ std::shared_ptr HybridNonlinearFactor::linearize( linearized_factors); } -} // namespace gtsam \ No newline at end of file +/* *******************************************************************************/ +HybridNonlinearFactor::shared_ptr HybridNonlinearFactor::prune( + const DecisionTreeFactor& discreteProbs) const { + // Find keys in discreteProbs.keys() but not in this->keys(): + std::set mine(this->keys().begin(), this->keys().end()); + std::set theirs(discreteProbs.keys().begin(), + discreteProbs.keys().end()); + std::vector diff; + std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(), + std::back_inserter(diff)); + + // Find maximum probability value for every combination of our keys. + Ordering keys(diff); + auto max = discreteProbs.max(keys); + + // Check the max value for every combination of our keys. + // If the max value is 0.0, we can prune the corresponding conditional. + auto pruner = + [&](const Assignment& choices, + const NonlinearFactorValuePair& pair) -> NonlinearFactorValuePair { + if (max->evaluate(choices) == 0.0) + return {nullptr, std::numeric_limits::infinity()}; + else + return pair; + }; + + FactorValuePairs prunedFactors = factors().apply(pruner); + return std::make_shared(discreteKeys(), prunedFactors); +} + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 325fa3eaab..e264b1d10d 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -166,6 +166,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { /// @} + /// Getter for NonlinearFactor decision tree + const FactorValuePairs& factors() const { return factors_; } + /// Linearize specific nonlinear factors based on the assignment in /// discreteValues. GaussianFactor::shared_ptr linearize( @@ -176,6 +179,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { std::shared_ptr linearize( const Values& continuousValues) const; + /// Prune this factor based on the discrete probabilities. + HybridNonlinearFactor::shared_ptr prune( + const DecisionTreeFactor& discreteProbs) const; + private: /// Helper struct to assist private constructor below. struct ConstructorHelper; diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 503afaa723..29e467d866 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -39,7 +40,6 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { - // TODO(Varun) Re-linearization doesn't take into account pruning reorderRelinearize(); reorderCounter_ = 0; } @@ -65,8 +65,21 @@ void HybridNonlinearISAM::reorderRelinearize() { // Obtain the new linearization point const Values newLinPoint = estimate(); + auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete()); + isam_.clear(); + // Prune nonlinear factors based on discrete conditional probabilities + HybridNonlinearFactorGraph pruned_factors; + for (auto&& factor : factors_) { + if (auto nf = std::dynamic_pointer_cast(factor)) { + pruned_factors.push_back(nf->prune(discreteProbs)); + } else { + pruned_factors.push_back(factor); + } + } + factors_ = pruned_factors; + // Just recreate the whole BayesTree // TODO: allow for constrained ordering here // TODO: decouple re-linearization and reordering to avoid diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 49ced968ed..1e3510fcc8 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -52,7 +52,8 @@ using symbol_shorthand::X; * @return HybridGaussianFactorGraph::shared_ptr */ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( - size_t K, std::function x = X, std::function m = M) { + size_t K, std::function x = X, std::function m = M, + const std::string &transitionProbabilityTable = "0 1 1 3") { HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1)); @@ -68,7 +69,8 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( hfg.add(HybridGaussianFactor({m(k), 2}, components)); if (k > 1) { - hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, "0 1 1 3")); + hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, + transitionProbabilityTable)); } } diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 9e886200a1..7381761148 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -74,9 +74,7 @@ TEST(HybridGaussianFactorGraph, hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); hfg.add(DecisionTreeFactor(m1, {2, 8})); - // TODO(Varun) Adding extra discrete variable not connected to continuous - // variable throws segfault - // hfg.add(DecisionTreeFactor({m1, m2, "1 2 3 4")); + hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4")); HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal(); @@ -176,7 +174,7 @@ TEST(HybridGaussianFactorGraph, Switching) { // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; + Ordering ordering; { std::vector naturalX(N); @@ -187,10 +185,6 @@ TEST(HybridGaussianFactorGraph, Switching) { auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto& l : lvls) { - l = -l; - } } { std::vector naturalC(N - 1); @@ -199,14 +193,11 @@ TEST(HybridGaussianFactorGraph, Switching) { std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) { return M(x); }); - // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); const auto [ndC, lvls] = makeBinaryOrdering(ordC); std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); } - auto ordering_full = Ordering(ordering); - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); + const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering); // 12 cliques in the bayes tree and 0 remaining variables to eliminate. EXPECT_LONGS_EQUAL(12, hbt->size()); @@ -230,7 +221,7 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; + Ordering ordering; { std::vector naturalX(N); @@ -241,10 +232,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto& l : lvls) { - l = -l; - } } { std::vector naturalC(N - 1); @@ -257,10 +244,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { const auto [ndC, lvls] = makeBinaryOrdering(ordC); std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); } - auto ordering_full = Ordering(ordering); - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); + const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering); auto new_fg = makeSwitchingChain(12); auto isam = HybridGaussianISAM(*hbt); @@ -460,12 +445,7 @@ TEST(HybridBayesTree, Optimize) { const auto [hybridBayesNet, remainingFactorGraph] = s.linearizedFactorGraph().eliminatePartialSequential(ordering); - DiscreteFactorGraph dfg; - for (auto&& f : *remainingFactorGraph) { - auto discreteFactor = dynamic_pointer_cast(f); - assert(discreteFactor); - dfg.push_back(discreteFactor); - } + DiscreteFactorGraph dfg = remainingFactorGraph->discreteFactors(); // Add the probabilities for each branch DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}}; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 05e05c79b7..9642796ab1 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -194,164 +194,6 @@ TEST(HybridGaussianFactor, Error) { 4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9); } -/* ************************************************************************* */ -namespace test_direct_factor_graph { -/** - * @brief Create a Factor Graph by directly specifying all - * the factors instead of creating conditionals first. - * This way we can directly provide the likelihoods and - * then perform linearization. - * - * @param values Initial values to linearize around. - * @param means The means of the HybridGaussianFactor components. - * @param sigmas The covariances of the HybridGaussianFactor components. - * @param m1 The discrete key. - * @return HybridGaussianFactorGraph - */ -static HybridGaussianFactorGraph CreateFactorGraph( - const gtsam::Values &values, const std::vector &means, - const std::vector &sigmas, DiscreteKey &m1, - double measurement_noise = 1e-3) { - auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); - auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); - auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise); - - auto f0 = - std::make_shared>(X(0), X(1), means[0], model0) - ->linearize(values); - auto f1 = - std::make_shared>(X(0), X(1), means[1], model1) - ->linearize(values); - - // Create HybridGaussianFactor - // We take negative since we want - // the underlying scalar to be log(\sqrt(|2πΣ|)) - std::vector factors{{f0, model0->negLogConstant()}, - {f1, model1->negLogConstant()}}; - HybridGaussianFactor motionFactor(m1, factors); - - HybridGaussianFactorGraph hfg; - hfg.push_back(motionFactor); - - hfg.push_back(PriorFactor(X(0), values.at(X(0)), prior_noise) - .linearize(values)); - - return hfg; -} -} // namespace test_direct_factor_graph - -/* ************************************************************************* */ -/** - * @brief Test components with differing means but the same covariances. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -TEST(HybridGaussianFactor, DifferentMeansFG) { - using namespace test_direct_factor_graph; - - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 0.0, x2 = 1.75; - values.insert(X(0), x1); - values.insert(X(1), x2); - - std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; - - HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1); - - { - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, - DiscreteValues{{M(1), 0}}); - - EXPECT(assert_equal(expected, actual)); - - DiscreteValues dv0{{M(1), 0}}; - VectorValues cont0 = bn->optimize(dv0); - double error0 = bn->error(HybridValues(cont0, dv0)); - // regression - EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); - - DiscreteValues dv1{{M(1), 1}}; - VectorValues cont1 = bn->optimize(dv1); - double error1 = bn->error(HybridValues(cont1, dv1)); - EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); - } - - { - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - hfg.push_back( - PriorFactor(X(1), means[1], prior_noise).linearize(values)); - - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, - DiscreteValues{{M(1), 1}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); - } - } -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances but the same means. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -TEST(HybridGaussianFactor, DifferentCovariancesFG) { - using namespace test_direct_factor_graph; - - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(0), x1); - values.insert(X(1), x2); - - std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; - - // Create FG with HybridGaussianFactor and prior on X1 - HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1); - auto hbn = fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(0), Vector1(0.0)); - cv.insert(X(1), Vector1(0.0)); - - DiscreteValues dv0{{M(1), 0}}; - DiscreteValues dv1{{M(1), 1}}; - - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); - - EXPECT(assert_equal(expected_m1, actual_m1)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 13a6cd614f..31e36101b2 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -545,7 +545,7 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { /* ****************************************************************************/ // Check that collectProductFactor works correctly. -TEST(HybridGaussianFactorGraph, collectProductFactor) { +TEST(HybridGaussianFactorGraph, CollectProductFactor) { const int num_measurements = 1; VectorValues vv{{Z(0), Vector1(5.0)}}; auto fg = tiny::createHybridGaussianFactorGraph(num_measurements, vv); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index b2e388529f..04b44f9041 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -56,7 +56,7 @@ const HybridGaussianFactorGraph graph2{lfg.at(4), lfg.at(1), lfg.at(2), /* ****************************************************************************/ // Test if we can perform elimination incrementally. -TEST(HybridGaussianElimination, IncrementalElimination) { +TEST(HybridGaussianISAM, IncrementalElimination) { using namespace switching3; HybridGaussianISAM isam; @@ -88,7 +88,7 @@ TEST(HybridGaussianElimination, IncrementalElimination) { /* ****************************************************************************/ // Test if we can incrementally do the inference -TEST(HybridGaussianElimination, IncrementalInference) { +TEST(HybridGaussianISAM, IncrementalInference) { using namespace switching3; HybridGaussianISAM isam; @@ -156,7 +156,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { /* ****************************************************************************/ // Test if we can approximately do the inference -TEST(HybridGaussianElimination, Approx_inference) { +TEST(HybridGaussianISAM, ApproxInference) { Switching switching(4); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph1; @@ -258,27 +258,26 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridGaussianElimination, IncrementalApproximate) { +TEST(HybridGaussianISAM, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; - HybridGaussianFactorGraph graph1; + HybridGaussianFactorGraph graph; /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 0; i < 3; i++) { - graph1.push_back(switching.linearBinaryFactors.at(i)); + graph.push_back(switching.linearBinaryFactors.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)); + graph.push_back(switching.linearUnaryFactors.at(i)); } // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph1); - incrementalHybrid.prune(maxComponents); + incrementalHybrid.update(graph, maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, // each with 2, 4, 8, and 5 (pruned) leaves respectively. @@ -293,13 +292,12 @@ TEST(HybridGaussianElimination, IncrementalApproximate) { 5, incrementalHybrid[X(3)]->conditional()->asHybrid()->nrComponents()); /***** Run Round 2 *****/ - HybridGaussianFactorGraph graph2; - graph2.push_back(switching.linearBinaryFactors.at(3)); // x3-x4 - graph2.push_back(switching.linearUnaryFactors.at(4)); // x4 + graph = HybridGaussianFactorGraph(); + graph.push_back(switching.linearBinaryFactors.at(3)); // hybrid x3-x4 + graph.push_back(switching.linearUnaryFactors.at(4)); // x4 // Run update with pruning a second time. - incrementalHybrid.update(graph2); - incrementalHybrid.prune(maxComponents); + incrementalHybrid.update(graph, maxComponents); // Check if we have a bayes tree with pruned hybrid nodes, // with 5 (pruned) leaves. @@ -470,8 +468,7 @@ TEST(HybridGaussianISAM, NonTrivial) { fg = HybridNonlinearFactorGraph(); // Keep pruning! - inc.update(gfg); - inc.prune(3); + inc.update(gfg, 3); // The final discrete graph should not be empty since we have eliminated // all continuous variables. diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 16a2bd476d..747a1b6883 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -124,7 +124,7 @@ std::pair approximateDiscreteMarginal( * the posterior probability of m1 should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel) { +TEST(HybridGaussianFactorGraph, TwoStateModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 0.5; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma); @@ -178,7 +178,7 @@ TEST(HybridGaussianFactor, TwoStateModel) { * the P(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel2) { +TEST(HybridGaussianFactorGraph, TwoStateModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 0.5, sigma1 = 2.0; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); @@ -198,7 +198,7 @@ TEST(HybridGaussianFactor, TwoStateModel2) { {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); + const auto &expectedDiscretePosterior = hbn.discretePosterior(vv); // Equality of posteriors asserts that the factor graph is correct (same // ratios for all modes) @@ -234,7 +234,7 @@ TEST(HybridGaussianFactor, TwoStateModel2) { {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); + const auto &expectedDiscretePosterior = hbn.discretePosterior(vv); // Equality of posteriors asserts that the factor graph is correct (same // ratios for all modes) @@ -281,7 +281,7 @@ TEST(HybridGaussianFactor, TwoStateModel2) { * the p(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel3) { +TEST(HybridGaussianFactorGraph, TwoStateModel3) { double mu = 1.0; double sigma0 = 0.5, sigma1 = 2.0; auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1); @@ -366,7 +366,7 @@ TEST(HybridGaussianFactor, TwoStateModel3) { * measurements and vastly different motion model: either stand still or move * far. This yields a very informative posterior. */ -TEST(HybridGaussianFactor, TwoStateModel4) { +TEST(HybridGaussianFactorGraph, TwoStateModel4) { double mu0 = 0.0, mu1 = 10.0; double sigma0 = 0.2, sigma1 = 5.0; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); @@ -385,6 +385,164 @@ TEST(HybridGaussianFactor, TwoStateModel4) { EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } +/* ************************************************************************* */ +namespace test_direct_factor_graph { +/** + * @brief Create a Factor Graph by directly specifying all + * the factors instead of creating conditionals first. + * This way we can directly provide the likelihoods and + * then perform linearization. + * + * @param values Initial values to linearize around. + * @param means The means of the HybridGaussianFactor components. + * @param sigmas The covariances of the HybridGaussianFactor components. + * @param m1 The discrete key. + * @return HybridGaussianFactorGraph + */ +static HybridGaussianFactorGraph CreateFactorGraph( + const gtsam::Values &values, const std::vector &means, + const std::vector &sigmas, DiscreteKey &m1, + double measurement_noise = 1e-3) { + auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); + auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); + auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise); + + auto f0 = + std::make_shared>(X(0), X(1), means[0], model0) + ->linearize(values); + auto f1 = + std::make_shared>(X(0), X(1), means[1], model1) + ->linearize(values); + + // Create HybridGaussianFactor + // We take negative since we want + // the underlying scalar to be log(\sqrt(|2πΣ|)) + std::vector factors{{f0, model0->negLogConstant()}, + {f1, model1->negLogConstant()}}; + HybridGaussianFactor motionFactor(m1, factors); + + HybridGaussianFactorGraph hfg; + hfg.push_back(motionFactor); + + hfg.push_back(PriorFactor(X(0), values.at(X(0)), prior_noise) + .linearize(values)); + + return hfg; +} +} // namespace test_direct_factor_graph + +/* ************************************************************************* */ +/** + * @brief Test components with differing means but the same covariances. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridGaussianFactorGraph, DifferentMeans) { + using namespace test_direct_factor_graph; + + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 0.0, x2 = 1.75; + values.insert(X(0), x1); + values.insert(X(1), x2); + + std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; + + HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1); + + { + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, + DiscreteValues{{M(1), 0}}); + + EXPECT(assert_equal(expected, actual)); + + DiscreteValues dv0{{M(1), 0}}; + VectorValues cont0 = bn->optimize(dv0); + double error0 = bn->error(HybridValues(cont0, dv0)); + // regression + EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); + + DiscreteValues dv1{{M(1), 1}}; + VectorValues cont1 = bn->optimize(dv1); + double error1 = bn->error(HybridValues(cont1, dv1)); + EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); + } + + { + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + hfg.push_back( + PriorFactor(X(1), means[1], prior_noise).linearize(values)); + + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, + DiscreteValues{{M(1), 1}}); + + EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); + } + } +} + +/* ************************************************************************* */ +/** + * @brief Test components with differing covariances but the same means. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridGaussianFactorGraph, DifferentCovariances) { + using namespace test_direct_factor_graph; + + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(0), x1); + values.insert(X(1), x2); + + std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; + + // Create FG with HybridGaussianFactor and prior on X1 + HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1); + auto hbn = fg.eliminateSequential(); + + VectorValues cv; + cv.insert(X(0), Vector1(0.0)); + cv.insert(X(1), Vector1(0.0)); + + DiscreteValues dv0{{M(1), 0}}; + DiscreteValues dv1{{M(1), 1}}; + + DiscreteConditional expected_m1(m1, "0.5/0.5"); + DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + + EXPECT(assert_equal(expected_m1, actual_m1)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index b22847e894..07f70e95c4 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -475,13 +475,8 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = self.linearizedFactorGraph().eliminatePartialSequential(ordering); - DiscreteFactorGraph discrete_fg; - // TODO(Varun) Make this a function of HybridGaussianFactorGraph? - for (auto &factor : (*remainingFactorGraph_partial)) { - auto df = dynamic_pointer_cast(factor); - assert(df); - discrete_fg.push_back(df); - } + DiscreteFactorGraph discrete_fg = + remainingFactorGraph_partial->discreteFactors(); ordering.clear(); for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k)); diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 6516ef1fba..67cec83199 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -49,7 +49,7 @@ using symbol_shorthand::Z; TEST(HybridNonlinearISAM, IncrementalElimination) { Switching switching(3); HybridNonlinearISAM isam; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Create initial factor graph @@ -57,17 +57,17 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { // | | | // X0 -*- X1 -*- X2 // \*-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) + graph.push_back(switching.unaryFactors.at(0)); // P(X0) + graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); initial.insert(X(2), 3); // Run update step - isam.update(graph1, initial); + isam.update(graph, initial); // Check that after update we have 3 hybrid Bayes net nodes: // P(X0 | X1, M0) and P(X1, X2 | M0, M1), P(M0, M1) @@ -80,14 +80,14 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { /********************************************************/ // New factor graph for incremental update. - HybridNonlinearFactorGraph graph2; + graph = HybridNonlinearFactorGraph(); initial = Values(); - 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) + graph.push_back(switching.unaryFactors.at(1)); // P(X1) + graph.push_back(switching.unaryFactors.at(2)); // P(X2) + graph.push_back(switching.modeChain.at(1)); // P(M0, M1) - isam.update(graph2, initial); + isam.update(graph, initial); bayesTree = isam.bayesTree(); // Check that after the second update we have @@ -103,7 +103,7 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { TEST(HybridNonlinearISAM, IncrementalInference) { Switching switching(3); HybridNonlinearISAM isam; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Create initial factor graph @@ -112,16 +112,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // X0 -*- X1 -*- X2 // | | // *-M0 - * - M1 - 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) + graph.push_back(switching.unaryFactors.at(0)); // P(X0) + graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph.push_back(switching.unaryFactors.at(1)); // P(X1) + graph.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); // Run update step - isam.update(graph1, initial); + isam.update(graph, initial); HybridGaussianISAM bayesTree = isam.bayesTree(); auto discreteConditional_m0 = bayesTree[M(0)]->conditional()->asDiscrete(); @@ -129,16 +129,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) { /********************************************************/ // New factor graph for incremental update. - HybridNonlinearFactorGraph graph2; + graph = HybridNonlinearFactorGraph(); initial = Values(); initial.insert(X(2), 3); - 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) + graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph.push_back(switching.unaryFactors.at(2)); // P(X2) + graph.push_back(switching.modeChain.at(1)); // P(M0, M1) - isam.update(graph2, initial); + isam.update(graph, initial); bayesTree = isam.bayesTree(); /********************************************************/ @@ -195,22 +195,22 @@ TEST(HybridNonlinearISAM, IncrementalInference) { } /* ****************************************************************************/ -// Test if we can approximately do the inference -TEST(HybridNonlinearISAM, Approx_inference) { +// Test if we can approximately do the inference (using pruning) +TEST(HybridNonlinearISAM, ApproxInference) { Switching switching(4); HybridNonlinearISAM incrementalHybrid; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 0; i < 3; i++) { - graph1.push_back(switching.binaryFactors.at(i)); + graph.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) for (size_t i = 0; i < 4; i++) { - graph1.push_back(switching.unaryFactors.at(i)); + graph.push_back(switching.unaryFactors.at(i)); initial.insert(X(i), i + 1); } @@ -226,7 +226,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { .BaseEliminateable::eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; - incrementalHybrid.update(graph1, initial); + incrementalHybrid.update(graph, initial); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); bayesTree.prune(maxNrLeaves); @@ -302,22 +302,22 @@ TEST(HybridNonlinearISAM, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridNonlinearISAM, Incremental_approximate) { +TEST(HybridNonlinearISAM, IncrementalApproximate) { Switching switching(5); HybridNonlinearISAM incrementalHybrid; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 0; i < 3; i++) { - graph1.push_back(switching.binaryFactors.at(i)); + graph.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) for (size_t i = 0; i < 4; i++) { - graph1.push_back(switching.unaryFactors.at(i)); + graph.push_back(switching.unaryFactors.at(i)); initial.insert(X(i), i + 1); } @@ -325,7 +325,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph1, initial); + incrementalHybrid.update(graph, initial); incrementalHybrid.prune(maxComponents); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); @@ -342,14 +342,14 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { 5, bayesTree[X(3)]->conditional()->asHybrid()->nrComponents()); /***** Run Round 2 *****/ - HybridGaussianFactorGraph graph2; - graph2.push_back(switching.binaryFactors.at(3)); // x3-x4 - graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement + graph = HybridGaussianFactorGraph(); + graph.push_back(switching.binaryFactors.at(3)); // x3-x4 + graph.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5); // Run update with pruning a second time. - incrementalHybrid.update(graph2, initial); + incrementalHybrid.update(graph, initial); incrementalHybrid.prune(maxComponents); bayesTree = incrementalHybrid.bayesTree();