diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.cpp b/gtsam/hybrid/GaussianHybridFactorGraph.cpp index 4798c6313a..62a7a6f2d4 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/GaussianHybridFactorGraph.cpp @@ -37,7 +37,8 @@ template class EliminateableFactorGraph; void GaussianHybridFactorGraph::print( const string& str, const gtsam::KeyFormatter& keyFormatter) const { - Base::print(str, keyFormatter); + std::cout << (str.empty() ? str : str + " ") << std::endl; + Base::print("", keyFormatter); factorGraph_.print("GaussianGraph", keyFormatter); } @@ -100,19 +101,20 @@ ostream& operator<<(ostream& os, } // The function type that does a single elimination step on a variable. -pair> EliminateHybrid( - const GaussianHybridFactorGraph& factors, const Ordering& ordering) { +pair> +EliminateHybrid(const GaussianHybridFactorGraph& factors, + const Ordering& ordering) { // STEP 1: SUM // Create a new decision tree with all factors gathered at leaves. auto sum = factors.sum(); // zero out all sums with null ptrs - auto zeroOut = [](const GaussianFactorGraph &gfg) { - bool hasNull = std::any_of(gfg.begin(), - gfg.end(), - [](const GaussianFactor::shared_ptr &ptr) { return !ptr; }); + auto zeroOut = [](const GaussianFactorGraph& gfg) { + bool hasNull = + std::any_of(gfg.begin(), gfg.end(), + [](const GaussianFactor::shared_ptr& ptr) { return !ptr; }); - return hasNull?GaussianFactorGraph():gfg; + return hasNull ? GaussianFactorGraph() : gfg; }; // TODO(fan): Now let's assume that all continuous will be eliminated first! @@ -123,8 +125,8 @@ pair> EliminateHybrid dfg.push_back(factors.discreteGraph()); auto dbn = EliminateForMPE(dfg, ordering); - auto &df = dbn.first; - auto &newFactor = dbn.second; + auto& df = dbn.first; + auto& newFactor = dbn.second; return {df, newFactor}; } @@ -140,17 +142,17 @@ pair> EliminateHybrid KeyVector keysOfEliminated; // Not the ordering KeyVector keysOfSeparator; // TODO(frank): Is this just (keys - ordering)? - auto eliminate = - [&](const GaussianFactorGraph &graph) -> GaussianFactorGraph::EliminationResult { - if (graph.empty()) return {nullptr, nullptr}; - auto result = EliminatePreferCholesky(graph, ordering); - if (keysOfEliminated.empty()) - keysOfEliminated = - result.first->keys(); // Initialize the keysOfEliminated to be the - // keysOfEliminated of the GaussianConditional - if (keysOfSeparator.empty()) keysOfSeparator = result.second->keys(); - return result; - }; + auto eliminate = [&](const GaussianFactorGraph& graph) + -> GaussianFactorGraph::EliminationResult { + if (graph.empty()) return {nullptr, nullptr}; + auto result = EliminatePreferCholesky(graph, ordering); + if (keysOfEliminated.empty()) + keysOfEliminated = + result.first->keys(); // Initialize the keysOfEliminated to be the + // keysOfEliminated of the GaussianConditional + if (keysOfSeparator.empty()) keysOfSeparator = result.second->keys(); + return result; + }; DecisionTree eliminationResults(sum, eliminate); // STEP 3: Create result diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 9a5a0086ff..a0684641cd 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -188,6 +188,17 @@ class HybridFactorGraph : public FactorGraph { /// The total number of factors in the DC factor graph. size_t nrDcFactors() const { return dcGraph_.size(); } + /** Directly resize the number of factors in the graph. If the new size is + * less than the original, factors at the end will be removed. If the new + * size is larger than the original, null factors will be appended. + */ + void resize(size_t size) override { + Base::resize(size); + factorGraph_.resize(size); + discreteGraph_.resize(size); + dcGraph_.resize(size); + } + /// Get all the discrete keys in the hybrid factor graph. virtual DiscreteKeys discreteKeys() const { DiscreteKeys result; diff --git a/gtsam/hybrid/IncrementalHybrid.cpp b/gtsam/hybrid/IncrementalHybrid.cpp index 7499d316bb..463380d580 100644 --- a/gtsam/hybrid/IncrementalHybrid.cpp +++ b/gtsam/hybrid/IncrementalHybrid.cpp @@ -18,8 +18,9 @@ */ #include -#include + #include +#include void gtsam::IncrementalHybrid::update(gtsam::GaussianHybridFactorGraph graph, const gtsam::Ordering &ordering, @@ -32,23 +33,30 @@ void gtsam::IncrementalHybrid::update(gtsam::GaussianHybridFactorGraph graph, for (auto &&conditional : *hybridBayesNet_) { for (auto &key : conditional->frontals()) { if (allVars.find(key) != allVars.end()) { - if (auto - gf = boost::dynamic_pointer_cast(conditional)) { + if (auto gf = + boost::dynamic_pointer_cast(conditional)) { graph.push_back(gf); - } else if (auto df = - boost::dynamic_pointer_cast(conditional)) { + } else if (auto df = boost::dynamic_pointer_cast( + conditional)) { graph.push_back(df); } break; } } } + } else { + // Initialize an empty HybridBayesNet + hybridBayesNet_ = boost::make_shared(); } // Eliminate partially. - std::tie(hybridBayesNet_, remainingFactorGraph_) = + HybridBayesNet::shared_ptr bayesNetFragment; + std::tie(bayesNetFragment, remainingFactorGraph_) = graph.eliminatePartialSequential(ordering); + // Add the partial bayes net to the posterior bayes net. + hybridBayesNet_->push_back(*bayesNetFragment); + // Prune if (maxNrLeaves) { const auto N = *maxNrLeaves; @@ -62,17 +70,15 @@ void gtsam::IncrementalHybrid::update(gtsam::GaussianHybridFactorGraph graph, // Let's assume that the structure of the last discrete density will be the // same as the last continuous std::vector probabilities; - // TODO(fan): The number of probabilities can be lower than the actual number of choices - discreteFactor->visit([&](const double &prob) { - probabilities.emplace_back(prob); - }); + // TODO(fan): The number of probabilities can be lower than the actual + // number of choices + discreteFactor->visit( + [&](const double &prob) { probabilities.emplace_back(prob); }); if (probabilities.size() < N) return; - std::nth_element(probabilities.begin(), - probabilities.begin() + N, - probabilities.end(), - std::greater{}); + std::nth_element(probabilities.begin(), probabilities.begin() + N, + probabilities.end(), std::greater{}); auto thresholdValue = probabilities[N - 1]; @@ -83,14 +89,16 @@ void gtsam::IncrementalHybrid::update(gtsam::GaussianHybridFactorGraph graph, DecisionTree thresholded(*discreteFactor, threshold); // Create a new factor with pruned tree - // DecisionTreeFactor newFactor(discreteFactor->discreteKeys(), thresholded); + // DecisionTreeFactor newFactor(discreteFactor->discreteKeys(), + // thresholded); discreteFactor->root_ = thresholded.root_; - std::vector> assignments = discreteFactor->enumerate(); + std::vector> assignments = + discreteFactor->enumerate(); // Loop over all assignments and create a vector of GaussianConditionals std::vector prunedConditionals; - for (auto && av : assignments) { + for (auto &&av : assignments) { const DiscreteValues &assignment = av.first; const double value = av.second; @@ -101,11 +109,10 @@ void gtsam::IncrementalHybrid::update(gtsam::GaussianHybridFactorGraph graph, } } - GaussianMixture::Factors prunedConditionalsTree( - lastDensity->discreteKeys(), - prunedConditionals - ); + GaussianMixture::Factors prunedConditionalsTree(lastDensity->discreteKeys(), + prunedConditionals); - hybridBayesNet_->atGaussian(hybridBayesNet_->size() - 1)->factors_ = prunedConditionalsTree; + hybridBayesNet_->atGaussian(hybridBayesNet_->size() - 1)->factors_ = + prunedConditionalsTree; } } diff --git a/gtsam/hybrid/NonlinearHybridFactorGraph.cpp b/gtsam/hybrid/NonlinearHybridFactorGraph.cpp index 130211c053..6cc2706deb 100644 --- a/gtsam/hybrid/NonlinearHybridFactorGraph.cpp +++ b/gtsam/hybrid/NonlinearHybridFactorGraph.cpp @@ -21,9 +21,11 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ void NonlinearHybridFactorGraph::print( const string& str, const gtsam::KeyFormatter& keyFormatter) const { - Base::print(str, keyFormatter); + std::cout << (str.empty() ? str : str + " ") << std::endl; + Base::print("", keyFormatter); factorGraph_.print("NonlinearFactorGraph", keyFormatter); } @@ -32,6 +34,7 @@ bool NonlinearHybridFactorGraph::equals(const NonlinearHybridFactorGraph& other, return Base::equals(other, tol); } +/* ************************************************************************* */ GaussianHybridFactorGraph NonlinearHybridFactorGraph::linearize( const Values& continuousValues) const { // linearize the continuous factors diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index f594dedbb3..98e6889527 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -18,8 +18,6 @@ * @date December 2021 */ -#include "Switching.h" - #include #include #include @@ -35,6 +33,8 @@ #include +#include "Switching.h" + // Include for test suite #include @@ -74,6 +74,75 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { EXPECT_LONGS_EQUAL(2, dcmfg.gaussianGraph().size()); } +/* ************************************************************************** */ +/// Test that the resize method works correctly for a +/// NonlinearHybridFactorGraph. +TEST(NonlinearHybridFactorGraph, Resize) { + NonlinearHybridFactorGraph fg; + auto nonlinearFactor = boost::make_shared>(); + fg.push_back(nonlinearFactor); + + auto discreteFactor = boost::make_shared(); + fg.push_back(discreteFactor); + + auto dcFactor = boost::make_shared>(); + fg.push_back(dcFactor); + + EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 1); + EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 1); + EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 1); + + EXPECT_LONGS_EQUAL(fg.size(), 3); + + fg.resize(0); + EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); + EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0); + EXPECT_LONGS_EQUAL(fg.nonlinearGraph().size(), 0); + + EXPECT_LONGS_EQUAL(fg.size(), 0); +} + +/* ************************************************************************** */ +/// Test that the resize method works correctly for a +/// GaussianHybridFactorGraph. +TEST(GaussianHybridFactorGraph, Resize) { + NonlinearHybridFactorGraph nhfg; + auto nonlinearFactor = boost::make_shared>( + X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); + nhfg.push_back(nonlinearFactor); + auto discreteFactor = boost::make_shared(); + nhfg.push_back(discreteFactor); + + KeyVector contKeys = {X(0), X(1)}; + auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); + auto still = boost::make_shared(X(0), X(1), 0.0, noise_model), + moving = boost::make_shared(X(0), X(1), 1.0, noise_model); + std::vector components = {still, moving}; + auto dcFactor = boost::make_shared>( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + nhfg.push_back(dcFactor); + + Values linearizationPoint; + linearizationPoint.insert(X(0), 0); + linearizationPoint.insert(X(1), 1); + + // Generate `GaussianHybridFactorGraph` by linearizing + GaussianHybridFactorGraph fg = nhfg.linearize(linearizationPoint); + + EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 1); + EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 1); + EXPECT_LONGS_EQUAL(fg.gaussianGraph().size(), 1); + + EXPECT_LONGS_EQUAL(fg.size(), 3); + + fg.resize(0); + EXPECT_LONGS_EQUAL(fg.dcGraph().size(), 0); + EXPECT_LONGS_EQUAL(fg.discreteGraph().size(), 0); + EXPECT_LONGS_EQUAL(fg.gaussianGraph().size(), 0); + + EXPECT_LONGS_EQUAL(fg.size(), 0); +} + /* **************************************************************************** * Test push_back on HFG makes the correct distinction. */ @@ -278,7 +347,8 @@ TEST(DCGaussianElimination, EliminateHybrid_2_Variable) { std::tie(abstractConditionalMixture, factorOnModes) = EliminateHybrid(factors, ordering); - auto gaussianConditionalMixture = dynamic_pointer_cast(abstractConditionalMixture); + auto gaussianConditionalMixture = + dynamic_pointer_cast(abstractConditionalMixture); CHECK(gaussianConditionalMixture); EXPECT_LONGS_EQUAL( @@ -367,11 +437,11 @@ TEST_UNSAFE(HybridFactorGraph, Partial_Elimination) { // GTSAM_PRINT(*remainingFactorGraph); // HybridFactorGraph EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size()); EXPECT(remainingFactorGraph->discreteGraph().at(0)->keys() == - KeyVector({M(1)})); + KeyVector({M(1)})); EXPECT(remainingFactorGraph->discreteGraph().at(1)->keys() == - KeyVector({M(2), M(1)})); + KeyVector({M(2), M(1)})); EXPECT(remainingFactorGraph->discreteGraph().at(2)->keys() == - KeyVector({M(2), M(1)})); + KeyVector({M(2), M(1)})); } /* ****************************************************************************/ @@ -425,13 +495,13 @@ TEST_UNSAFE(HybridFactorGraph, Full_Elimination) { // P(m1 | m2) EXPECT(hybridBayesNet->at(3)->frontals() == KeyVector{M(1)}); EXPECT(hybridBayesNet->at(3)->parents() == KeyVector({M(2)})); - EXPECT(dynamic_pointer_cast(hybridBayesNet->at(3))->equals( - *discreteBayesNet.at(0))); + EXPECT(dynamic_pointer_cast(hybridBayesNet->at(3)) + ->equals(*discreteBayesNet.at(0))); // P(m2) EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(2)}); EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents()); - EXPECT(dynamic_pointer_cast(hybridBayesNet->at(4))->equals( - *discreteBayesNet.at(1))); + EXPECT(dynamic_pointer_cast(hybridBayesNet->at(4)) + ->equals(*discreteBayesNet.at(1))); } /* ****************************************************************************/ @@ -452,20 +522,28 @@ TEST(HybridFactorGraph, Printing) { linearizedFactorGraph.eliminatePartialSequential(ordering); string expected_hybridFactorGraph = - "size: 8\nDiscreteFactorGraph\n" + "\nsize: 8\nDiscreteFactorGraph\n" "size: 2\nfactor 0: P( m1 ):\n" " Leaf 0.5\n\nfactor 1: P( m2 | m1 ):\n" " Choice(m2) \n 0 Choice(m1) \n 0 0 Leaf 0.3333\n 0 1 Leaf 0.6\n" " 1 Choice(m1) \n 1 0 Leaf 0.6667\n 1 1 Leaf 0.4\n\nDCFactorGraph \n" - "size: 2\nfactor 0: [ x1 x2; m1 ]{\n Choice(m1) \n 0 Leaf Jacobian factor on 2 keys: \n" - " A[x1] = [\n\t-1\n]\n A[x2] = [\n\t1\n]\n b = [ -1 ]\n No noise model\n\n\n" - " 1 Leaf Jacobian factor on 2 keys: \n A[x1] = [\n\t-1\n]\n A[x2] = [\n\t1\n]\n b = [ -0 ]\n" + "size: 2\nfactor 0: [ x1 x2; m1 ]{\n Choice(m1) \n 0 Leaf Jacobian " + "factor on 2 keys: \n" + " A[x1] = [\n\t-1\n]\n A[x2] = [\n\t1\n]\n b = [ -1 ]\n No noise " + "model\n\n\n" + " 1 Leaf Jacobian factor on 2 keys: \n A[x1] = [\n\t-1\n]\n A[x2] = " + "[\n\t1\n]\n b = [ -0 ]\n" " No noise model\n\n\n}\nfactor 1: [ x2 x3; m2 ]{\n Choice(m2) \n" - " 0 Leaf Jacobian factor on 2 keys: \n A[x2] = [\n\t-1\n]\n A[x3] = [\n\t1\n]\n b = [ -1 ]\n" - " No noise model\n\n\n 1 Leaf Jacobian factor on 2 keys: \n A[x2] = [\n\t-1\n]\n A[x3] = [\n\t1\n]\n" - " b = [ -0 ]\n No noise model\n\n\n}\nGaussianGraph \nsize: 4\nfactor 0: \n A[x1] = [\n\t10\n]\n" - " b = [ -10 ]\n No noise model\nfactor 1: \n A[x1] = [\n\t10\n]\n b = [ -10 ]\n No noise model\n" - "factor 2: \n A[x2] = [\n\t10\n]\n b = [ -10 ]\n No noise model\nfactor 3: \n A[x3] = [\n\t10\n]\n" + " 0 Leaf Jacobian factor on 2 keys: \n A[x2] = [\n\t-1\n]\n A[x3] = " + "[\n\t1\n]\n b = [ -1 ]\n" + " No noise model\n\n\n 1 Leaf Jacobian factor on 2 keys: \n A[x2] = " + "[\n\t-1\n]\n A[x3] = [\n\t1\n]\n" + " b = [ -0 ]\n No noise model\n\n\n}\nGaussianGraph \nsize: 4\nfactor " + "0: \n A[x1] = [\n\t10\n]\n" + " b = [ -10 ]\n No noise model\nfactor 1: \n A[x1] = [\n\t10\n]\n b " + "= [ -10 ]\n No noise model\n" + "factor 2: \n A[x2] = [\n\t10\n]\n b = [ -10 ]\n No noise " + "model\nfactor 3: \n A[x3] = [\n\t10\n]\n" " b = [ -10 ]\n No noise model\n"; EXPECT(assert_print_equal(expected_hybridFactorGraph, linearizedFactorGraph)); diff --git a/gtsam/hybrid/tests/testIncrementalHybrid.cpp b/gtsam/hybrid/tests/testIncrementalHybrid.cpp index fe96149666..637a1fa63a 100644 --- a/gtsam/hybrid/tests/testIncrementalHybrid.cpp +++ b/gtsam/hybrid/tests/testIncrementalHybrid.cpp @@ -17,8 +17,6 @@ * @date Jan 2021 */ -#include "Switching.h" - #include #include #include @@ -30,6 +28,8 @@ #include +#include "Switching.h" + // Include for test suite #include @@ -91,11 +91,11 @@ TEST_UNSAFE(DCGaussianElimination, Incremental_inference) { auto hybridBayesNet2 = incrementalHybrid.hybridBayesNet_; CHECK(hybridBayesNet2); - EXPECT_LONGS_EQUAL(2, hybridBayesNet2->size()); - EXPECT(hybridBayesNet2->at(0)->frontals() == KeyVector{X(2)}); - EXPECT(hybridBayesNet2->at(0)->parents() == KeyVector({X(3), M(2), M(1)})); - EXPECT(hybridBayesNet2->at(1)->frontals() == KeyVector{X(3)}); - EXPECT(hybridBayesNet2->at(1)->parents() == KeyVector({M(2), M(1)})); + EXPECT_LONGS_EQUAL(4, hybridBayesNet2->size()); + EXPECT(hybridBayesNet2->at(2)->frontals() == KeyVector{X(2)}); + EXPECT(hybridBayesNet2->at(2)->parents() == KeyVector({X(3), M(2), M(1)})); + EXPECT(hybridBayesNet2->at(3)->frontals() == KeyVector{X(3)}); + EXPECT(hybridBayesNet2->at(3)->parents() == KeyVector({M(2), M(1)})); auto remainingFactorGraph2 = incrementalHybrid.remainingFactorGraph_; CHECK(remainingFactorGraph2); @@ -117,16 +117,15 @@ TEST_UNSAFE(DCGaussianElimination, Incremental_inference) { switching.linearizedFactorGraph.eliminatePartialSequential(ordering); // The densities on X(1) should be the same - EXPECT( - assert_equal(*(hybridBayesNet->atGaussian(0)), - *(expectedHybridBayesNet->atGaussian(0)))); + EXPECT(assert_equal(*(hybridBayesNet->atGaussian(0)), + *(expectedHybridBayesNet->atGaussian(0)))); // The densities on X(2) should be the same - EXPECT(assert_equal(*(hybridBayesNet2->atGaussian(0)), + EXPECT(assert_equal(*(hybridBayesNet2->atGaussian(2)), *(expectedHybridBayesNet->atGaussian(1)))); // The densities on X(3) should be the same - EXPECT(assert_equal(*(hybridBayesNet2->atGaussian(1)), + EXPECT(assert_equal(*(hybridBayesNet2->atGaussian(3)), *(expectedHybridBayesNet->atGaussian(2)))); // we only do the manual continuous elimination for 0,0 @@ -236,7 +235,7 @@ TEST(DCGaussianElimination, Approx_inference) { EXPECT(discreteFactor_m1.keys() == KeyVector({M(3), M(2), M(1)})); // Check number of elements equal to zero - auto count = [](const double& value, int count) { + auto count = [](const double &value, int count) { return value > 0 ? count + 1 : count; }; EXPECT_LONGS_EQUAL(5, discreteFactor_m1.fold(count, 0)); @@ -246,7 +245,7 @@ TEST(DCGaussianElimination, Approx_inference) { * factor 1: [x2 | x3 m2 m1 ], 4 components * factor 2: [x3 | x4 m3 m2 m1 ], 8 components * factor 3: [x4 | m3 m2 m1 ], 8 components - */ + */ auto hybridBayesNet = incrementalHybrid.hybridBayesNet_; CHECK(hybridBayesNet); @@ -258,8 +257,8 @@ TEST(DCGaussianElimination, Approx_inference) { auto &lastDensity = *(hybridBayesNet->atGaussian(3)); auto &unprunedLastDensity = *(unprunedHybridBayesNet->atGaussian(3)); - std::vector> - assignments = discreteFactor_m1.enumerate(); + std::vector> assignments = + discreteFactor_m1.enumerate(); // Loop over all assignments and check the pruned components for (auto &&av : assignments) { const DiscreteValues &assignment = av.first; @@ -326,7 +325,6 @@ TEST_UNSAFE(DCGaussianElimination, Incremental_approximate) { EXPECT_LONGS_EQUAL(5, actualBayesNet.atGaussian(1)->nrComponents()); } - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index afea63da84..101134c838 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -361,7 +361,7 @@ class FactorGraph { * less than the original, factors at the end will be removed. If the new * size is larger than the original, null factors will be appended. */ - void resize(size_t size) { factors_.resize(size); } + virtual void resize(size_t size) { factors_.resize(size); } /** delete factor without re-arranging indexes by inserting a nullptr pointer */