diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 0aa4af2e1..34ca8ab7f 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -26,74 +26,105 @@ #include #include +#include +#include // for cout :-( #include -#include // for cout :-( +#include namespace gtsam { - /* ************************************************************************* */ - template - void FactorGraph::print(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i]) - factors_[i]->print(ss.str(), formatter); +/* ************************************************************************* */ +template +void FactorGraph::print(const std::string& s, + const KeyFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i]) factors_[i]->print(ss.str(), formatter); + } +} + +/* ************************************************************************* */ +template +bool FactorGraph::equals(const This& fg, double tol) const { + // check whether the two factor graphs have the same number of factors. + if (factors_.size() != fg.size()) return false; + + // check whether the factors are the same, in same order. + for (size_t i = 0; i < factors_.size(); i++) { + sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; + if (f1 == NULL && f2 == NULL) continue; + if (f1 == NULL || f2 == NULL) return false; + if (!f1->equals(*f2, tol)) return false; + } + return true; +} + +/* ************************************************************************* */ +template +size_t FactorGraph::nrFactors() const { + size_t size_ = 0; + for (const sharedFactor& factor : factors_) + if (factor) size_++; + return size_; +} + +/* ************************************************************************* */ +template +KeySet FactorGraph::keys() const { + KeySet keys; + for (const sharedFactor& factor : this->factors_) { + if (factor) keys.insert(factor->begin(), factor->end()); + } + return keys; +} + +/* ************************************************************************* */ +template +KeyVector FactorGraph::keyVector() const { + KeyVector keys; + keys.reserve(2 * size()); // guess at size + for (const sharedFactor& factor : factors_) + if (factor) keys.insert(keys.end(), factor->begin(), factor->end()); + std::sort(keys.begin(), keys.end()); + auto last = std::unique(keys.begin(), keys.end()); + keys.erase(last, keys.end()); + return keys; +} + +/* ************************************************************************* */ +template +template +FactorIndices FactorGraph::add_factors(const CONTAINER& factors, + bool useEmptySlots) { + const size_t num_factors = factors.size(); + FactorIndices newFactorIndices(num_factors); + if (useEmptySlots) { + size_t i = 0; + for (size_t j = 0; j < num_factors; ++j) { + // Loop to find the next available factor slot + do { + if (i >= size()) + // Make room for remaining factors, happens only once. + resize(size() + num_factors - j); + else if (at(i)) + ++i; // Move on to the next slot or past end. + else + break; // We found an empty slot, break to fill it. + } while (true); + + // Use the current slot, updating graph and newFactorSlots. + at(i) = factors[j]; + newFactorIndices[j] = i; } + } else { + // We're not looking for unused slots, so just add the factors at the end. + for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size(); + push_back(factors); } + return newFactorIndices; +} - /* ************************************************************************* */ - template - bool FactorGraph::equals(const This& fg, double tol) const { - /** check whether the two factor graphs have the same number of factors_ */ - if (factors_.size() != fg.size()) return false; - - /** check whether the factors_ are the same */ - for (size_t i = 0; i < factors_.size(); i++) { - // TODO: Doesn't this force order of factor insertion? - sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; - if (!f1->equals(*f2, tol)) return false; - } - return true; - } - - /* ************************************************************************* */ - template - size_t FactorGraph::nrFactors() const { - size_t size_ = 0; - for(const sharedFactor& factor: factors_) - if (factor) size_++; - return size_; - } - - /* ************************************************************************* */ - template - KeySet FactorGraph::keys() const { - KeySet keys; - for(const sharedFactor& factor: this->factors_) { - if(factor) - keys.insert(factor->begin(), factor->end()); - } - return keys; - } - - /* ************************************************************************* */ - template - KeyVector FactorGraph::keyVector() const { - KeyVector keys; - keys.reserve(2 * size()); // guess at size - for (const sharedFactor& factor: factors_) - if (factor) - keys.insert(keys.end(), factor->begin(), factor->end()); - std::sort(keys.begin(), keys.end()); - auto last = std::unique(keys.begin(), keys.end()); - keys.erase(last, keys.end()); - return keys; - } - - /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index ed14f1863..0959989f9 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -273,6 +273,14 @@ class FactorGraph { bayesTree.addFactorsToGraph(*this); } + /** + * Add new factors to a factor graph and returns a list of new factor indices, + * optionally finding and reusing empty factor slots. + */ + template > + FactorIndices add_factors(const CONTAINER& factors, + bool useEmptySlots = false); + /// @} /// @name Testable /// @{ diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 3fd318456..8f4eb3c08 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -15,14 +15,16 @@ * @author Christian Potthast **/ -#include - #include + +#include #include #include #include #include +#include + #include using namespace std; @@ -46,11 +48,10 @@ TEST(SymbolicFactorGraph, keys2) { } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminateFullSequential) -{ +TEST(SymbolicFactorGraph, eliminateFullSequential) { // Test with simpleTestGraph1 Ordering order; - order += 0,1,2,3,4; + order += 0, 1, 2, 3, 4; SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order); EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1)); @@ -60,24 +61,20 @@ TEST(SymbolicFactorGraph, eliminateFullSequential) } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminatePartialSequential) -{ +TEST(SymbolicFactorGraph, eliminatePartialSequential) { // Eliminate 0 and 1 const Ordering order = list_of(0)(1); - const SymbolicBayesNet expectedBayesNet = list_of - (SymbolicConditional(0,1,2)) - (SymbolicConditional(1,2,3,4)); + const SymbolicBayesNet expectedBayesNet = + list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4)); - const SymbolicFactorGraph expectedSfg = list_of - (SymbolicFactor(2,3)) - (SymbolicFactor(4,5)) - (SymbolicFactor(2,3,4)); + const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))( + SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4)); SymbolicBayesNet::shared_ptr actualBayesNet; SymbolicFactorGraph::shared_ptr actualSfg; boost::tie(actualBayesNet, actualSfg) = - simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1))); + simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1))); EXPECT(assert_equal(expectedSfg, *actualSfg)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet)); @@ -85,75 +82,71 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) SymbolicBayesNet::shared_ptr actualBayesNet2; SymbolicFactorGraph::shared_ptr actualSfg2; boost::tie(actualBayesNet2, actualSfg2) = - simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container()); + simpleTestGraph2.eliminatePartialSequential( + list_of(0)(1).convert_to_container()); EXPECT(assert_equal(expectedSfg, *actualSfg2)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2)); } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminateFullMultifrontal) -{ - Ordering ordering; ordering += 0,1,2,3; - SymbolicBayesTree actual1 = - *simpleChain.eliminateMultifrontal(ordering); +TEST(SymbolicFactorGraph, eliminateFullMultifrontal) { + Ordering ordering; + ordering += 0, 1, 2, 3; + SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering); EXPECT(assert_equal(simpleChainBayesTree, actual1)); - SymbolicBayesTree actual2 = - *asiaGraph.eliminateMultifrontal(asiaOrdering); + SymbolicBayesTree actual2 = *asiaGraph.eliminateMultifrontal(asiaOrdering); EXPECT(assert_equal(asiaBayesTree, actual2)); } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) -{ +TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree expectedBayesTree; - SymbolicConditional::shared_ptr root = boost::make_shared( - SymbolicConditional::FromKeys(list_of(4)(5)(1), 2)); - expectedBayesTree.insertRoot(boost::make_shared(root)); + SymbolicConditional::shared_ptr root = + boost::make_shared( + SymbolicConditional::FromKeys(list_of(4)(5)(1), 2)); + expectedBayesTree.insertRoot( + boost::make_shared(root)); - SymbolicFactorGraph expectedFactorGraph = list_of - (SymbolicFactor(0,1)) - (SymbolicFactor(0,2)) - (SymbolicFactor(1,3)) - (SymbolicFactor(2,3)) - (SymbolicFactor(1)); + SymbolicFactorGraph expectedFactorGraph = + list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))( + SymbolicFactor(2, 3))(SymbolicFactor(1)); SymbolicBayesTree::shared_ptr actualBayesTree; SymbolicFactorGraph::shared_ptr actualFactorGraph; boost::tie(actualBayesTree, actualFactorGraph) = - simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5))); + simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5))); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph)); EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); SymbolicBayesTree expectedBayesTree2; - SymbolicBayesTreeClique::shared_ptr root2 = boost::make_shared( - boost::make_shared(4,1)); + SymbolicBayesTreeClique::shared_ptr root2 = + boost::make_shared( + boost::make_shared(4, 1)); root2->children.push_back(boost::make_shared( - boost::make_shared(5,4))); + boost::make_shared(5, 4))); expectedBayesTree2.insertRoot(root2); SymbolicBayesTree::shared_ptr actualBayesTree2; SymbolicFactorGraph::shared_ptr actualFactorGraph2; boost::tie(actualBayesTree2, actualFactorGraph2) = - simpleTestGraph2.eliminatePartialMultifrontal(list_of(4)(5).convert_to_container()); + simpleTestGraph2.eliminatePartialMultifrontal( + list_of(4)(5).convert_to_container()); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2)); EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2)); } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) -{ - SymbolicBayesNet expectedBayesNet = list_of - (SymbolicConditional(0, 1, 2)) - (SymbolicConditional(1, 2, 3)) - (SymbolicConditional(2, 3)) - (SymbolicConditional(3)); +TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { + SymbolicBayesNet expectedBayesNet = + list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))( + SymbolicConditional(2, 3))(SymbolicConditional(3)); SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( - Ordering(list_of(0)(1)(2)(3))); + Ordering(list_of(0)(1)(2)(3))); EXPECT(assert_equal(expectedBayesNet, actual1)); } @@ -167,104 +160,75 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { // create expected Chordal bayes Net SymbolicBayesNet expected; - expected.push_back(boost::make_shared(0,1,2)); - expected.push_back(boost::make_shared(1,2)); + expected.push_back(boost::make_shared(0, 1, 2)); + expected.push_back(boost::make_shared(1, 2)); expected.push_back(boost::make_shared(2)); - expected.push_back(boost::make_shared(3,4)); + expected.push_back(boost::make_shared(3, 4)); expected.push_back(boost::make_shared(4)); Ordering order; - order += 0,1,2,3,4; + order += 0, 1, 2, 3, 4; SymbolicBayesNet actual = *fg.eliminateSequential(order); - EXPECT(assert_equal(expected,actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -//TEST(SymbolicFactorGraph, marginals) -//{ -// // Create factor graph -// SymbolicFactorGraph fg; -// fg.push_factor(0, 1); -// fg.push_factor(0, 2); -// fg.push_factor(1, 4); -// fg.push_factor(2, 4); -// fg.push_factor(3, 4); -// -// // eliminate -// SymbolicSequentialSolver solver(fg); -// SymbolicBayesNet::shared_ptr actual = solver.eliminate(); -// SymbolicBayesNet expected; -// expected.push_front(boost::make_shared(4)); -// expected.push_front(boost::make_shared(3, 4)); -// expected.push_front(boost::make_shared(2, 4)); -// expected.push_front(boost::make_shared(1, 2, 4)); -// expected.push_front(boost::make_shared(0, 1, 2)); -// EXPECT(assert_equal(expected,*actual)); -// -// { -// // jointBayesNet -// vector js; -// js.push_back(0); -// js.push_back(4); -// js.push_back(3); -// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); -// SymbolicBayesNet expectedBN; -// expectedBN.push_front(boost::make_shared(3)); -// expectedBN.push_front(boost::make_shared(4, 3)); -// expectedBN.push_front(boost::make_shared(0, 4)); -// EXPECT( assert_equal(expectedBN,*actualBN)); -// -// // jointFactorGraph -// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); -// SymbolicFactorGraph expectedFG; -// expectedFG.push_factor(0, 4); -// expectedFG.push_factor(4, 3); -// expectedFG.push_factor(3); -// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); -// } -// -// { -// // jointBayesNet -// vector js; -// js.push_back(0); -// js.push_back(2); -// js.push_back(3); -// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); -// SymbolicBayesNet expectedBN; -// expectedBN.push_front(boost::make_shared(2)); -// expectedBN.push_front(boost::make_shared(3, 2)); -// expectedBN.push_front(boost::make_shared(0, 3, 2)); -// EXPECT( assert_equal(expectedBN,*actualBN)); -// -// // jointFactorGraph -// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); -// SymbolicFactorGraph expectedFG; -// expectedFG.push_factor(0, 3, 2); -// expectedFG.push_factor(3, 2); -// expectedFG.push_factor(2); -// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); -// } -// -// { -// // conditionalBayesNet -// vector js; -// js.push_back(0); -// js.push_back(2); -// js.push_back(3); -// size_t nrFrontals = 2; -// SymbolicBayesNet::shared_ptr actualBN = // -// solver.conditionalBayesNet(js, nrFrontals); -// SymbolicBayesNet expectedBN; -// expectedBN.push_front(boost::make_shared(2, 3)); -// expectedBN.push_front(boost::make_shared(0, 2, 3)); -// EXPECT( assert_equal(expectedBN,*actualBN)); -// } -//} +TEST(SymbolicFactorGraph, marginals) { + // Create factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + // eliminate + Ordering ord(list_of(3)(4)(2)(1)(0)); + auto actual = fg.eliminateSequential(ord); + SymbolicBayesNet expected; + expected.emplace_shared(3, 4); + expected.emplace_shared(4, 1, 2); + expected.emplace_shared(2, 0, 1); + expected.emplace_shared(1, 0); + expected.emplace_shared(0); + EXPECT(assert_equal(expected, *actual)); + + { + // jointBayesNet + Ordering ord(list_of(0)(4)(3)); + auto actual = fg.eliminatePartialSequential(ord); + SymbolicBayesNet expectedBN; + expectedBN.emplace_shared(0, 1, 2); + expectedBN.emplace_shared(4, 1, 2, 3); + expectedBN.emplace_shared(3, 1, 2); + EXPECT(assert_equal(expectedBN, *(actual.first))); + } + + { + // jointBayesNet + Ordering ord(list_of(0)(2)(3)); + auto actual = fg.eliminatePartialSequential(ord); + SymbolicBayesNet expectedBN; + expectedBN.emplace_shared(0, 1, 2); + expectedBN.emplace_shared(2, 1, 4); + expectedBN.emplace_shared(3, 4); + EXPECT(assert_equal(expectedBN, *(actual.first))); + } + + { + // conditionalBayesNet + Ordering ord(list_of(0)(2)); + auto actual = fg.eliminatePartialSequential(ord); + SymbolicBayesNet expectedBN; + expectedBN.emplace_shared(0, 1, 2); + expectedBN.emplace_shared(2, 1, 4); + EXPECT(assert_equal(expectedBN, *(actual.first))); + } +} /* ************************************************************************* */ -TEST( SymbolicFactorGraph, constructFromBayesNet ) -{ +TEST(SymbolicFactorGraph, constructFromBayesNet) { // create expected factor graph SymbolicFactorGraph expected; expected.push_factor(0, 1, 2); @@ -284,8 +248,7 @@ TEST( SymbolicFactorGraph, constructFromBayesNet ) } /* ************************************************************************* */ -TEST( SymbolicFactorGraph, constructFromBayesTree ) -{ +TEST(SymbolicFactorGraph, constructFromBayesTree) { // create expected factor graph SymbolicFactorGraph expected; expected.push_factor(_E_, _L_, _B_); @@ -300,8 +263,7 @@ TEST( SymbolicFactorGraph, constructFromBayesTree ) } /* ************************************************************************* */ -TEST( SymbolicFactorGraph, push_back ) -{ +TEST(SymbolicFactorGraph, push_back) { // Create two factor graphs and expected combined graph SymbolicFactorGraph fg1, fg2, expected; @@ -321,8 +283,47 @@ TEST( SymbolicFactorGraph, push_back ) actual.push_back(fg1); actual.push_back(fg2); CHECK(assert_equal(expected, actual)); + + // combine in second way + SymbolicFactorGraph actual2 = fg1; + actual2.push_back(fg2); + CHECK(assert_equal(expected, actual2)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +TEST(SymbolicFactorGraph, add_factors) { + SymbolicFactorGraph fg1; + fg1.push_factor(10); + fg1 += SymbolicFactor::shared_ptr(); // empty slot! + fg1.push_factor(11); + + SymbolicFactorGraph fg2; + fg2.push_factor(1); + fg2.push_factor(2); + + SymbolicFactorGraph expected; + expected.push_factor(10); + expected.push_factor(1); + expected.push_factor(11); + expected.push_factor(2); + const FactorIndices expectedIndices = list_of(1)(3); + const FactorIndices actualIndices = fg1.add_factors(fg2, true); + + EXPECT(assert_equal(expected, fg1)); + EXPECT(assert_container_equality(expectedIndices, actualIndices)); + + expected.push_factor(1); + expected.push_factor(2); + const FactorIndices expectedIndices2 = list_of(4)(5); + const FactorIndices actualIndices2 = fg1.add_factors(fg2, false); + + EXPECT(assert_equal(expected, fg1)); + EXPECT(assert_container_equality(expectedIndices2, actualIndices2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */