diff --git a/.cproject b/.cproject index b9903731f..a668ce85f 100644 --- a/.cproject +++ b/.cproject @@ -676,6 +676,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -692,6 +700,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -756,6 +780,22 @@ true true + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j2 + tests/testSymbolicBayesNet.run + true + true + true + make -j2 @@ -820,14 +860,6 @@ true true - - make - -j2 - testGaussianFactorGraph - true - true - true - make -j2 @@ -876,6 +908,14 @@ true true + + make + -j2 + testNonlinearFactor.run + true + true + true + make -j2 @@ -1411,6 +1451,22 @@ true true + + make + -j2 + testVirtual.run + true + true + true + + + make + -j2 + tests/timeVirtual.run + true + true + true + make -j2 @@ -1443,6 +1499,22 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testGaussianFactorGraph.run + true + true + true + make -j2 @@ -2359,6 +2431,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -2375,6 +2455,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -2439,6 +2535,22 @@ true true + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j2 + tests/testSymbolicBayesNet.run + true + true + true + make -j2 @@ -2503,14 +2615,6 @@ true true - - make - -j2 - testGaussianFactorGraph - true - true - true - make -j2 @@ -2559,6 +2663,14 @@ true true + + make + -j2 + testNonlinearFactor.run + true + true + true + make -j2 @@ -3094,6 +3206,22 @@ true true + + make + -j2 + testVirtual.run + true + true + true + + + make + -j2 + tests/timeVirtual.run + true + true + true + make -j2 @@ -3126,6 +3254,22 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testGaussianFactorGraph.run + true + true + true + make -j2 diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index e02cb2e10..8e62e08e1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -97,6 +97,7 @@ namespace gtsam { double tol_; equals_star(double tol = 1e-9) : tol_(tol) {} bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { + if (!actual || !expected) return false; return (actual->equals(*expected, tol_)); } }; diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 4a4495c15..9f950096e 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -280,8 +280,8 @@ namespace gtsam { // TODO, why do we actually return a shared pointer, why does eliminate? /* ************************************************************************* */ template - BayesNet - BayesTree::Clique::shortcut(shared_ptr R) { + BayesNet BayesTree::Clique::shortcut(shared_ptr R, + Eliminate function) { static const bool debug = false; @@ -296,17 +296,17 @@ namespace gtsam { } // The root conditional - FactorGraph p_R(*R); + FactorGraph p_R(*R); // The parent clique has a CONDITIONAL for each frontal node in Fp // so we can obtain P(Fp|Sp) in factor graph form - FactorGraph p_Fp_Sp(*parent); + FactorGraph p_Fp_Sp(*parent); // If not the base case, obtain the parent shortcut P(Sp|R) as factors - FactorGraph p_Sp_R(parent->shortcut(R)); + FactorGraph p_Sp_R(parent->shortcut(R, function)); // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) - FactorGraph p_Cp_R; + FactorGraph p_Cp_R; p_Cp_R.push_back(p_R); p_Cp_R.push_back(p_Fp_Sp); p_Cp_R.push_back(p_Sp_R); @@ -345,9 +345,10 @@ namespace gtsam { vector(variablesAtBack.begin(), variablesAtBack.end()), R->back()->key() + 1); Permutation::shared_ptr toBackInverse(toBack.inverse()); - BOOST_FOREACH(const typename CONDITIONAL::FactorType::shared_ptr& factor, p_Cp_R) { + BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { factor->permuteWithInverse(*toBackInverse); } - typename BayesNet::shared_ptr eliminated(EliminationTree::Create(p_Cp_R)->eliminate()); + typename BayesNet::shared_ptr eliminated(EliminationTree< + FactorType>::Create(p_Cp_R)->eliminate(function)); // Take only the conditionals for p(S|R). We check for each variable being // in the separator set because if some separator variables overlap with @@ -380,34 +381,37 @@ namespace gtsam { // Because the root clique could be very big. /* ************************************************************************* */ template - FactorGraph BayesTree::Clique::marginal(shared_ptr R) { + FactorGraph BayesTree::Clique::marginal( + shared_ptr R, Eliminate function) { // If we are the root, just return this root // NOTE: immediately cast to a factor graph if (R.get()==this) return *R; // Combine P(F|S), P(S|R), and P(R) - BayesNet p_FSR = this->shortcut(R); + BayesNet p_FSR = this->shortcut(R, function); p_FSR.push_front(*this); p_FSR.push_back(*R); assertInvariants(); - return *GenericSequentialSolver(p_FSR).jointFactorGraph(keys()); + GenericSequentialSolver solver(p_FSR); + return *solver.jointFactorGraph(keys(), function); } /* ************************************************************************* */ // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) /* ************************************************************************* */ template - FactorGraph BayesTree::Clique::joint(shared_ptr C2, shared_ptr R) { + FactorGraph BayesTree::Clique::joint( + shared_ptr C2, shared_ptr R, Eliminate function) { // For now, assume neither is the root // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) - FactorGraph joint; - if (!isRoot()) joint.push_back(*this); // P(F1|S1) - if (!isRoot()) joint.push_back(shortcut(R)); // P(S1|R) - if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2) - if (!C2->isRoot()) joint.push_back(C2->shortcut(R)); // P(S2|R) - joint.push_back(*R); // P(R) + FactorGraph joint; + if (!isRoot()) joint.push_back(*this); // P(F1|S1) + if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R) + if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2) + if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R) + joint.push_back(*R); // P(R) // Find the keys of both C1 and C2 vector keys1(keys()); @@ -420,7 +424,8 @@ namespace gtsam { vector keys12vector; keys12vector.reserve(keys12.size()); keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); assertInvariants(); - return *GenericSequentialSolver(joint).jointFactorGraph(keys12vector); + GenericSequentialSolver solver(joint); + return *solver.jointFactorGraph(keys12vector, function); } /* ************************************************************************* */ @@ -729,53 +734,59 @@ namespace gtsam { // First finds clique marginal then marginalizes that /* ************************************************************************* */ template - typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor(Index key) const { + typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor( + Index key, Eliminate function) const { // get clique containing key sharedClique clique = (*this)[key]; // calculate or retrieve its marginal - FactorGraph cliqueMarginal = clique->marginal(root_); + FactorGraph cliqueMarginal = clique->marginal(root_,function); - return GenericSequentialSolver(cliqueMarginal).marginalFactor(key); + return GenericSequentialSolver (cliqueMarginal).marginalFactor( + key, function); } /* ************************************************************************* */ template - typename BayesNet::shared_ptr BayesTree::marginalBayesNet(Index key) const { + typename BayesNet::shared_ptr BayesTree::marginalBayesNet( + Index key, Eliminate function) const { // calculate marginal as a factor graph - FactorGraph fg; - fg.push_back(this->marginalFactor(key)); + FactorGraph fg; + fg.push_back(this->marginalFactor(key,function)); // eliminate factor graph marginal to a Bayes net - return GenericSequentialSolver(fg).eliminate(); + return GenericSequentialSolver(fg).eliminate(function); } /* ************************************************************************* */ // Find two cliques, their joint, then marginalizes /* ************************************************************************* */ template - typename FactorGraph::shared_ptr - BayesTree::joint(Index key1, Index key2) const { + typename FactorGraph::shared_ptr BayesTree< + CONDITIONAL>::joint(Index key1, Index key2, Eliminate function) const { // get clique C1 and C2 sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; // calculate joint - FactorGraph p_C1C2(C1->joint(C2, root_)); + FactorGraph p_C1C2(C1->joint(C2, root_, function)); // eliminate remaining factor graph to get requested joint vector key12(2); key12[0] = key1; key12[1] = key2; - return GenericSequentialSolver(p_C1C2).jointFactorGraph(key12); + GenericSequentialSolver solver(p_C1C2); + return solver.jointFactorGraph(key12,function); } /* ************************************************************************* */ template - typename BayesNet::shared_ptr BayesTree::jointBayesNet(Index key1, Index key2) const { + typename BayesNet::shared_ptr BayesTree::jointBayesNet( + Index key1, Index key2, Eliminate function) const { - // eliminate factor graph marginal to a Bayes net - return GenericSequentialSolver(*this->joint(key1, key2)).eliminate(); + // eliminate factor graph marginal to a Bayes net + return GenericSequentialSolver ( + *this->joint(key1, key2, function)).eliminate(function); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 8f9a1865a..064a116a1 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -46,6 +46,8 @@ namespace gtsam { typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr > sharedBayesNet; + typedef typename CONDITIONAL::FactorType FactorType; + typedef typename FactorGraph::Eliminate Eliminate; /** * A Clique in the tree is an incomplete Bayes net: the variables @@ -63,7 +65,7 @@ namespace gtsam { weak_ptr parent_; std::list children_; std::list separator_; /** separator keys */ - typename CONDITIONAL::FactorType::shared_ptr cachedFactor_; + typename FactorType::shared_ptr cachedFactor_; friend class BayesTree; @@ -96,7 +98,7 @@ namespace gtsam { size_t treeSize() const; /** Access the cached factor (this is a hack) */ - typename CONDITIONAL::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + typename FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } /** print this node and entire subtree below it */ void printTree(const std::string& indent="") const; @@ -113,13 +115,13 @@ namespace gtsam { /** return the conditional P(S|Root) on the separator given the root */ // TODO: create a cached version - BayesNet shortcut(shared_ptr root); + BayesNet shortcut(shared_ptr root, Eliminate function); /** return the marginal P(C) of the clique */ - FactorGraph marginal(shared_ptr root); + FactorGraph marginal(shared_ptr root, Eliminate function); /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ - FactorGraph joint(shared_ptr C2, shared_ptr root); + FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); }; // \struct Clique @@ -262,20 +264,24 @@ namespace gtsam { CliqueData getCliqueData() const; /** return marginal on any variable */ - typename CONDITIONAL::FactorType::shared_ptr marginalFactor(Index key) const; + typename CONDITIONAL::FactorType::shared_ptr marginalFactor(Index key, + Eliminate function) const; /** * return marginal on any variable, as a Bayes Net * NOTE: this function calls marginal, and then eliminates it into a Bayes Net * This is more expensive than the above function */ - typename BayesNet::shared_ptr marginalBayesNet(Index key) const; + typename BayesNet::shared_ptr marginalBayesNet(Index key, + Eliminate function) const; /** return joint on two variables */ - typename FactorGraph::shared_ptr joint(Index key1, Index key2) const; + typename FactorGraph::shared_ptr joint(Index key1, Index key2, + Eliminate function) const; /** return joint on two variables as a BayesNet */ - typename BayesNet::shared_ptr jointBayesNet(Index key1, Index key2) const; + typename BayesNet::shared_ptr jointBayesNet(Index key1, + Index key2, Eliminate function) const; /** * Read only with side effects diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 4083d4105..85742388f 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -24,10 +24,8 @@ #include #include #include -#include #include #include -#include namespace gtsam { @@ -168,18 +166,6 @@ public: /** print */ void print(const std::string& s = "Conditional") const; - /** Permute the variables when only separator variables need to be permuted. - * Returns true if any reordered variables appeared in the separator and - * false if not. - */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); - - /** - * Permutes the Conditional, but for efficiency requires the permutation - * to already be inverted. - */ - void permuteWithInverse(const Permutation& inversePermutation); - private: /** Serialization function */ friend class boost::serialization::access; @@ -200,37 +186,4 @@ void Conditional::print(const std::string& s) const { std::cout << ")" << std::endl; } -/* ************************************************************************* */ -template -bool Conditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { -#ifndef NDEBUG - BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); } -#endif - bool parentChanged = false; - BOOST_FOREACH(Key& parent, parents()) { - Key newParent = inversePermutation[parent]; - if(parent != newParent) { - parentChanged = true; - parent = newParent; - } - } - assertInvariants(); - return parentChanged; -} - -/* ************************************************************************* */ -template -void Conditional::permuteWithInverse(const Permutation& inversePermutation) { - // The permutation may not move the separators into the frontals -#ifndef NDEBUG - BOOST_FOREACH(const Key frontal, this->frontals()) { - BOOST_FOREACH(const Key separator, this->parents()) { - assert(inversePermutation[frontal] < inversePermutation[separator]); - } - } -#endif - FactorType::permuteWithInverse(inversePermutation); - assertInvariants(); -} - -} +} // gtsam diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index a4b0b9294..e077bf8fe 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -27,8 +27,8 @@ namespace gtsam { /* ************************************************************************* */ template -typename EliminationTree::sharedFactor -EliminationTree::eliminate_(Conditionals& conditionals) const { +typename EliminationTree::sharedFactor EliminationTree::eliminate_( + Eliminate function, Conditionals& conditionals) const { static const bool debug = false; @@ -43,11 +43,11 @@ EliminationTree::eliminate_(Conditionals& conditionals) const { // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] BOOST_FOREACH(const shared_ptr& child, subTrees_) { - factors.push_back(child->eliminate_(conditionals)); } + factors.push_back(child->eliminate_(function, conditionals)); } // Combine all factors (from this node and from subtrees) into a joint factor - pair eliminated( - FACTOR::CombineAndEliminate(factors, 1)); + pair + eliminated(function(factors, 1)); conditionals[this->key_] = eliminated.first->front(); if(debug) cout << "Eliminated " << this->key_ << " to get:\n"; @@ -57,39 +57,6 @@ EliminationTree::eliminate_(Conditionals& conditionals) const { return eliminated.second; } -/* ************************************************************************* */ -// This is the explicit specialization for symbolic factors, i.e. IndexFactor -template<> inline FastSet EliminationTree::eliminateSymbolic_(Conditionals& conditionals) const { - - static const bool debug = false; - - if(debug) cout << "ETree: eliminating " << this->key_ << endl; - - FastSet variables; - BOOST_FOREACH(const sharedFactor& factor, factors_) { - variables.insert(factor->begin(), factor->end()); - } - BOOST_FOREACH(const shared_ptr& child, subTrees_) { - sharedFactor factor(child->eliminate_(conditionals)); - variables.insert(factor->begin(), factor->end()); - } - conditionals[this->key_] = IndexConditional::FromRange(variables.begin(), variables.end(), 1); - variables.erase(variables.begin()); - - if(debug) cout << "Eliminated " << this->key_ << " to get:\n"; - if(debug) conditionals[this->key_]->print("Conditional: "); - - return variables; -} - -/* ************************************************************************* */ -// This non-specialized version cannot be called. -template FastSet -EliminationTree::eliminateSymbolic_(Conditionals& conditionals) const { - throw invalid_argument("symbolic eliminate should never be called from a non-IndexFactor EliminationTree"); - return FastSet(); -} - /* ************************************************************************* */ template vector EliminationTree::ComputeParents(const VariableIndex& structure) { @@ -126,8 +93,9 @@ vector EliminationTree::ComputeParents(const VariableIndex& struc /* ************************************************************************* */ template template -typename EliminationTree::shared_ptr -EliminationTree::Create(const FactorGraph& factorGraph, const VariableIndex& structure) { +typename EliminationTree::shared_ptr EliminationTree::Create( + const FactorGraph& factorGraph, + const VariableIndex& structure) { static const bool debug = false; @@ -218,12 +186,12 @@ bool EliminationTree::equals(const EliminationTree& ex /* ************************************************************************* */ template typename EliminationTree::BayesNet::shared_ptr -EliminationTree::eliminate() const { +EliminationTree::eliminate(Eliminate function) const { // call recursive routine tic(1, "ET recursive eliminate"); Conditionals conditionals(this->key_ + 1); - (void)eliminate_(conditionals); + (void)eliminate_(function, conditionals); toc(1, "ET recursive eliminate"); // Add conditionals to BayesNet @@ -238,28 +206,4 @@ EliminationTree::eliminate() const { return bayesNet; } -/* ************************************************************************* */ -// Specialization for symbolic elimination that calls the optimized eliminateSymbolic_ -template<> -inline EliminationTree::BayesNet::shared_ptr -EliminationTree::eliminate() const { - - // call recursive routine - tic(1, "ET recursive eliminate"); - Conditionals conditionals(this->key_ + 1); - (void)eliminateSymbolic_(conditionals); - toc(1, "ET recursive eliminate"); - - // Add conditionals to BayesNet - tic(2, "assemble BayesNet"); - BayesNet::shared_ptr bayesNet(new BayesNet); - BOOST_FOREACH(const BayesNet::sharedConditional& conditional, conditionals) { - if(conditional) - bayesNet->push_back(conditional); - } - toc(2, "assemble BayesNet"); - - return bayesNet; -} - } diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index e56da35ce..b2f97c616 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -42,6 +42,9 @@ public: typedef boost::shared_ptr > shared_ptr; typedef gtsam::BayesNet BayesNet; + /** typedef for an eliminate subroutine */ + typedef typename FactorGraph::Eliminate Eliminate; + private: typedef FastList Factors; @@ -70,13 +73,7 @@ private: /** * Recursive routine that eliminates the factors arranged in an elimination tree */ - sharedFactor eliminate_(Conditionals& conditionals) const; - - /** - * Special optimized eliminate for symbolic factors. Will not compile if - * called in a non-IndexFactor EliminationTree. - */ - FastSet eliminateSymbolic_(Conditionals& conditionals) const; + sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const; // Allow access to constructor and add methods for testing purposes friend class ::EliminationTreeTester; @@ -101,7 +98,7 @@ public: bool equals(const EliminationTree& other, double tol = 1e-9) const; /** Eliminate the factors to a Bayes Net */ - typename BayesNet::shared_ptr eliminate() const; + typename BayesNet::shared_ptr eliminate(Eliminate function) const; }; } diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index 68e45f157..543e738e1 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -18,99 +18,81 @@ #pragma once -#include - +#include #include #include -#include -#include -#include -#include +#include namespace gtsam { -/* ************************************************************************* */ -template -Factor::Factor(const Factor& f) : keys_(f.keys_) { assertInvariants(); } + /* ************************************************************************* */ + template + Factor::Factor(const Factor& f) : + keys_(f.keys_) { + assertInvariants(); + } -/* ************************************************************************* */ -template -Factor::Factor(const ConditionalType& c) : keys_(c.keys()) { assertInvariants(); } + /* ************************************************************************* */ + template + Factor::Factor(const ConditionalType& c) : + keys_(c.keys()) { + assertInvariants(); + } -/* ************************************************************************* */ -template -void Factor::assertInvariants() const { + /* ************************************************************************* */ + template + void Factor::assertInvariants() const { #ifndef NDEBUG - // Check that keys are all unique - std::multiset nonunique(keys_.begin(), keys_.end()); - std::set unique(keys_.begin(), keys_.end()); - assert(nonunique.size() == unique.size() && std::equal(nonunique.begin(), nonunique.end(), unique.begin())); + // Check that keys are all unique + std::multiset nonunique(keys_.begin(), keys_.end()); + std::set unique(keys_.begin(), keys_.end()); + assert(nonunique.size() == unique.size() && std::equal(nonunique.begin(), nonunique.end(), unique.begin())); #endif -} + } -/* ************************************************************************* */ -template -void Factor::print(const std::string& s) const { - std::cout << s << " "; - BOOST_FOREACH(KEY key, keys_) std::cout << " " << key; - std::cout << std::endl; -} + /* ************************************************************************* */ + template + void Factor::print(const std::string& s) const { + std::cout << s << " "; + BOOST_FOREACH(KEY key, keys_) + std::cout << " " << key; + std::cout << std::endl; + } -/* ************************************************************************* */ -template -bool Factor::equals(const This& other, double tol) const { - return keys_ == other.keys_; -} + /* ************************************************************************* */ + template + bool Factor::equals(const This& other, double tol) const { + return keys_ == other.keys_; + } -/* ************************************************************************* */ -template -template -typename DERIVED::shared_ptr Factor::Combine(const FactorGraph& factors, const FastMap >& variableSlots) { - typedef const FastMap > VariableSlots; - typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)) FirstGetter; - typedef boost::transform_iterator< - FirstGetter, typename VariableSlots::const_iterator, - KEY, KEY> IndexIterator; - FirstGetter firstGetter(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)); - IndexIterator keysBegin(variableSlots.begin(), firstGetter); - IndexIterator keysEnd(variableSlots.end(), firstGetter); - return typename DERIVED::shared_ptr(new DERIVED(keysBegin, keysEnd)); -} + /* ************************************************************************* */ +#ifdef TRACK_ELIMINATE + template + template + typename COND::shared_ptr Factor::eliminateFirst() { + assert(!keys_.empty()); + assertInvariants(); + KEY eliminated = keys_.front(); + keys_.erase(keys_.begin()); + assertInvariants(); + return typename COND::shared_ptr(new COND(eliminated, keys_)); + } -/* ************************************************************************* */ -template -template -typename CONDITIONAL::shared_ptr Factor::eliminateFirst() { - assert(!keys_.empty()); - assertInvariants(); - KEY eliminated = keys_.front(); - keys_.erase(keys_.begin()); - assertInvariants(); - return typename CONDITIONAL::shared_ptr(new CONDITIONAL(eliminated, keys_)); -} - -/* ************************************************************************* */ -template -template -typename BayesNet::shared_ptr Factor::eliminate(size_t nrFrontals) { - assert(keys_.size() >= nrFrontals); - assertInvariants(); - typename BayesNet::shared_ptr fragment(new BayesNet()); - const_iterator nextFrontal = this->begin(); - for(KEY n = 0; n < nrFrontals; ++n, ++nextFrontal) - fragment->push_back(CONDITIONAL::FromRange( - nextFrontal, const_iterator(this->end()), 1)); - if(nrFrontals > 0) - keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents()); - assertInvariants(); - return fragment; -} - -/* ************************************************************************* */ -template -void Factor::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(KEY& key, keys_) { key = inversePermutation[key]; } - assertInvariants(); -} + /* ************************************************************************* */ + template + template + typename BayesNet::shared_ptr Factor::eliminate(size_t nrFrontals) { + assert(keys_.size() >= nrFrontals); + assertInvariants(); + typename BayesNet::shared_ptr fragment(new BayesNet ()); + const_iterator it = this->begin(); + for (KEY n = 0; n < nrFrontals; ++n, ++it) + fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1)); + if (nrFrontals > 0) keys_.assign(fragment->back()->beginParents(), + fragment->back()->endParents()); + assertInvariants(); + return fragment; + } +#endif } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index b484b801a..ea394ca9d 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -21,15 +21,14 @@ #pragma once -#include #include +#include +#include #include // for noncopyable #include #include -#include #include #include -#include namespace gtsam { @@ -117,10 +116,7 @@ public: keys_.push_back(key); assertInvariants(); } - /** Create a combined joint factor (new style for EliminationTree). */ - template - static typename DERIVED::shared_ptr Combine(const FactorGraph& factors, const FastMap >& variableSlots); - +#ifdef TRACK_ELIMINATE /** * eliminate the first variable involved in this factor * @return a conditional on the eliminated variable @@ -133,12 +129,7 @@ public: */ template typename BayesNet::shared_ptr eliminate(size_t nrFrontals = 1); - - /** - * Permutes the factor, but for efficiency requires the permutation - * to already be inverted. - */ - void permuteWithInverse(const Permutation& inversePermutation); +#endif /** iterators */ const_iterator begin() const { return keys_.begin(); } diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 8767ec6df..51aab7d43 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -30,7 +30,10 @@ #include #include #include +#include +#include #include +#include #include #include @@ -112,5 +115,21 @@ namespace gtsam { return fg; } + /* ************************************************************************* */ + template + typename DERIVED::shared_ptr Combine(const FactorGraph& factors, + const FastMap >& variableSlots) { + typedef const FastMap > VariableSlots; + typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)) + FirstGetter; + typedef boost::transform_iterator IndexIterator; + FirstGetter firstGetter(boost::lambda::bind( + &VariableSlots::value_type::first, boost::lambda::_1)); + IndexIterator keysBegin(variableSlots.begin(), firstGetter); + IndexIterator keysEnd(variableSlots.end(), firstGetter); + return typename DERIVED::shared_ptr(new DERIVED(keysBegin, keysEnd)); + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index eefd7f09f..7cc099d6c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -24,8 +24,10 @@ #include #include #include +#include #include +#include #include #include #include @@ -47,6 +49,14 @@ namespace gtsam { typedef typename std::vector::iterator iterator; typedef typename std::vector::const_iterator const_iterator; + /** typedef for elimination result */ + typedef std::pair< + typename BayesNet::shared_ptr, + typename FACTOR::shared_ptr> EliminationResult; + + /** typedef for an eliminate subroutine */ + typedef boost::function&, size_t)> Eliminate; + protected: /** Collection of factors */ @@ -176,6 +186,11 @@ namespace gtsam { } }; // FactorGraph + /** Create a combined joint factor (new style for EliminationTree). */ + template + typename DERIVED::shared_ptr Combine(const FactorGraph& factors, + const FastMap >& variableSlots); + /** * static function that combines two factor graphs * @param const &fg1 Linear factor graph diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h index 58a8b22b1..bd916fbe2 100644 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ b/gtsam/inference/GenericMultifrontalSolver-inl.h @@ -48,34 +48,37 @@ void GenericMultifrontalSolver::replaceFactors(const typen /* ************************************************************************* */ template -typename JUNCTIONTREE::BayesTree::shared_ptr -GenericMultifrontalSolver::eliminate() const { - typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree(new typename JUNCTIONTREE::BayesTree); - bayesTree->insert(junctionTree_->eliminate()); - return bayesTree; +typename JUNCTIONTREE::BayesTree::shared_ptr GenericMultifrontalSolver< + FACTOR, JUNCTIONTREE>::eliminate( + typename FactorGraph::Eliminate function) const { + typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree( + new typename JUNCTIONTREE::BayesTree); + bayesTree->insert(junctionTree_->eliminate(function)); + return bayesTree; } /* ************************************************************************* */ template -typename FactorGraph::shared_ptr -GenericMultifrontalSolver::jointFactorGraph(const std::vector& js) const { +typename FactorGraph::shared_ptr GenericMultifrontalSolver::jointFactorGraph(const std::vector& js, + Eliminate function) const { - // We currently have code written only for computing the + // We currently have code written only for computing the - if(js.size() != 2) - throw domain_error( - "*MultifrontalSolver::joint(js) currently can only compute joint marginals\n" - "for exactly two variables. You can call marginal to compute the\n" - "marginal for one variable. *SequentialSolver::joint(js) can compute the\n" - "joint marginal over any number of variables, so use that if necessary.\n"); + if (js.size() != 2) throw domain_error( + "*MultifrontalSolver::joint(js) currently can only compute joint marginals\n" + "for exactly two variables. You can call marginal to compute the\n" + "marginal for one variable. *SequentialSolver::joint(js) can compute the\n" + "joint marginal over any number of variables, so use that if necessary.\n"); - return eliminate()->joint(js[0], js[1]); + return eliminate(function)->joint(js[0], js[1], function); } /* ************************************************************************* */ template -typename FACTOR::shared_ptr GenericMultifrontalSolver::marginalFactor(Index j) const { - return eliminate()->marginalFactor(j); +typename FACTOR::shared_ptr GenericMultifrontalSolver::marginalFactor( + Index j, Eliminate function) const { + return eliminate(function)->marginalFactor(j, function); } } diff --git a/gtsam/inference/GenericMultifrontalSolver.h b/gtsam/inference/GenericMultifrontalSolver.h index 3ac85fc70..a7266129d 100644 --- a/gtsam/inference/GenericMultifrontalSolver.h +++ b/gtsam/inference/GenericMultifrontalSolver.h @@ -44,9 +44,11 @@ protected: // Junction tree that performs elimination. typename JUNCTIONTREE::shared_ptr junctionTree_; - public: + typedef typename FactorGraph::shared_ptr sharedGraph; + typedef typename FactorGraph::Eliminate Eliminate; + /** * Construct the solver for a factor graph. This builds the junction * tree, which does the symbolic elimination, identifies the cliques, @@ -59,33 +61,35 @@ public: * VariableIndex. The solver will store these pointers, so this constructor * is the fastest. */ - GenericMultifrontalSolver(const typename FactorGraph::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex); + GenericMultifrontalSolver(const sharedGraph& factorGraph, + const VariableIndex::shared_ptr& variableIndex); /** * Replace the factor graph with a new one having the same structure. The * This function can be used if the numerical part of the factors changes, * such as during relinearization or adjusting of noise models. */ - void replaceFactors(const typename FactorGraph::shared_ptr& factorGraph); + void replaceFactors(const sharedGraph& factorGraph); /** * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - typename JUNCTIONTREE::BayesTree::shared_ptr eliminate() const; + typename JUNCTIONTREE::BayesTree::shared_ptr + eliminate(Eliminate function) const; /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. This function returns the result as a factor * graph. */ - typename FactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const; + sharedGraph jointFactorGraph(const std::vector& js, Eliminate function) const; /** * Compute the marginal density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename FACTOR::shared_ptr marginalFactor(Index j) const; + typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const; }; diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 8cdd6cb88..802c2b3f2 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -48,9 +49,9 @@ namespace gtsam { /* ************************************************************************* */ template void GenericSequentialSolver::print(const std::string& s) const { - this->factors_->print(s+" factors:"); - this->structure_->print(s+" structure:\n"); - this->eliminationTree_->print(s+" etree:"); + this->factors_->print(s + " factors:"); + this->structure_->print(s + " structure:\n"); + this->eliminationTree_->print(s + " etree:"); } /* ************************************************************************* */ @@ -77,15 +78,17 @@ namespace gtsam { /* ************************************************************************* */ template typename BayesNet::shared_ptr // - GenericSequentialSolver::eliminate() const { - return eliminationTree_->eliminate(); + GenericSequentialSolver::eliminate( + typename EliminationTree::Eliminate function) const { + return eliminationTree_->eliminate(function); } /* ************************************************************************* */ template typename FactorGraph::shared_ptr // GenericSequentialSolver::jointFactorGraph( - const std::vector& js) const { + const std::vector& js, + typename EliminationTree::Eliminate function) const { // Compute a COLAMD permutation with the marginal variable constrained to the end. Permutation::shared_ptr permutation(Inference::PermutationCOLAMD( @@ -100,7 +103,7 @@ namespace gtsam { // Eliminate all variables typename BayesNet::shared_ptr bayesNet( - EliminationTree::Create(*factors_)->eliminate()); + EliminationTree::Create(*factors_)->eliminate(function)); // Undo the permuation on the original factors and on the structure. BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) @@ -125,13 +128,14 @@ namespace gtsam { /* ************************************************************************* */ template typename FACTOR::shared_ptr // - GenericSequentialSolver::marginalFactor(Index j) const { + GenericSequentialSolver::marginalFactor(Index j, + typename EliminationTree::Eliminate function) const { // Create a container for the one variable index vector js(1); js[0] = j; // Call joint and return the only factor in the factor graph it returns - return (*this->jointFactorGraph(js))[0]; + return (*this->jointFactorGraph(js, function))[0]; } } // namespace gtsam diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index c97a0e162..c8cba1113 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -27,7 +27,8 @@ namespace gtsam { template - class GenericSequentialSolver : public Testable > { + class GenericSequentialSolver: public Testable< + GenericSequentialSolver > { protected: @@ -57,11 +58,11 @@ namespace gtsam { const typename FactorGraph::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex); - /** Print to cout */ - void print(const std::string& name = "GenericSequentialSolver: ") const; + /** Print to cout */ + void print(const std::string& name = "GenericSequentialSolver: ") const; - /** Test whether is equal to another */ - bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; + /** Test whether is equal to another */ + bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; /** * Replace the factor graph with a new one having the same structure. The @@ -76,7 +77,7 @@ namespace gtsam { * to recursively eliminate. */ typename BayesNet::shared_ptr - eliminate() const; + eliminate(typename EliminationTree::Eliminate function) const; /** * Compute the marginal joint over a set of variables, by integrating out @@ -84,13 +85,15 @@ namespace gtsam { * graph. */ typename FactorGraph::shared_ptr jointFactorGraph( - const std::vector& js) const; + const std::vector& js, + typename EliminationTree::Eliminate function) const; /** * Compute the marginal Gaussian density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename FACTOR::shared_ptr marginalFactor(Index j) const; + typename FACTOR::shared_ptr marginalFactor(Index j, + typename EliminationTree::Eliminate function) const; }; // GenericSequentialSolver diff --git a/gtsam/inference/ISAM-inl.h b/gtsam/inference/ISAM-inl.h index fe6d37749..0ab3d8d48 100644 --- a/gtsam/inference/ISAM-inl.h +++ b/gtsam/inference/ISAM-inl.h @@ -41,19 +41,20 @@ namespace gtsam { /* ************************************************************************* */ template - template - void ISAM::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) { + template void ISAM::update_internal( + const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) { // Remove the contaminated part of the Bayes tree BayesNet bn; removeTop(newFactors.keys(), bn, orphans); - FACTORGRAPH factors(bn); + FG factors(bn); // add the factors themselves factors.push_back(newFactors); // eliminate into a Bayes net - typename BayesNet::shared_ptr bayesNet = GenericSequentialSolver(factors).eliminate(); + GenericSequentialSolver solver(factors); + typename BayesNet::shared_ptr bayesNet = solver.eliminate(function); // insert conditionals back in, straight into the topless bayesTree typename BayesNet::const_reverse_iterator rit; @@ -61,17 +62,17 @@ namespace gtsam { this->insert(*rit); // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) { + BOOST_FOREACH(sharedClique orphan, orphans) this->insert(orphan); - } - } + /* ************************************************************************* */ template - template - void ISAM::update(const FACTORGRAPH& newFactors) { + template + void ISAM::update(const FG& newFactors, + typename FG::Eliminate function) { Cliques orphans; - this->update_internal(newFactors, orphans); + this->update_internal(newFactors, orphans, function); } } diff --git a/gtsam/inference/ISAM.h b/gtsam/inference/ISAM.h index 6346b9fe6..e02aee5b9 100644 --- a/gtsam/inference/ISAM.h +++ b/gtsam/inference/ISAM.h @@ -52,10 +52,12 @@ namespace gtsam { /** * iSAM. (update_internal provides access to list of orphans for drawing purposes) */ - template - void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans); - template - void update(const FACTORGRAPH& newFactors); + template + void update_internal(const FG& newFactors, Cliques& orphans, + typename FG::Eliminate function); + + template + void update(const FG& newFactors, typename FG::Eliminate function); }; // ISAM diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index 70012a3f2..7ade6ca11 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -41,4 +41,37 @@ namespace gtsam { #endif } + /* ************************************************************************* */ + bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + #ifndef NDEBUG + BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); } + #endif + bool parentChanged = false; + BOOST_FOREACH(Key& parent, parents()) { + Key newParent = inversePermutation[parent]; + if(parent != newParent) { + parentChanged = true; + parent = newParent; + } + } + assertInvariants(); + return parentChanged; + } + + /* ************************************************************************* */ + void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) { + // The permutation may not move the separators into the frontals + #ifndef NDEBUG + BOOST_FOREACH(const Key frontal, this->frontals()) { + BOOST_FOREACH(const Key separator, this->parents()) { + assert(inversePermutation[frontal] < inversePermutation[separator]); + } + } + #endif + BOOST_FOREACH(Index& key, keys_) + key = inversePermutation[key]; + assertInvariants(); + } + /* ************************************************************************* */ + } diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index 83ddb9b5d..2f379b030 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -18,8 +18,10 @@ #pragma once +#include #include #include +#include namespace gtsam { @@ -80,6 +82,18 @@ namespace gtsam { /** Convert to a factor */ IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); } + /** Permute the variables when only separator variables need to be permuted. + * Returns true if any reordered variables appeared in the separator and + * false if not. + */ + bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** + * Permutes the Conditional, but for efficiency requires the permutation + * to already be inverted. + */ + void permuteWithInverse(const Permutation& inversePermutation); + }; } diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp index 22e1a4619..6b49ff471 100644 --- a/gtsam/inference/IndexFactor.cpp +++ b/gtsam/inference/IndexFactor.cpp @@ -16,76 +16,57 @@ * @created Oct 17, 2010 */ -#include -#include #include +#include #include -#include using namespace std; namespace gtsam { -template class Factor; + template class Factor ; -/* ************************************************************************* */ -void IndexFactor::assertInvariants() const { - Base::assertInvariants(); -#ifndef NDEBUG -#ifndef GTSAM_NO_ENFORCE_ORDERING - std::set uniqueSorted(keys_.begin(), keys_.end()); - assert(uniqueSorted.size() == keys_.size() && std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin())); + /* ************************************************************************* */ + void IndexFactor::assertInvariants() const { + Base::assertInvariants(); +//#ifndef NDEBUG +//#ifndef GTSAM_NO_ENFORCE_ORDERING +// std::set uniqueSorted(keys_.begin(), keys_.end()); +// assert(uniqueSorted.size() == keys_.size() && std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin())); +//#endif +//#endif + } + + /* ************************************************************************* */ + IndexFactor::IndexFactor(const IndexConditional& c) : + Base(c) { + assertInvariants(); + } + + /* ************************************************************************* */ +#ifdef TRACK_ELIMINATE + boost::shared_ptr IndexFactor::eliminateFirst() { + boost::shared_ptr result(Base::eliminateFirst< + IndexConditional>()); + assertInvariants(); + return result; + } + + /* ************************************************************************* */ + boost::shared_ptr > IndexFactor::eliminate( + size_t nrFrontals) { + boost::shared_ptr > result(Base::eliminate< + IndexConditional>(nrFrontals)); + assertInvariants(); + return result; + } #endif -#endif -} -/* ************************************************************************* */ -IndexFactor::IndexFactor(const IndexConditional& c): Base(c) { - assertInvariants(); -} - -/* ************************************************************************* */ -pair::shared_ptr, IndexFactor::shared_ptr> IndexFactor::CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals) { - - FastSet variables; - BOOST_FOREACH(const shared_ptr& factor, factors) { - BOOST_FOREACH(Index var, *factor) { - variables.insert(var); } } - - if(variables.size() < 1) - throw invalid_argument("IndexFactor::CombineAndEliminate called on factors with zero total variables."); - - pair::shared_ptr, shared_ptr> result; - result.first.reset(new BayesNet()); - FastSet::const_iterator var; - for(var = variables.begin(); result.first->size() < nrFrontals; ++var) - result.first->push_back(IndexConditional::FromRange(var, variables.end(), 1)); - result.second.reset(new IndexFactor(var, variables.end())); - - return result; -} - -/* ************************************************************************* */ -IndexFactor::shared_ptr IndexFactor::Combine( - const FactorGraph& factors, const FastMap >& variableSlots) { - IndexFactor::shared_ptr combined(Base::Combine(factors, variableSlots)); - combined->assertInvariants(); - return combined; -} - -/* ************************************************************************* */ -boost::shared_ptr IndexFactor::eliminateFirst() { - boost::shared_ptr result(Base::eliminateFirst()); - assertInvariants(); - return result; -} - -/* ************************************************************************* */ -boost::shared_ptr > IndexFactor::eliminate(size_t nrFrontals) { - boost::shared_ptr > result(Base::eliminate(nrFrontals)); - assertInvariants(); - return result; -} - -} + /* ************************************************************************* */ + void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) { + BOOST_FOREACH(Index& key, keys_) + key = inversePermutation[key]; + assertInvariants(); + } + /* ************************************************************************* */ +} // gtsam diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index 2ff0dd940..08f78cb17 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -19,93 +19,111 @@ #pragma once #include +#include namespace gtsam { - // Forward declaration of IndexConditional - class IndexConditional; + // Forward declaration of IndexConditional + class IndexConditional; - /** - * IndexFactor serves two purposes. It is the base class for all linear - * factors (GaussianFactor, JacobianFactor, HessianFactor), and also - * functions as a symbolic factor with Index keys, used to do symbolic - * elimination by JunctionTree. - * - * This class provides symbolic elimination, via the CombineAndEliminate - * function. Combine and eliminate can also be called separately, but for - * this and derived classes calling them separately generally does extra - * work. - * - * It derives from Factor with a key type of Index, which is an unsigned - * integer. - */ - class IndexFactor : public Factor { + /** + * IndexFactor serves two purposes. It is the base class for all linear + * factors (GaussianFactor, JacobianFactor, HessianFactor), and also + * functions as a symbolic factor with Index keys, used to do symbolic + * elimination by JunctionTree. + * + * It derives from Factor with a key type of Index, an unsigned integer. + */ + class IndexFactor: public Factor { - protected: + protected: - // Internal function for checking class invariants (sorted keys for this factor) - void assertInvariants() const; + // Internal function for checking class invariants (sorted keys for this factor) + void assertInvariants() const; - public: + public: - typedef IndexFactor This; - typedef Factor Base; + typedef IndexFactor This; + typedef Factor Base; - /** Elimination produces an IndexConditional */ - typedef IndexConditional ConditionalType; + /** Elimination produces an IndexConditional */ + typedef IndexConditional ConditionalType; - /** Overriding the shared_ptr typedef */ - typedef boost::shared_ptr shared_ptr; + /** Overriding the shared_ptr typedef */ + typedef boost::shared_ptr shared_ptr; - /** Copy constructor */ - IndexFactor(const This& f) : Base(f) { assertInvariants(); } + /** Copy constructor */ + IndexFactor(const This& f) : + Base(f) { + assertInvariants(); + } - /** Construct from derived type */ - IndexFactor(const IndexConditional& c); + /** Construct from derived type */ + IndexFactor(const IndexConditional& c); - /** Constructor from a collection of keys */ - template IndexFactor(KeyIterator beginKey, KeyIterator endKey) : - Base(beginKey, endKey) { assertInvariants(); } + /** Constructor from a collection of keys */ + template IndexFactor(KeyIterator beginKey, + KeyIterator endKey) : + Base(beginKey, endKey) { + assertInvariants(); + } - /** Default constructor for I/O */ - IndexFactor() { assertInvariants(); } + /** Default constructor for I/O */ + IndexFactor() { + assertInvariants(); + } - /** Construct unary factor */ - IndexFactor(Index j) : Base(j) { assertInvariants(); } + /** Construct unary factor */ + IndexFactor(Index j) : + Base(j) { + assertInvariants(); + } - /** Construct binary factor */ - IndexFactor(Index j1, Index j2) : Base(j1, j2) { assertInvariants(); } + /** Construct binary factor */ + IndexFactor(Index j1, Index j2) : + Base(j1, j2) { + assertInvariants(); + } - /** Construct ternary factor */ - IndexFactor(Index j1, Index j2, Index j3) : Base(j1, j2, j3) { assertInvariants(); } + /** Construct ternary factor */ + IndexFactor(Index j1, Index j2, Index j3) : + Base(j1, j2, j3) { + assertInvariants(); + } - /** Construct 4-way factor */ - IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) { assertInvariants(); } + /** Construct 4-way factor */ + IndexFactor(Index j1, Index j2, Index j3, Index j4) : + Base(j1, j2, j3, j4) { + assertInvariants(); + } - /** Construct n-way factor */ - IndexFactor(const std::set& js) : Base(js) { assertInvariants(); } + /** Construct n-way factor */ + IndexFactor(const std::set& js) : + Base(js) { + assertInvariants(); + } - /** - * Combine and eliminate several factors. - */ - static std::pair::shared_ptr, shared_ptr> CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals=1); +#ifdef TRACK_ELIMINATE + /** + * eliminate the first variable involved in this factor + * @return a conditional on the eliminated variable + */ + boost::shared_ptr eliminateFirst(); - /** Create a combined joint factor (new style for EliminationTree). */ - static shared_ptr - Combine(const FactorGraph& factors, const FastMap >& variableSlots); + /** eliminate the first nrFrontals frontal variables.*/ + boost::shared_ptr > eliminate(size_t nrFrontals = + 1); +#endif - /** - * eliminate the first variable involved in this factor - * @return a conditional on the eliminated variable - */ - boost::shared_ptr eliminateFirst(); + /** + * Permutes the factor, but for efficiency requires the permutation + * to already be inverted. + */ + void permuteWithInverse(const Permutation& inversePermutation); - /** - * eliminate the first nrFrontals frontal variables. - */ - boost::shared_ptr > eliminate(size_t nrFrontals = 1); + virtual ~IndexFactor() { + } - }; + }; // IndexFactor } diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h index e155862db..6f24b6c25 100644 --- a/gtsam/inference/JunctionTree-inl.h +++ b/gtsam/inference/JunctionTree-inl.h @@ -40,10 +40,11 @@ namespace gtsam { void JunctionTree::construct(const FG& fg, const VariableIndex& variableIndex) { tic(1, "JT Constructor"); tic(1, "JT symbolic ET"); - const typename EliminationTree::shared_ptr symETree(EliminationTree::Create(fg, variableIndex)); + const typename EliminationTree::shared_ptr symETree = + EliminationTree::Create(fg, variableIndex); toc(1, "JT symbolic ET"); tic(2, "JT symbolic eliminate"); - SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(); + SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic); toc(2, "JT symbolic eliminate"); tic(3, "symbolic BayesTree"); SymbolicBayesTree sbt(*sbn); @@ -148,9 +149,11 @@ namespace gtsam { } /* ************************************************************************* */ - template - pair::BayesTree::sharedClique, typename FG::sharedFactor> - JunctionTree::eliminateOneClique(const boost::shared_ptr& current, bool cache) const { + template + pair::BayesTree::sharedClique, + typename FG::sharedFactor> JunctionTree::eliminateOneClique( + typename FG::Eliminate function, + const boost::shared_ptr& current, bool cache) const { FG fg; // factor graph will be assembled from local factors and marginalized children fg.reserve(current->size() + current->children().size()); @@ -160,7 +163,7 @@ namespace gtsam { list children; BOOST_FOREACH(const boost::shared_ptr& child, current->children()) { pair tree_factor( - eliminateOneClique(child, cache)); + eliminateOneClique(function, child, cache)); children.push_back(tree_factor.first); fg.push_back(tree_factor.second); } @@ -171,8 +174,10 @@ namespace gtsam { // Now that we know which factors and variables, and where variables // come from and go to, create and eliminate the new joint factor. tic(2, "CombineAndEliminate"); - pair::shared_ptr, typename FG::sharedFactor> eliminated( - FG::FactorType::CombineAndEliminate(fg, current->frontal.size())); + pair< + typename BayesNet::shared_ptr, + typename FG::sharedFactor> eliminated(function(fg, + current->frontal.size())); toc(2, "CombineAndEliminate"); assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin())); @@ -193,17 +198,19 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename JunctionTree::BayesTree::sharedClique JunctionTree::eliminate(bool cache) const { - if(this->root()) { - tic(2,"JT eliminate"); - pair ret = this->eliminateOneClique(this->root(), cache); - if (ret.second->size() != 0) - throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!"); - toc(2,"JT eliminate"); - return ret.first; - } else - return typename BayesTree::sharedClique(); - } + template + typename JunctionTree::BayesTree::sharedClique JunctionTree::eliminate( + typename FG::Eliminate function, bool cache) const { + if (this->root()) { + tic(2, "JT eliminate"); + pair ret = + this->eliminateOneClique(function, this->root(), cache); + if (ret.second->size() != 0) throw runtime_error( + "JuntionTree::eliminate: elimination failed because of factors left over!"); + toc(2, "JT eliminate"); + return ret.first; + } else + return typename BayesTree::sharedClique(); + } } //namespace gtsam diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index fb5b1704b..9037de969 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -27,6 +27,7 @@ #include #include #include +#include namespace gtsam { @@ -64,7 +65,8 @@ namespace gtsam { // recursive elimination function std::pair - eliminateOneClique(const boost::shared_ptr& clique, bool cache=false) const; + eliminateOneClique(typename FG::Eliminate function, + const boost::shared_ptr& clique, bool cache = false) const; // internal constructor void construct(const FG& fg, const VariableIndex& variableIndex); @@ -80,7 +82,8 @@ namespace gtsam { JunctionTree(const FG& fg, const VariableIndex& variableIndex); // eliminate the factors in the subgraphs - typename BayesTree::sharedClique eliminate(bool cache=false) const; + typename BayesTree::sharedClique eliminate(typename FG::Eliminate function, + bool cache = false) const; }; // JunctionTree diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index 9fd3c4fcb..f23ddcd54 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -16,19 +16,15 @@ * Author: Frank Dellaert */ -#include -#include -#include -#include #include +#include #include -#include #include -using namespace std; - namespace gtsam { + using namespace std; + // Explicitly instantiate so we don't have to include everywhere template class FactorGraph; template class BayesNet; @@ -71,13 +67,37 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + IndexFactor::shared_ptr CombineSymbolic( + const FactorGraph& factors, const FastMap >& variableSlots) { + IndexFactor::shared_ptr combined(Combine (factors, + variableSlots)); +// combined->assertInvariants(); + return combined; + } -// /* ************************************************************************* */ -// SymbolicBayesNet -// SymbolicFactorGraph::eliminateFrontals(const Ordering& ordering) -// { -// return Inference::Eliminate(ordering); -// } + /* ************************************************************************* */ + pair::shared_ptr, IndexFactor::shared_ptr> // + EliminateSymbolic(const FactorGraph& factors, size_t nrFrontals) { + + FastSet keys; + BOOST_FOREACH(const IndexFactor::shared_ptr& factor, factors) + BOOST_FOREACH(Index var, *factor) + keys.insert(var); + + if (keys.size() < 1) throw invalid_argument( + "IndexFactor::CombineAndEliminate called on factors with no variables."); + + pair::shared_ptr, IndexFactor::shared_ptr> result; + result.first.reset(new BayesNet ()); + FastSet::const_iterator it; + for (it = keys.begin(); result.first->size() < nrFrontals; ++it) + result.first->push_back(IndexConditional::FromRange(it, keys.end(), 1)); + result.second.reset(new IndexFactor(it, keys.end())); + + return result; + } /* ************************************************************************* */ } diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h index 307813c53..f48aa3ad4 100644 --- a/gtsam/inference/SymbolicFactorGraph.h +++ b/gtsam/inference/SymbolicFactorGraph.h @@ -18,72 +18,75 @@ #pragma once -#include -#include -#include -#include -#include -#include #include #include #include namespace gtsam { -typedef BayesNet SymbolicBayesNet; -typedef EliminationTree SymbolicEliminationTree; + typedef BayesNet SymbolicBayesNet; + typedef EliminationTree SymbolicEliminationTree; -/** Symbolic IndexFactor Graph */ -class SymbolicFactorGraph: public FactorGraph { + /** Symbolic IndexFactor Graph */ + class SymbolicFactorGraph: public FactorGraph { -public: + public: - /** Construct empty factor graph */ - SymbolicFactorGraph() {} + /** Construct empty factor graph */ + SymbolicFactorGraph() { + } - /** Construct from a BayesNet */ - SymbolicFactorGraph(const BayesNet& bayesNet); + /** Construct from a BayesNet */ + SymbolicFactorGraph(const BayesNet& bayesNet); - /** Push back unary factor */ - void push_factor(Index key); + /** Push back unary factor */ + void push_factor(Index key); - /** Push back binary factor */ - void push_factor(Index key1, Index key2); + /** Push back binary factor */ + void push_factor(Index key1, Index key2); - /** Push back ternary factor */ - void push_factor(Index key1, Index key2, Index key3); + /** Push back ternary factor */ + void push_factor(Index key1, Index key2, Index key3); - /** Push back 4-way factor */ - void push_factor(Index key1, Index key2, Index key3, Index key4); + /** Push back 4-way factor */ + void push_factor(Index key1, Index key2, Index key3, Index key4); - /** - * Construct from a factor graph of any type - */ - template - SymbolicFactorGraph(const FactorGraph& fg); + /** + * Construct from a factor graph of any type + */ + template + SymbolicFactorGraph(const FactorGraph& fg); - /** - * Return the set of variables involved in the factors (computes a set - * union). - */ - FastSet keys() const; + /** + * Return the set of variables involved in the factors (computes a set + * union). + */ + FastSet keys() const; + }; - /** - * Same as eliminate in the SymbolicFactorGraph case - */ - // SymbolicBayesNet eliminateFrontals(const Ordering& ordering); -}; + /** Create a combined joint factor (new style for EliminationTree). */ + IndexFactor::shared_ptr CombineSymbolic( + const FactorGraph& factors, const FastMap >& variableSlots); -/* Template function implementation */ -template -SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph& fg) { - for (size_t i = 0; i < fg.size(); i++) { - if(fg[i]) { - IndexFactor::shared_ptr factor(new IndexFactor(*fg[i])); - push_back(factor); - } else - push_back(IndexFactor::shared_ptr()); - } -} + /** + * CombineAndEliminate provides symbolic elimination. + * Combine and eliminate can also be called separately, but for this and + * derived classes calling them separately generally does extra work. + */ + std::pair::shared_ptr, IndexFactor::shared_ptr> + EliminateSymbolic(const FactorGraph&, size_t nrFrontals = 1); + + /* Template function implementation */ + template + SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph& fg) { + for (size_t i = 0; i < fg.size(); i++) { + if (fg[i]) { + IndexFactor::shared_ptr factor(new IndexFactor(*fg[i])); + push_back(factor); + } else + push_back(IndexFactor::shared_ptr()); + } + } } // namespace gtsam diff --git a/gtsam/inference/tests/testEliminationTree.cpp b/gtsam/inference/tests/testEliminationTree.cpp index 0a4d84fd7..7477c1e66 100644 --- a/gtsam/inference/tests/testEliminationTree.cpp +++ b/gtsam/inference/tests/testEliminationTree.cpp @@ -89,7 +89,8 @@ TEST(EliminationTree, eliminate ) fg.push_factor(3, 4); // eliminate - SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); + SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate( + &EliminateSymbolic); CHECK(assert_equal(expected,actual)); } diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp index 21c020f40..ea74a77fb 100644 --- a/gtsam/inference/tests/testJunctionTree.cpp +++ b/gtsam/inference/tests/testJunctionTree.cpp @@ -82,9 +82,10 @@ TEST( JunctionTree, eliminate) fg.push_factor(x3,x4); SymbolicJunctionTree jt(fg); - SymbolicBayesTree::sharedClique actual = jt.eliminate(); + SymbolicBayesTree::sharedClique actual = jt.eliminate(&EliminateSymbolic); - BayesNet bn(*SymbolicSequentialSolver(fg).eliminate()); + BayesNet bn(*SymbolicSequentialSolver(fg).eliminate( + &EliminateSymbolic)); SymbolicBayesTree expected(bn); // cout << "BT from JT:\n"; diff --git a/gtsam/inference/tests/testSymbolicBayesNet.cpp b/gtsam/inference/tests/testSymbolicBayesNet.cpp index ee365fc8d..bdff9116f 100644 --- a/gtsam/inference/tests/testSymbolicBayesNet.cpp +++ b/gtsam/inference/tests/testSymbolicBayesNet.cpp @@ -21,8 +21,8 @@ using namespace boost::assign; #include -//#define GTSAM_MAGIC_KEY - +#include +#ifdef ALL #include using namespace std; @@ -100,6 +100,7 @@ TEST( SymbolicBayesNet, combine ) CHECK(assert_equal(expected,p_ABC)); } +#endif /* ************************************************************************* */ int main() { diff --git a/gtsam/inference/tests/testSymbolicFactor.cpp b/gtsam/inference/tests/testSymbolicFactor.cpp index 39bb1233c..be37b921d 100644 --- a/gtsam/inference/tests/testSymbolicFactor.cpp +++ b/gtsam/inference/tests/testSymbolicFactor.cpp @@ -45,6 +45,7 @@ TEST(SymbolicFactor, constructor) { } /* ************************************************************************* */ +#ifdef TRACK_ELIMINATE TEST(SymbolicFactor, eliminate) { vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; IndexFactor actual(keys.begin(), keys.end()); @@ -62,9 +63,9 @@ TEST(SymbolicFactor, eliminate) { CHECK(assert_equal(**fragmentCond++, *expected1)); CHECK(assert_equal(**fragmentCond++, *expected2)); } - +#endif /* ************************************************************************* */ -TEST(SymbolicFactor, CombineAndEliminate) { +TEST(SymbolicFactor, EliminateSymbolic) { SymbolicFactorGraph factors; factors.push_factor(2,4,6); factors.push_factor(1,2,5); @@ -88,7 +89,7 @@ TEST(SymbolicFactor, CombineAndEliminate) { BayesNet::shared_ptr actual_bn; IndexFactor::shared_ptr actual_factor; - boost::tie(actual_bn, actual_factor) = IndexFactor::CombineAndEliminate(factors, 4); + boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4); CHECK(assert_equal(expected_bn, *actual_bn)); CHECK(assert_equal(expected_factor, *actual_factor)); diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 19bb1234d..72bd1df72 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -16,188 +16,16 @@ * @author Richard Roberts, Christian Potthast */ -#include -#include #include -#include -#include -#include -#include - -#include - -#include - -using namespace std; +#include namespace gtsam { - /* ************************************************************************* */ - GaussianFactor::GaussianFactor(const GaussianConditional& c) : IndexFactor(c) {} + using namespace std; - /* ************************************************************************* */ - pair GaussianFactor::CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals, SolveMethod solveMethod) { - - const bool debug = ISDEBUG("GaussianFactor::CombineAndEliminate"); - - // If any JacobianFactors have constrained noise models, we have to convert - // all factors to JacobianFactors. Otherwise, we can convert all factors - // to HessianFactors. This is because QR can handle constrained noise - // models but Cholesky cannot. - - // Decide whether to use QR or Cholesky - bool useQR; - if(solveMethod == SOLVE_QR) { - useQR = true; - } else if(solveMethod == SOLVE_PREFER_CHOLESKY) { - // Check if any JacobianFactors have constrained noise models. - useQR = false; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if(jacobianFactor && jacobianFactor->get_model()->isConstrained()) { - useQR = true; - break; - } - } - } - - // Convert all factors to the appropriate type and call the type-specific CombineAndEliminate. - if(useQR) { - if(debug) cout << "Using QR:"; - - tic(1, "convert to Jacobian"); - FactorGraph jacobianFactors; - jacobianFactors.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - if(factor) { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if(jacobianFactor) { - jacobianFactors.push_back(jacobianFactor); - if(debug) jacobianFactor->print("Existing JacobianFactor: "); - } else { - HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); - if(!hessianFactor) throw std::invalid_argument( - "In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor."); - jacobianFactors.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*hessianFactor))); - if(debug) { - cout << "Converted HessianFactor to JacobianFactor:\n"; - hessianFactor->print("HessianFactor: "); - jacobianFactors.back()->print("JacobianFactor: "); - } - } - } - } - toc(1, "convert to Jacobian"); - tic(2, "Jacobian CombineAndEliminate"); - pair ret( - JacobianFactor::CombineAndEliminate(jacobianFactors, nrFrontals)); - toc(2, "Jacobian CombineAndEliminate"); - return ret; - - } else { - - const bool checkCholesky = ISDEBUG("GaussianFactor::CombineAndEliminate Check Cholesky"); - -// FactorGraph hessianFactors; -// tic(1, "convert to Hessian"); -// hessianFactors.reserve(factors.size()); -// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { -// if(factor) { -// HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); -// if(hessianFactor) -// hessianFactors.push_back(hessianFactor); -// else { -// JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); -// if(!jacobianFactor) throw std::invalid_argument( -// "In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor."); -// HessianFactor::shared_ptr convertedHessianFactor; -// try { -// convertedHessianFactor.reset(new HessianFactor(*jacobianFactor)); -// if(checkCholesky) -// if(!assert_equal(HessianFactor(*jacobianFactor), HessianFactor(JacobianFactor(*convertedHessianFactor)), 1e-3)) -// throw runtime_error("Conversion between Jacobian and Hessian incorrect"); -// } catch(const exception& e) { -// cout << "Exception converting to Hessian: " << e.what() << endl; -// jacobianFactor->print("jacobianFactor: "); -// convertedHessianFactor->print("convertedHessianFactor: "); -// SETDEBUG("choleskyPartial", true); -// SETDEBUG("choleskyCareful", true); -// HessianFactor(JacobianFactor(*convertedHessianFactor)).print("Jacobian->Hessian->Jacobian->Hessian: "); -// throw; -// } -// hessianFactors.push_back(convertedHessianFactor); -// } -// } -// } -// toc(1, "convert to Hessian"); - - pair ret; - try { - tic(2, "Hessian CombineAndEliminate"); - ret = HessianFactor::CombineAndEliminate(factors, nrFrontals); - toc(2, "Hessian CombineAndEliminate"); - } catch(const exception& e) { - cout << "Exception in HessianFactor::CombineAndEliminate: " << e.what() << endl; - SETDEBUG("HessianFactor::CombineAndEliminate", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - SETDEBUG("choleskyPartial", true); - factors.print("Combining factors: "); - HessianFactor::CombineAndEliminate(factors, nrFrontals); - throw; - } - - if(checkCholesky) { - pair expected; - FactorGraph jacobianFactors; - try { - // Compare with QR - jacobianFactors.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - if(factor) { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if(jacobianFactor) - jacobianFactors.push_back(jacobianFactor); - else { - HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); - if(!hessianFactor) throw std::invalid_argument( - "In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor."); - JacobianFactor::shared_ptr convertedJacobianFactor(new JacobianFactor(*hessianFactor)); - // if(!assert_equal(*hessianFactor, HessianFactor(*convertedJacobianFactor), 1e-3)) - // throw runtime_error("Conversion between Jacobian and Hessian incorrect"); - jacobianFactors.push_back(convertedJacobianFactor); - } - } - } - expected = JacobianFactor::CombineAndEliminate(jacobianFactors, nrFrontals); - } catch(...) { - cout << "Exception in QR" << endl; - throw; - } - - HessianFactor actual_factor(*ret.second); - HessianFactor expected_factor(*expected.second); - if(!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(expected_factor, actual_factor, 1.0)) { - cout << "Cholesky and QR do not agree" << endl; - - SETDEBUG("HessianFactor::CombineAndEliminate", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - jacobianFactors.print("Jacobian Factors: "); - JacobianFactor::CombineAndEliminate(jacobianFactors, nrFrontals); - HessianFactor::CombineAndEliminate(factors, nrFrontals); - factors.print("Combining factors: "); - - throw runtime_error("Cholesky and QR do not agree"); - } - } - - return ret; - } - - } + /* ************************************************************************* */ + GaussianFactor::GaussianFactor(const GaussianConditional& c) : + IndexFactor(c) { + } } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index ba11a1e93..579f2d4f4 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,9 +20,7 @@ #pragma once -#include #include -#include #include #include @@ -75,8 +73,6 @@ namespace gtsam { public: - enum SolveMethod { SOLVE_QR, SOLVE_PREFER_CHOLESKY }; - typedef GaussianConditional ConditionalType; typedef boost::shared_ptr shared_ptr; @@ -96,40 +92,6 @@ namespace gtsam { */ virtual void permuteWithInverse(const Permutation& inversePermutation) = 0; - /** - * Combine and eliminate several factors. - */ - static std::pair >, shared_ptr> CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals=1, SolveMethod solveMethod=SOLVE_QR); - }; // GaussianFactor - - /** unnormalized error */ - template - double gaussianError(const FactorGraph& fg, const VectorValues& x) { - double total_error = 0.; - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) { - total_error += factor->error(x); - } - return total_error; - } - - /** return A*x-b */ - template - Errors gaussianErrors(const FactorGraph& fg, const VectorValues& x) { - return *gaussianErrors_(fg, x); - } - - /** shared pointer version */ - template - boost::shared_ptr gaussianErrors_(const FactorGraph& fg, const VectorValues& x) { - boost::shared_ptr e(new Errors); - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) { - e->push_back(factor->error_vector(x)); - } - return e; - } - - } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b442572b2..8c841beed 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -14,126 +14,482 @@ * @brief Linear Factor Graph where all factors are Gaussians * @author Kai Ni * @author Christian Potthast + * @author Richard Roberts + * @author Frank Dellaert */ -#include - -#include -#include -#include -#include -#include +#include +#include +#include #include #include -#include -#include - using namespace std; using namespace gtsam; -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - namespace gtsam { + // Explicitly instantiate so we don't have to include everywhere + INSTANTIATE_FACTOR_GRAPH(GaussianFactor) + ; - // Explicitly instantiate so we don't have to include everywhere - INSTANTIATE_FACTOR_GRAPH(GaussianFactor); + /* ************************************************************************* */ + GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : + FactorGraph (CBN) { + } + + /* ************************************************************************* */ + GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { + FastSet keys; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) keys.insert(factor->begin(), factor->end()); + return keys; + } + + /* ************************************************************************* */ + void GaussianFactorGraph::permuteWithInverse( + const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) + { + factor->permuteWithInverse(inversePermutation); + } + } + + /* ************************************************************************* */ + void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) { + for (const_iterator factor = lfg.factors_.begin(); factor + != lfg.factors_.end(); factor++) { + push_back(*factor); + } + } + + /* ************************************************************************* */ + GaussianFactorGraph GaussianFactorGraph::combine2( + const GaussianFactorGraph& lfg1, const GaussianFactorGraph& lfg2) { + + // create new linear factor graph equal to the first one + GaussianFactorGraph fg = lfg1; + + // add the second factors_ in the graph + for (const_iterator factor = lfg2.factors_.begin(); factor + != lfg2.factors_.end(); factor++) { + fg.push_back(*factor); + } + return fg; + } + + /* ************************************************************************* */ + std::vector > GaussianFactorGraph::sparseJacobian( + const std::vector& columnIndices) const { + std::vector > entries; + size_t i = 0; + BOOST_FOREACH(const sharedFactor& factor, *this) { + // Convert to JacobianFactor if necessary + JacobianFactor::shared_ptr jacobianFactor( + boost::dynamic_pointer_cast(factor)); + if (!jacobianFactor) { + HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast< + HessianFactor>(factor)); + if (hessian) + jacobianFactor.reset(new JacobianFactor(*hessian)); + else + throw invalid_argument( + "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + + // Add entries, adjusting the row index i + std::vector > factorEntries( + jacobianFactor->sparse(columnIndices)); + entries.reserve(entries.size() + factorEntries.size()); + for (size_t entry = 0; entry < factorEntries.size(); ++entry) + entries.push_back(boost::make_tuple( + factorEntries[entry].get<0> () + i, factorEntries[entry].get< + 1> (), factorEntries[entry].get<2> ())); + + // Increment row index + i += jacobianFactor->size1(); + } + return entries; + } + + // VectorValues GaussianFactorGraph::allocateVectorValuesb() const { + // std::vector dimensions(size()) ; + // Index i = 0 ; + // BOOST_FOREACH( const sharedFactor& factor, factors_) { + // dimensions[i] = factor->numberOfRows() ; + // i++; + // } + // + // return VectorValues(dimensions) ; + // } + // + // void GaussianFactorGraph::getb(VectorValues &b) const { + // Index i = 0 ; + // BOOST_FOREACH( const sharedFactor& factor, factors_) { + // b[i] = factor->getb(); + // i++; + // } + // } + // + // VectorValues GaussianFactorGraph::getb() const { + // VectorValues b = allocateVectorValuesb() ; + // getb(b) ; + // return b ; + // } /* ************************************************************************* */ - GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : - FactorGraph (CBN) { - } + // Helper functions for Combine + static boost::tuple, size_t, size_t> countDims(const std::vector& factors, const VariableSlots& variableSlots) { + #ifndef NDEBUG + vector varDims(variableSlots.size(), numeric_limits::max()); + #else + vector varDims(variableSlots.size()); + #endif + size_t m = 0; + size_t n = 0; + { + Index jointVarpos = 0; + BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { - /* ************************************************************************* */ - GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) { - if(factor) keys.insert(factor->begin(), factor->end()); } - return keys; - } + assert(slots.second.size() == factors.size()); - /* ************************************************************************* */ - void GaussianFactorGraph::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){ - for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){ - push_back(*factor); - } - } - - /* ************************************************************************* */ - GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1, - const GaussianFactorGraph& lfg2) { - - // create new linear factor graph equal to the first one - GaussianFactorGraph fg = lfg1; - - // add the second factors_ in the graph - for (const_iterator factor = lfg2.factors_.begin(); factor - != lfg2.factors_.end(); factor++) { - fg.push_back(*factor); - } - return fg; - } - - /* ************************************************************************* */ - std::vector > - GaussianFactorGraph::sparseJacobian(const std::vector& columnIndices) const { - std::vector > entries; - size_t i = 0; - BOOST_FOREACH(const sharedFactor& factor, *this) { - // Convert to JacobianFactor if necessary - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if(!jacobianFactor) { - HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); - if(hessianFactor) - jacobianFactor.reset(new JacobianFactor(*hessianFactor)); - else - throw invalid_argument("GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); + Index sourceFactorI = 0; + BOOST_FOREACH(const Index sourceVarpos, slots.second) { + if(sourceVarpos < numeric_limits::max()) { + const JacobianFactor& sourceFactor = *factors[sourceFactorI]; + size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); + #ifndef NDEBUG + if(varDims[jointVarpos] == numeric_limits::max()) { + varDims[jointVarpos] = vardim; + n += vardim; + } else + assert(varDims[jointVarpos] == vardim); + #else + varDims[jointVarpos] = vardim; + n += vardim; + break; + #endif + } + ++ sourceFactorI; + } + ++ jointVarpos; + } + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { + m += factor->size1(); } - - // Add entries, adjusting the row index i - std::vector > factorEntries(jacobianFactor->sparse(columnIndices)); - entries.reserve(entries.size() + factorEntries.size()); - for(size_t entry=0; entry()+i, factorEntries[entry].get<1>(), factorEntries[entry].get<2>())); - - // Increment row index - i += jacobianFactor->size1(); } - return entries; + return boost::make_tuple(varDims, m, n); } -// VectorValues GaussianFactorGraph::allocateVectorValuesb() const { -// std::vector dimensions(size()) ; -// Index i = 0 ; -// BOOST_FOREACH( const sharedFactor& factor, factors_) { -// dimensions[i] = factor->numberOfRows() ; -// i++; -// } -// -// return VectorValues(dimensions) ; -// } -// -// void GaussianFactorGraph::getb(VectorValues &b) const { -// Index i = 0 ; -// BOOST_FOREACH( const sharedFactor& factor, factors_) { -// b[i] = factor->getb(); -// i++; -// } -// } -// -// VectorValues GaussianFactorGraph::getb() const { -// VectorValues b = allocateVectorValuesb() ; -// getb(b) ; -// return b ; -// } + /* ************************************************************************* */ + JacobianFactor::shared_ptr CombineJacobians( + const FactorGraph& factors, + const VariableSlots& variableSlots) { + + const bool debug = ISDEBUG("CombineJacobians"); + if (debug) factors.print("Combining factors: "); + if (debug) variableSlots.print(); + + if (debug) cout << "Determine dimensions" << endl; + tic(1, "countDims"); + vector varDims; + size_t m, n; + boost::tie(varDims, m, n) = countDims(factors, variableSlots); + if (debug) { + cout << "Dims: " << m << " x " << n << "\n"; + BOOST_FOREACH(const size_t dim, varDims) cout << dim << " "; + cout << endl; + } + toc(1, "countDims"); + + if (debug) cout << "Sort rows" << endl; + tic(2, "sort rows"); + vector rowSources; + rowSources.reserve(m); + bool anyConstrained = false; + for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) { + const JacobianFactor& sourceFactor(*factors[sourceFactorI]); + sourceFactor.collectInfo(sourceFactorI, rowSources); + if (sourceFactor.isConstrained()) anyConstrained = true; + } + assert(rowSources.size() == m); + std::sort(rowSources.begin(), rowSources.end()); + toc(2, "sort rows"); + + if (debug) cout << "Allocate new factor" << endl; + tic(3, "allocate"); + JacobianFactor::shared_ptr combined(new JacobianFactor()); + combined->allocate(variableSlots, varDims, m); + Vector sigmas(m); + toc(3, "allocate"); + + if (debug) cout << "Copy rows" << endl; + tic(4, "copy rows"); + Index combinedSlot = 0; + BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { + for (size_t row = 0; row < m; ++row) { + const JacobianFactor::_RowSource& info(rowSources[row]); + const JacobianFactor& source(*factors[info.factorI]); + size_t sourceRow = info.factorRowI; + Index sourceSlot = varslot.second[info.factorI]; + combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot); + } + ++combinedSlot; + } + toc(4, "copy rows"); + + if (debug) cout << "Copy rhs (b), sigma, and firstNonzeroBlocks" << endl; + tic(5, "copy vectors"); + for (size_t row = 0; row < m; ++row) { + const JacobianFactor::_RowSource& info(rowSources[row]); + const JacobianFactor& source(*factors[info.factorI]); + const size_t sourceRow = info.factorRowI; + combined->getb()(row) = source.getb()(sourceRow); + sigmas(row) = source.get_model()->sigmas()(sourceRow); + } + combined->copyFNZ(m, variableSlots.size(),rowSources); + toc(5, "copy vectors"); + + if (debug) cout << "Create noise model from sigmas" << endl; + tic(6, "noise model"); + combined->setModel( anyConstrained,sigmas); + toc(6, "noise model"); + + if (debug) cout << "Assert Invariants" << endl; + combined->assertInvariants(); + + return combined; + } + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< + JacobianFactor>& factors, size_t nrFrontals) { + tic(1, "Combine"); + JacobianFactor::shared_ptr jointFactor = + CombineJacobians(factors, VariableSlots(factors)); + toc(1, "Combine"); + tic(2, "eliminate"); + GaussianBayesNet::shared_ptr gbn(jointFactor->eliminate(nrFrontals)); + toc(2, "eliminate"); + return make_pair(gbn, jointFactor); + } + + /* ************************************************************************* */ + static FastMap findScatterAndDims// + (const FactorGraph& factors) { + + static const bool debug = false; + + // The "scatter" is a map from global variable indices to slot indices in the + // union of involved variables. We also include the dimensionality of the + // variable. + + Scatter scatter; + + // First do the set union. + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { + scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); + } + } + + // Next fill in the slot indices (we can only get these after doing the set + // union. + size_t slot = 0; + BOOST_FOREACH(Scatter::value_type& var_slot, scatter) { + var_slot.second.slot = (slot ++); + if(debug) + cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl; + } + + return scatter; + } + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals) { + + const bool debug = ISDEBUG("EliminateCholesky"); + + // Find the scatter and variable dimensions + tic(1, "find scatter"); + Scatter scatter(findScatterAndDims(factors)); + toc(1, "find scatter"); + + // Pull out keys and dimensions + tic(2, "keys"); + vector keys(scatter.size()); + vector dimensions(scatter.size() + 1); + BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { + keys[var_slot.second.slot] = var_slot.first; + dimensions[var_slot.second.slot] = var_slot.second.dimension; + } + // This is for the r.h.s. vector + dimensions.back() = 1; + toc(2, "keys"); + + // Form Ab' * Ab + tic(3, "combine"); + HessianFactor::shared_ptr // + combinedFactor(new HessianFactor(factors, dimensions, scatter)); + toc(3, "combine"); + + // Do Cholesky, note that after this, the lower triangle still contains + // some untouched non-zeros that should be zero. We zero them while + // extracting submatrices next. + tic(4, "partial Cholesky"); + combinedFactor->partialCholesky(nrFrontals); + toc(4, "partial Cholesky"); + + // Extract conditionals and fill in details of the remaining factor + tic(5, "split"); + GaussianBayesNet::shared_ptr conditionals = + combinedFactor->splitEliminatedFactor(nrFrontals, keys); + if (debug) { + conditionals->print("Extracted conditionals: "); + combinedFactor->print("Eliminated factor (L piece): "); + } + toc(5, "split"); + + combinedFactor->assertInvariants(); + return make_pair(conditionals, combinedFactor); + } + + /* ************************************************************************* */ + static FactorGraph convertToJacobians(const FactorGraph< + GaussianFactor>& factors) { + + typedef JacobianFactor J; + typedef HessianFactor H; + + const bool debug = ISDEBUG("convertToJacobians"); + + FactorGraph jacobians; + jacobians.reserve(factors.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) + if (factor) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian) { + jacobians.push_back(jacobian); + if (debug) jacobian->print("Existing JacobianFactor: "); + } else { + H::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + if (!hessian) throw std::invalid_argument( + "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor."); + J::shared_ptr converted(new J(*hessian)); + if (debug) { + if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error( + "convertToJacobians: Conversion between Jacobian and Hessian incorrect"); + cout << "Converted HessianFactor to JacobianFactor:\n"; + hessian->print("HessianFactor: "); + converted->print("JacobianFactor: "); + } + jacobians.push_back(converted); + } + } + return jacobians; + } + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals) { + + const bool debug = ISDEBUG("EliminateQR"); + + // Convert all factors to the appropriate type and call the type-specific EliminateGaussian. + if (debug) cout << "Using QR:"; + + tic(1, "convert to Jacobian"); + FactorGraph jacobians = convertToJacobians(factors); + toc(1, "convert to Jacobian"); + + tic(2, "Jacobian EliminateGaussian"); + GaussianBayesNet::shared_ptr bn; + GaussianFactor::shared_ptr factor; + boost::tie(bn, factor) = EliminateJacobians(jacobians, nrFrontals); + toc(2, "Jacobian EliminateGaussian"); + + return make_pair(bn, factor); + } // EliminateQR + + /* ************************************************************************* */ + GaussianFactorGraph::EliminationResult EliminatePreferCholesky( + const FactorGraph& factors, size_t nrFrontals) { + + typedef JacobianFactor J; + typedef HessianFactor H; + + // If any JacobianFactors have constrained noise models, we have to convert + // all factors to JacobianFactors. Otherwise, we can convert all factors + // to HessianFactors. This is because QR can handle constrained noise + // models but Cholesky cannot. + + // Decide whether to use QR or Cholesky + // Check if any JacobianFactors have constrained noise models. + bool useQR = false; + useQR = false; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian && jacobian->get_model()->isConstrained()) { + useQR = true; + break; + } + } + + // Convert all factors to the appropriate type + // and call the type-specific EliminateGaussian. + if (useQR) return EliminateQR(factors, nrFrontals); + + GaussianFactorGraph::EliminationResult ret; + try { + tic(2, "EliminateCholesky"); + ret = EliminateCholesky(factors, nrFrontals); + toc(2, "EliminateCholesky"); + } catch (const exception& e) { + cout << "Exception in EliminateCholesky: " << e.what() << endl; + SETDEBUG("EliminateCholesky", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + SETDEBUG("choleskyPartial", true); + factors.print("Combining factors: "); + EliminateCholesky(factors, nrFrontals); + throw; + } + + const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky"); + if (checkCholesky) { + GaussianFactorGraph::EliminationResult expected; + FactorGraph jacobians = convertToJacobians(factors); + try { + // Compare with QR + expected = EliminateJacobians(jacobians, nrFrontals); + } catch (...) { + cout << "Exception in QR" << endl; + throw; + } + + H actual_factor(*ret.second); + H expected_factor(*expected.second); + if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal( + expected_factor, actual_factor, 1.0)) { + cout << "Cholesky and QR do not agree" << endl; + + SETDEBUG("EliminateCholesky", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + jacobians.print("Jacobian Factors: "); + EliminateJacobians(jacobians, nrFrontals); + EliminateCholesky(factors, nrFrontals); + factors.print("Combining factors: "); + + throw runtime_error("Cholesky and QR do not agree"); + } + } + + return ret; + + } // EliminatePreferCholesky } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 4d260852a..1e3ac8fd6 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -15,6 +15,8 @@ * @author Kai Ni * @author Christian Potthast * @author Alireza Fathi + * @author Richard Roberts + * @author Frank Dellaert */ #pragma once @@ -25,13 +27,40 @@ #include #include #include -#include #include +#include #include -#include namespace gtsam { + class SharedDiagonal; + + /** unnormalized error */ + template + double gaussianError(const FactorGraph& fg, const VectorValues& x) { + double total_error = 0.; + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) { + total_error += factor->error(x); + } + return total_error; + } + + /** return A*x-b */ + template + Errors gaussianErrors(const FactorGraph& fg, const VectorValues& x) { + return *gaussianErrors_(fg, x); + } + + /** shared pointer version */ + template + boost::shared_ptr gaussianErrors_(const FactorGraph& fg, const VectorValues& x) { + boost::shared_ptr e(new Errors); + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) { + e->push_back(factor->error_vector(x)); + } + return e; + } + /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == GaussianFactor @@ -138,4 +167,26 @@ namespace gtsam { }; + /** + * Combine and eliminate several factors. + */ + JacobianFactor::shared_ptr CombineJacobians( + const FactorGraph& factors, + const VariableSlots& variableSlots); + + GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< + JacobianFactor>& factors, size_t nrFrontals = 1); + + GaussianFactorGraph::EliminationResult EliminateHessians(const FactorGraph< + HessianFactor>& factors, size_t nrFrontals = 1); + + GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals = 1); + + GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals = 1); + + GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< + GaussianFactor>& factors, size_t nrFrontals = 1); + } // namespace gtsam diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index d54e4cc31..c10584f1c 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -28,14 +28,27 @@ namespace ublas = boost::numeric::ublas; namespace gtsam { +/* ************************************************************************* */ +GaussianFactor::shared_ptr GaussianISAM::marginalFactor(Index j) const { + return Super::marginalFactor(j, &EliminateQR); +} + +/* ************************************************************************* */ +BayesNet::shared_ptr GaussianISAM::marginalBayesNet(Index j) const { + return Super::marginalBayesNet(j, &EliminateQR); +} + /* ************************************************************************* */ std::pair GaussianISAM::marginal(Index j) const { - GaussianFactor::shared_ptr factor = this->marginalFactor(j); - FactorGraph graph; - graph.push_back(factor); - GaussianConditional::shared_ptr conditional = GaussianFactor::CombineAndEliminate(graph,1).first->front(); - Matrix R = conditional->get_R(); - return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R))); + GaussianConditional::shared_ptr conditional = marginalBayesNet(j)->front(); + Matrix R = conditional->get_R(); + return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R))); +} + +/* ************************************************************************* */ + BayesNet::shared_ptr GaussianISAM::jointBayesNet( + Index key1, Index key2) const { + return Super::jointBayesNet(key1, key2, &EliminateQR); } /* ************************************************************************* */ @@ -60,4 +73,10 @@ VectorValues optimize(const GaussianISAM& bayesTree) { return result; } +/* ************************************************************************* */ +BayesNet GaussianISAM::shortcut(sharedClique clique, sharedClique root) { + return clique->shortcut(root,&EliminateQR); +} +/* ************************************************************************* */ + } /// namespace gtsam diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index 732742f8c..ddb4255bf 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -19,29 +19,30 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { class GaussianISAM : public ISAM { + typedef ISAM Super; std::deque > dims_; public: /** Create an empty Bayes Tree */ - GaussianISAM() : ISAM() {} + GaussianISAM() : Super() {} /** Create a Bayes Tree from a Bayes Net */ - GaussianISAM(const GaussianBayesNet& bayesNet) : ISAM(bayesNet) {} + GaussianISAM(const GaussianBayesNet& bayesNet) : Super(bayesNet) {} /** Override update_internal to also keep track of variable dimensions. */ template void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) { - ISAM::update_internal(newFactors, orphans); + Super::update_internal(newFactors, orphans, &EliminateQR); // update dimensions BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) { @@ -63,15 +64,22 @@ public: } void clear() { - ISAM::clear(); + Super::clear(); dims_.clear(); } friend VectorValues optimize(const GaussianISAM&); - /** return marginal on any variable */ + /** return marginal on any variable as a factor, Bayes net, or mean/cov */ + GaussianFactor::shared_ptr marginalFactor(Index j) const; + BayesNet::shared_ptr marginalBayesNet(Index key) const; std::pair marginal(Index key) const; + /** return joint between two variables, as a Bayes net */ + BayesNet::shared_ptr jointBayesNet(Index key1, Index key2) const; + + /** return the conditional P(S|Root) on the separator given the root */ + static BayesNet shortcut(sharedClique clique, sharedClique root); }; // recursively optimize this conditional and all subtrees diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index 61ec7fd26..ab23470a6 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -34,9 +34,6 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - /** - * GaussianJunctionTree - */ void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr& current, VectorValues& config) const { // solve the bayes net in the current node GaussianBayesNet::const_reverse_iterator it = current->rbegin(); @@ -64,10 +61,10 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues GaussianJunctionTree::optimize() const { + VectorValues GaussianJunctionTree::optimize(Eliminate function) const { tic(1, "GJT eliminate"); // eliminate from leaves to the root - boost::shared_ptr rootClique(this->eliminate()); + boost::shared_ptr rootClique(this->eliminate(function)); toc(1, "GJT eliminate"); // Allocate solution vector @@ -84,4 +81,5 @@ namespace gtsam { return result; } + /* ************************************************************************* */ } //namespace gtsam diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index ed5e08d5d..09e996754 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -32,10 +32,12 @@ namespace gtsam { * GaussianJunctionTree that does the optimization */ class GaussianJunctionTree: public JunctionTree { + public: typedef boost::shared_ptr shared_ptr; typedef JunctionTree Base; typedef Base::sharedClique sharedClique; + typedef GaussianFactorGraph::Eliminate Eliminate; protected: // back-substitute in topological sort order (parents first) @@ -53,7 +55,8 @@ namespace gtsam { GaussianJunctionTree(const GaussianFactorGraph& fg, const VariableIndex& variableIndex) : Base(fg, variableIndex) {} // optimize the linear graph - VectorValues optimize() const; + VectorValues optimize(Eliminate function) const; + }; // GaussianJunctionTree } // namespace gtsam diff --git a/gtsam/linear/GaussianMultifrontalSolver.cpp b/gtsam/linear/GaussianMultifrontalSolver.cpp index c1e16b826..8f7be46aa 100644 --- a/gtsam/linear/GaussianMultifrontalSolver.cpp +++ b/gtsam/linear/GaussianMultifrontalSolver.cpp @@ -48,32 +48,32 @@ void GaussianMultifrontalSolver::replaceFactors(const FactorGraph::shared_ptr GaussianMultifrontalSolver::eliminate() const { - return Base::eliminate(); + return Base::eliminate(&EliminateQR); } /* ************************************************************************* */ VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const { tic(2,"optimize"); - VectorValues::shared_ptr values(new VectorValues(junctionTree_->optimize())); + VectorValues::shared_ptr values(new VectorValues(junctionTree_->optimize(&EliminateQR))); toc(2,"optimize"); return values; } /* ************************************************************************* */ GaussianFactorGraph::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector& js) const { - return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js))); + return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminateQR))); } /* ************************************************************************* */ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const { - return Base::marginalFactor(j); + return Base::marginalFactor(j,&EliminateQR); } /* ************************************************************************* */ std::pair GaussianMultifrontalSolver::marginalCovariance(Index j) const { FactorGraph fg; - fg.push_back(Base::marginalFactor(j)); - GaussianConditional::shared_ptr conditional = GaussianFactor::CombineAndEliminate(fg,1).first->front(); + fg.push_back(Base::marginalFactor(j,&EliminateQR)); + GaussianConditional::shared_ptr conditional = EliminateQR(fg,1).first->front(); Matrix R = conditional->get_R(); return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R))); } diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp index c2d91ed1b..461f78f04 100644 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ b/gtsam/linear/GaussianSequentialSolver.cpp @@ -49,7 +49,7 @@ void GaussianSequentialSolver::replaceFactors(const FactorGraph: /* ************************************************************************* */ GaussianBayesNet::shared_ptr GaussianSequentialSolver::eliminate() const { - return Base::eliminate(); + return Base::eliminate(&EliminateQR); } /* ************************************************************************* */ @@ -82,20 +82,23 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const { /* ************************************************************************* */ GaussianFactor::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const { - return Base::marginalFactor(j); -} - -std::pair GaussianSequentialSolver::marginalCovariance(Index j) const { - FactorGraph fg; - fg.push_back(Base::marginalFactor(j)); - GaussianConditional::shared_ptr conditional = GaussianFactor::CombineAndEliminate(fg,1).first->front(); - Matrix R = conditional->get_R(); - return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R))); + return Base::marginalFactor(j,&EliminateQR); } /* ************************************************************************* */ -GaussianFactorGraph::shared_ptr GaussianSequentialSolver::jointFactorGraph(const std::vector& js) const { - return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js))); +std::pair GaussianSequentialSolver::marginalCovariance(Index j) const { + FactorGraph fg; + fg.push_back(Base::marginalFactor(j, &EliminateQR)); + GaussianConditional::shared_ptr conditional = EliminateQR(fg, 1).first->front(); + Matrix R = conditional->get_R(); + return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R),R))); +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr +GaussianSequentialSolver::jointFactorGraph(const std::vector& js) const { + return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph( + *Base::jointFactorGraph(js, &EliminateQR))); } } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index d77bfcc87..452a7a87b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -173,6 +173,40 @@ namespace gtsam { } /* ************************************************************************* */ + HessianFactor::HessianFactor(const FactorGraph& factors, + const vector& dimensions, const Scatter& scatter) : + info_(matrix_) { + + const bool debug = ISDEBUG("EliminateCholesky"); + // Form Ab' * Ab + tic(1, "allocate"); + info_.resize(dimensions.begin(), dimensions.end(), false); + toc(1, "allocate"); + tic(2, "zero"); + ublas::noalias(matrix_) = ublas::zero_matrix(matrix_.size1(),matrix_.size2()); + toc(2, "zero"); + tic(3, "update"); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) + { + shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + if (hessian) + updateATA(*hessian, scatter); + else { + JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); + if (jacobianFactor) + updateATA(*jacobianFactor, scatter); + else + throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); + } + } + toc(3, "update"); + + if (debug) gtsam::print(matrix_, "Ab' * Ab: "); + + assertInvariants(); + } + + /* ************************************************************************* */ void HessianFactor::print(const std::string& s) const { cout << s << "\n"; cout << " keys: "; @@ -202,36 +236,6 @@ namespace gtsam { 2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0)); } - /* ************************************************************************* */ - static FastMap findScatterAndDims(const FactorGraph& factors) { - - static const bool debug = false; - - // The "scatter" is a map from global variable indices to slot indices in the - // union of involved variables. We also include the dimensionality of the - // variable. - - Scatter scatter; - - // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { - scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); - } - } - - // Next fill in the slot indices (we can only get these after doing the set - // union. - size_t slot = 0; - BOOST_FOREACH(Scatter::value_type& var_slot, scatter) { - var_slot.second.slot = (slot ++); - if(debug) - cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl; - } - - return scatter; - } - void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) { // This function updates 'combined' with the information in 'update'. @@ -433,7 +437,14 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt // toc(2, "update"); } -GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& keys) { +/* ************************************************************************* */ +void HessianFactor::partialCholesky(size_t nrFrontals) { + choleskyPartial(matrix_, info_.offset(nrFrontals)); +} + +/* ************************************************************************* */ +GaussianBayesNet::shared_ptr +HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& keys) { static const bool debug = false; @@ -482,75 +493,4 @@ GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFront return conditionals; } -/* ************************************************************************* */ -pair HessianFactor::CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals) { - - const bool debug = ISDEBUG("HessianFactor::CombineAndEliminate"); - - // Find the scatter and variable dimensions - tic(1, "find scatter"); - Scatter scatter(findScatterAndDims(factors)); - toc(1, "find scatter"); - - // Pull out keys and dimensions - tic(2, "keys"); - vector keys(scatter.size()); - vector dimensions(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - keys[var_slot.second.slot] = var_slot.first; - dimensions[var_slot.second.slot] = var_slot.second.dimension; - } - // This is for the r.h.s. vector - dimensions.back() = 1; - toc(2, "keys"); - - // Form Ab' * Ab - tic(3, "combine"); - tic(1, "allocate"); - HessianFactor::shared_ptr combinedFactor(new HessianFactor()); - combinedFactor->info_.resize(dimensions.begin(), dimensions.end(), false); - toc(1, "allocate"); - tic(2, "zero"); - ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2()); - toc(2, "zero"); - tic(3, "update"); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); - if(hessianFactor) - combinedFactor->updateATA(*hessianFactor, scatter); - else { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if(jacobianFactor) - combinedFactor->updateATA(*jacobianFactor, scatter); - else - throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - } - } - toc(3, "update"); - if(debug) gtsam::print(combinedFactor->matrix_, "Ab' * Ab: "); - toc(3, "combine"); - - // Do Cholesky, note that after this, the lower triangle still contains - // some untouched non-zeros that should be zero. We zero them while - // extracting submatrices next. - tic(4, "partial Cholesky"); - choleskyPartial(combinedFactor->matrix_, combinedFactor->info_.offset(nrFrontals)); - if(debug) gtsam::print(combinedFactor->matrix_, "chol(Ab' * Ab): "); - toc(4, "partial Cholesky"); - - // Extract conditionals and fill in details of the remaining factor - tic(5, "split"); - GaussianBayesNet::shared_ptr conditionals(combinedFactor->splitEliminatedFactor(nrFrontals, keys)); - if(debug) { - conditionals->print("Extracted conditionals: "); - combinedFactor->print("Eliminated factor (L piece): "); - } - toc(5, "split"); - - combinedFactor->assertInvariants(); - - return make_pair(conditionals, combinedFactor); -} - -} +} // gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 39d0ebfbe..2fd1e8b5f 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -20,6 +20,7 @@ #include #include +#include #include // Forward declarations for friend unit tests @@ -49,8 +50,6 @@ namespace gtsam { InfoMatrix matrix_; // The full information matrix [A b]^T * [A b] BlockInfo info_; // The block view of the full information matrix. - void assertInvariants() const; - boost::shared_ptr > splitEliminatedFactor(size_t nrFrontals, const std::vector& keys); void updateATA(const JacobianFactor& update, const Scatter& scatter); void updateATA(const HessianFactor& update, const Scatter& scatter); @@ -96,7 +95,12 @@ namespace gtsam { /** Convert from a JacobianFactor or HessianFactor (computes A^T * A) */ HessianFactor(const GaussianFactor& factor); - virtual ~HessianFactor() {} + /** Special constructor used in EliminateCholesky */ + HessianFactor(const FactorGraph& factors, + const std::vector& dimensions, const Scatter& scatter); + + virtual ~HessianFactor() { + } // Implementing Testable interface virtual void print(const std::string& s = "") const; @@ -121,13 +125,7 @@ namespace gtsam { * variable. The order of the variables within the factor is not changed. */ virtual void permuteWithInverse(const Permutation& inversePermutation) { - Factor::permuteWithInverse(inversePermutation); } - - /** - * Combine and eliminate several factors. - */ - static std::pair >, shared_ptr> CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals=1); + IndexFactor::permuteWithInverse(inversePermutation); } // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; @@ -135,6 +133,21 @@ namespace gtsam { // Friend JacobianFactor for conversion friend class JacobianFactor; + // used in eliminateCholesky: + + /** + * Do Cholesky. Note that after this, the lower triangle still contains + * some untouched non-zeros that should be zero. We zero them while + * extracting submatrices in splitEliminatedFactor. Frank says :-( + */ + void partialCholesky(size_t nrFrontals); + + /** split partially eliminated factor */ + boost::shared_ptr > splitEliminatedFactor( + size_t nrFrontals, const std::vector& keys); + + /** assert invariants */ + void assertInvariants() const; }; } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 1f67e588c..9b783d84f 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -493,175 +493,68 @@ namespace gtsam { } /* ************************************************************************* */ - /* Used internally by JacobianFactor::Combine for sorting */ - struct _RowSource { - size_t firstNonzeroVar; - size_t factorI; - size_t factorRowI; - _RowSource(size_t _firstNonzeroVar, size_t _factorI, size_t _factorRowI) : - firstNonzeroVar(_firstNonzeroVar), factorI(_factorI), factorRowI(_factorRowI) {} - bool operator<(const _RowSource& o) const { return firstNonzeroVar < o.firstNonzeroVar; } - }; + void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const { + assertInvariants(); + for (size_t row = 0; row < size1(); ++row) { + Index firstNonzeroVar; + if (firstNonzeroBlocks_[row] < size()) { + firstNonzeroVar = keys_[firstNonzeroBlocks_[row]]; + } else if (firstNonzeroBlocks_[row] == size()) { + firstNonzeroVar = back() + 1; + } else { + assert(false); + } + rowSources.push_back(_RowSource(firstNonzeroVar, index, row)); + } + } /* ************************************************************************* */ - // Helper functions for Combine - static boost::tuple, size_t, size_t> countDims(const std::vector& factors, const VariableSlots& variableSlots) { - #ifndef NDEBUG - vector varDims(variableSlots.size(), numeric_limits::max()); - #else - vector varDims(variableSlots.size()); - #endif - size_t m = 0; - size_t n = 0; - { - Index jointVarpos = 0; - BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { - - assert(slots.second.size() == factors.size()); - - Index sourceFactorI = 0; - BOOST_FOREACH(const Index sourceVarpos, slots.second) { - if(sourceVarpos < numeric_limits::max()) { - const JacobianFactor& sourceFactor = *factors[sourceFactorI]; - size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); - #ifndef NDEBUG - if(varDims[jointVarpos] == numeric_limits::max()) { - varDims[jointVarpos] = vardim; - n += vardim; - } else - assert(varDims[jointVarpos] == vardim); - #else - varDims[jointVarpos] = vardim; - n += vardim; - break; - #endif - } - ++ sourceFactorI; - } - ++ jointVarpos; - } - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { - m += factor->size1(); - } - } - return boost::make_tuple(varDims, m, n); - } + void JacobianFactor::allocate(const VariableSlots& variableSlots, vector< + size_t>& varDims, size_t m) { + keys_.resize(variableSlots.size()); + std::transform(variableSlots.begin(), variableSlots.end(), keys_.begin(), + bind(&VariableSlots::const_iterator::value_type::first, + boost::lambda::_1)); + varDims.push_back(1); + Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m)); + firstNonzeroBlocks_.resize(m); + } /* ************************************************************************* */ - JacobianFactor::shared_ptr JacobianFactor::Combine(const FactorGraph& factors, const VariableSlots& variableSlots) { - - const bool debug = ISDEBUG("JacobianFactor::Combine"); - - if(debug) factors.print("Combining factors: "); - - if(debug) variableSlots.print(); - - // Determine dimensions - tic(1, "countDims"); - vector varDims; - size_t m; - size_t n; - boost::tie(varDims, m, n) = countDims(factors, variableSlots); - if(debug) { - cout << "Dims: " << m << " x " << n << "\n"; - BOOST_FOREACH(const size_t dim, varDims) { cout << dim << " "; } - cout << endl; - } - toc(1, "countDims"); - - // Sort rows - tic(2, "sort rows"); - vector<_RowSource> rowSources; rowSources.reserve(m); - bool anyConstrained = false; - for(size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) { - const JacobianFactor& sourceFactor(*factors[sourceFactorI]); - sourceFactor.assertInvariants(); - for(size_t sourceFactorRow = 0; sourceFactorRow < sourceFactor.size1(); ++sourceFactorRow) { - Index firstNonzeroVar; - if(sourceFactor.firstNonzeroBlocks_[sourceFactorRow] < sourceFactor.size()) - firstNonzeroVar = sourceFactor.keys_[sourceFactor.firstNonzeroBlocks_[sourceFactorRow]]; - else if(sourceFactor.firstNonzeroBlocks_[sourceFactorRow] == sourceFactor.size()) - firstNonzeroVar = sourceFactor.back() + 1; - else - assert(false); - rowSources.push_back(_RowSource(firstNonzeroVar, sourceFactorI, sourceFactorRow)); - } - if(sourceFactor.model_->isConstrained()) anyConstrained = true; - } - assert(rowSources.size() == m); - std::sort(rowSources.begin(), rowSources.end()); - toc(2, "sort rows"); - - // Allocate new factor - tic(3, "allocate"); - shared_ptr combined(new JacobianFactor()); - combined->keys_.resize(variableSlots.size()); - std::transform(variableSlots.begin(), variableSlots.end(), combined->keys_.begin(), bind(&VariableSlots::const_iterator::value_type::first, boost::lambda::_1)); - varDims.push_back(1); - combined->Ab_.copyStructureFrom(BlockAb(combined->matrix_, varDims.begin(), varDims.end(), m)); - combined->firstNonzeroBlocks_.resize(m); - Vector sigmas(m); - toc(3, "allocate"); - - // Copy rows - tic(4, "copy rows"); - Index combinedSlot = 0; - BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { - for(size_t row = 0; row < m; ++row) { - const Index sourceSlot = varslot.second[rowSources[row].factorI]; - ABlock combinedBlock(combined->Ab_(combinedSlot)); - if(sourceSlot != numeric_limits::max()) { - const JacobianFactor& source(*factors[rowSources[row].factorI]); - const size_t sourceRow = rowSources[row].factorRowI; - if(source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) { - const constABlock sourceBlock(source.Ab_(sourceSlot)); - ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow); - } else - ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector(combinedBlock.size2()); - } else - ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector(combinedBlock.size2()); - } - ++ combinedSlot; - } - toc(4, "copy rows"); - - // Copy rhs (b), sigma, and firstNonzeroBlocks - tic(5, "copy vectors"); - Index firstNonzeroSlot = 0; - for(size_t row = 0; row < m; ++row) { - const JacobianFactor& source(*factors[rowSources[row].factorI]); - const size_t sourceRow = rowSources[row].factorRowI; - combined->getb()(row) = source.getb()(sourceRow); - sigmas(row) = source.get_model()->sigmas()(sourceRow); - while(firstNonzeroSlot < variableSlots.size() && rowSources[row].firstNonzeroVar > combined->keys_[firstNonzeroSlot]) - ++ firstNonzeroSlot; - combined->firstNonzeroBlocks_[row] = firstNonzeroSlot; - } - toc(5, "copy vectors"); - - // Create noise model from sigmas - tic(6, "noise model"); - if(anyConstrained) combined->model_ = noiseModel::Constrained::MixedSigmas(sigmas); - else combined->model_ = noiseModel::Diagonal::Sigmas(sigmas); - toc(6, "noise model"); - - combined->assertInvariants(); - - return combined; - } + void JacobianFactor::copyRow(const JacobianFactor& source, Index sourceRow, + size_t sourceSlot, size_t row, Index slot) { + ABlock combinedBlock(Ab_(slot)); + if (sourceSlot != numeric_limits::max()) { + if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) { + const constABlock sourceBlock(source.Ab_(sourceSlot)); + ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row( + sourceBlock, sourceRow); + } else + ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector< + double>(combinedBlock.size2()); + } else + ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector< + double>(combinedBlock.size2()); + } /* ************************************************************************* */ - pair JacobianFactor::CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals) { - tic(1, "Combine"); - shared_ptr jointFactor(Combine(factors, VariableSlots(factors))); - toc(1, "Combine"); - tic(2, "eliminate"); - GaussianBayesNet::shared_ptr gbn(jointFactor->eliminate(nrFrontals)); - toc(2, "eliminate"); - return make_pair(gbn, jointFactor); - } + void JacobianFactor::copyFNZ(size_t m, size_t n, + vector<_RowSource>& rowSources) { + Index i = 0; + for (size_t row = 0; row < m; ++row) { + while (i < n && rowSources[row].firstNonzeroVar > keys_[i]) + ++i; + firstNonzeroBlocks_[row] = i; + } + } + /* ************************************************************************* */ + void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) { + if (anyConstrained) + model_ = noiseModel::Constrained::MixedSigmas(sigmas); + else + model_ = noiseModel::Diagonal::Sigmas(sigmas); + } /* ************************************************************************* */ Errors operator*(const FactorGraph& fg, const VectorValues& x) { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 46c238de0..bb0065d6a 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -58,10 +59,6 @@ namespace gtsam { typedef BlockAb::Column BVector; typedef BlockAb::constColumn constBVector; - protected: - void assertInvariants() const; - static JacobianFactor::shared_ptr Combine(const FactorGraph& factors, const VariableSlots& variableSlots); - public: /** Copy constructor */ @@ -116,6 +113,9 @@ namespace gtsam { */ bool empty() const { return Ab_.size1() == 0;} + /** is noise model constrained ? */ + bool isConstrained() const { return model_->isConstrained();} + /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? */ @@ -194,12 +194,6 @@ namespace gtsam { * model. */ JacobianFactor whiten() const; - /** - * Combine and eliminate several factors. - */ - static std::pair >, shared_ptr> CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals=1); - boost::shared_ptr eliminateFirst(); boost::shared_ptr > eliminate(size_t nrFrontals = 1); @@ -210,8 +204,43 @@ namespace gtsam { // Friend unit tests (see also forward declarations above) friend class ::Combine2GaussianFactorTest; friend class ::eliminateFrontalsGaussianFactorTest; - }; + /* Used by ::CombineJacobians for sorting */ + struct _RowSource { + size_t firstNonzeroVar; + size_t factorI; + size_t factorRowI; + _RowSource(size_t _firstNonzeroVar, size_t _factorI, size_t _factorRowI) : + firstNonzeroVar(_firstNonzeroVar), factorI(_factorI), factorRowI(_factorRowI) {} + bool operator<(const _RowSource& o) const { + return firstNonzeroVar < o.firstNonzeroVar; + } + }; + + // following methods all used in CombineJacobians: + // Many imperative, perhaps all need to be combined in constructor + + /** Collect information on Jacobian rows */ + void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const; + + /** allocate space */ + void allocate(const VariableSlots& variableSlots, + std::vector& varDims, size_t m); + + /** copy a slot from source */ + void copyRow(const JacobianFactor& source, + Index sourceRow, size_t sourceSlot, size_t row, Index slot); + + /** copy firstNonzeroBlocks structure */ + void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources); + + /** set noiseModel correctly */ + void setModel(bool anyConstrained, const Vector& sigmas); + + /** Assert invariants after elimination */ + void assertInvariants() const; + + }; // JacobianFactor /** return A*x */ Errors operator*(const FactorGraph& fg, const VectorValues& x); @@ -237,5 +266,5 @@ namespace gtsam { void multiply(const FactorGraph& fg, const VectorValues &x, VectorValues &r); void transposeMultiply(const FactorGraph& fg, const VectorValues &r, VectorValues &x); -} +} // gtsam diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index a5825da1d..0af811c99 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -31,7 +31,7 @@ sources += GaussianJunctionTree.cpp sources += GaussianConditional.cpp GaussianBayesNet.cpp sources += GaussianISAM.cpp check_PROGRAMS += tests/testHessianFactor tests/testGaussianFactor tests/testGaussianConditional -check_PROGRAMS += tests/testGaussianJunctionTree +check_PROGRAMS += tests/testGaussianFactorGraph tests/testGaussianJunctionTree # Iterative Methods headers += iterative-inl.h diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index ebcb74e82..7d4da0c2f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include using namespace std; diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h index babee1cec..ca13fc42c 100644 --- a/gtsam/linear/SubgraphSolver-inl.h +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -80,7 +80,8 @@ void SubgraphSolver::replaceFactors(const typename LINEAR:: // theta_bar_ = composePoses (T_, tree, theta0[root]); LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! - SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree::Create(sacrificialAb1)->eliminate(); + SubgraphPreconditioner::sharedBayesNet Rc1 = + EliminationTree::Create(sacrificialAb1)->eliminate(&EliminateQR); SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); pc_ = boost::make_shared( diff --git a/gtsam/linear/tests/testGaussianFactor.cpp b/gtsam/linear/tests/testGaussianFactor.cpp index 185e0fa93..ef10ea521 100644 --- a/gtsam/linear/tests/testGaussianFactor.cpp +++ b/gtsam/linear/tests/testGaussianFactor.cpp @@ -16,32 +16,14 @@ * @author Frank Dellaert **/ -#include -#include - -#include -#include // for operator += -#include -#include // for insert -using namespace boost::assign; - #include #include -//#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include #include -#include -#include +#include using namespace std; using namespace gtsam; -using namespace boost; namespace ublas = boost::numeric::ublas; static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8; @@ -87,302 +69,37 @@ TEST(GaussianFactor, constructor2) EXPECT(assert_equal(b, actualb)); } -///* ************************************************************************* */ -//TEST(GaussianFactor, Combine) -//{ -// Matrix A00 = Matrix_(3,3, -// 1.0, 0.0, 0.0, -// 0.0, 1.0, 0.0, -// 0.0, 0.0, 1.0); -// Vector b0 = Vector_(3, 0.0, 0.0, 0.0); -// Vector s0 = Vector_(3, 0.0, 0.0, 0.0); -// -// Matrix A10 = Matrix_(3,3, -// 0.0, -2.0, -4.0, -// 2.0, 0.0, 2.0, -// 0.0, 0.0, -10.0); -// Matrix A11 = Matrix_(3,3, -// 2.0, 0.0, 0.0, -// 0.0, 2.0, 0.0, -// 0.0, 0.0, 10.0); -// Vector b1 = Vector_(3, 6.0, 2.0, 0.0); -// Vector s1 = Vector_(3, 1.0, 1.0, 1.0); -// -// Matrix A20 = Matrix_(3,3, -// 1.0, 0.0, 0.0, -// 0.0, 1.0, 0.0, -// 0.0, 0.0, 1.0); -// Vector b2 = Vector_(3, 0.0, 0.0, 0.0); -// Vector s2 = Vector_(3, 100.0, 100.0, 100.0); -// -// GaussianFactorGraph gfg; -// gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true)); -// gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); -// gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true)); -// -// GaussianVariableIndex varindex(gfg); -// vector factors(3); factors[0]=0; factors[1]=1; factors[2]=2; -// vector variables(2); variables[0]=0; variables[1]=1; -// vector > variablePositions(3); -// variablePositions[0].resize(1); variablePositions[0][0]=0; -// variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1; -// variablePositions[2].resize(1); variablePositions[2][0]=0; -// -// JacobianFactor actual = *JacobianFactor::Combine(gfg, varindex, factors, variables, variablePositions); -// -// Matrix zero3x3 = zeros(3,3); -// Matrix A0 = gtsam::stack(3, &A00, &A10, &A20); -// Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3); -// Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); -// Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); -// -// JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); -// -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ -TEST(GaussianFactor, Combine2) +#ifdef BROKEN +TEST(GaussianFactor, operators ) { - Matrix A01 = Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); + Matrix I = eye(2); + Vector b = Vector_(2,0.2,-0.1); + JacobianFactor lf(_x1_, -I, _x2_, I, b, sigma0_1); - Matrix A10 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); + VectorValues c; + c[_x1_] = Vector_(2,10.,20.); + c[_x2_] = Vector_(2,30.,60.); - Matrix A21 = Matrix_(3,3, - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); + // test A*x + Vector expectedE = Vector_(2,200.,400.), e = lf*c; + EXPECT(assert_equal(expectedE,e)); - GaussianFactorGraph gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + // test A^e + VectorValues expectedX; + expectedX[_x1_] = Vector_(2,-2000.,-4000.); + expectedX[_x2_] = Vector_(2, 2000., 4000.); + EXPECT(assert_equal(expectedX,lf^e)); - JacobianFactor actual = *JacobianFactor::Combine(*gfg.dynamicCastFactors >(), VariableSlots(gfg)); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); - - JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - - EXPECT(assert_equal(expected, actual)); + // test transposeMultiplyAdd + VectorValues x; + x[_x1_] = Vector_(2, 1.,2.); + x[_x2_] = Vector_(2, 3.,4.); + VectorValues expectedX2 = x + 0.1 * (lf^e); + lf.transposeMultiplyAdd(0.1,e,x); + EXPECT(assert_equal(expectedX2,x)); } - -/* ************************************************************************* */ -TEST_UNSAFE(GaussianFactor, CombineAndEliminate) -{ - Matrix A01 = Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); - - Matrix A10 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); - - Matrix A21 = Matrix_(3,3, - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); - - GaussianFactorGraph gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); - - JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - GaussianBayesNet expectedBN(*expectedFactor.eliminate(1)); - - pair actualQR(JacobianFactor::CombineAndEliminate( - *gfg.dynamicCastFactors >(), 1)); - - EXPECT(assert_equal(expectedBN, *actualQR.first)); - EXPECT(assert_equal(expectedFactor, *actualQR.second)); -} - -///* ************************************************************************* */ -//TEST(GaussianFactor, operators ) -//{ -// Matrix I = eye(2); -// Vector b = Vector_(2,0.2,-0.1); -// JacobianFactor lf(_x1_, -I, _x2_, I, b, sigma0_1); -// -// VectorValues c; -// c.insert(_x1_,Vector_(2,10.,20.)); -// c.insert(_x2_,Vector_(2,30.,60.)); -// -// // test A*x -// Vector expectedE = Vector_(2,200.,400.), e = lf*c; -// EXPECT(assert_equal(expectedE,e)); -// -// // test A^e -// VectorValues expectedX; -// expectedX.insert(_x1_,Vector_(2,-2000.,-4000.)); -// expectedX.insert(_x2_,Vector_(2, 2000., 4000.)); -// EXPECT(assert_equal(expectedX,lf^e)); -// -// // test transposeMultiplyAdd -// VectorValues x; -// x.insert(_x1_,Vector_(2, 1.,2.)); -// x.insert(_x2_,Vector_(2, 3.,4.)); -// VectorValues expectedX2 = x + 0.1 * (lf^e); -// lf.transposeMultiplyAdd(0.1,e,x); -// EXPECT(assert_equal(expectedX2,x)); -//} - -///* ************************************************************************* */ -//TEST( NonlinearFactorGraph, combine2){ -// double sigma1 = 0.0957; -// Matrix A11(2,2); -// A11(0,0) = 1; A11(0,1) = 0; -// A11(1,0) = 0; A11(1,1) = 1; -// Vector b(2); -// b(0) = 2; b(1) = -1; -// JacobianFactor::shared_ptr f1(new JacobianFactor(_x1_, A11, b*sigma1, sharedSigma(2,sigma1))); -// -// double sigma2 = 0.5; -// A11(0,0) = 1; A11(0,1) = 0; -// A11(1,0) = 0; A11(1,1) = -1; -// b(0) = 4 ; b(1) = -5; -// JacobianFactor::shared_ptr f2(new JacobianFactor(_x1_, A11, b*sigma2, sharedSigma(2,sigma2))); -// -// double sigma3 = 0.25; -// A11(0,0) = 1; A11(0,1) = 0; -// A11(1,0) = 0; A11(1,1) = -1; -// b(0) = 3 ; b(1) = -88; -// JacobianFactor::shared_ptr f3(new JacobianFactor(_x1_, A11, b*sigma3, sharedSigma(2,sigma3))); -// -// // TODO: find a real sigma value for this example -// double sigma4 = 0.1; -// A11(0,0) = 6; A11(0,1) = 0; -// A11(1,0) = 0; A11(1,1) = 7; -// b(0) = 5 ; b(1) = -6; -// JacobianFactor::shared_ptr f4(new JacobianFactor(_x1_, A11*sigma4, b*sigma4, sharedSigma(2,sigma4))); -// -// vector lfg; -// lfg.push_back(f1); -// lfg.push_back(f2); -// lfg.push_back(f3); -// lfg.push_back(f4); -// JacobianFactor combined(lfg); -// -// Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4); -// Matrix A22(8,2); -// A22(0,0) = 1; A22(0,1) = 0; -// A22(1,0) = 0; A22(1,1) = 1; -// A22(2,0) = 1; A22(2,1) = 0; -// A22(3,0) = 0; A22(3,1) = -1; -// A22(4,0) = 1; A22(4,1) = 0; -// A22(5,0) = 0; A22(5,1) = -1; -// A22(6,0) = 0.6; A22(6,1) = 0; -// A22(7,0) = 0; A22(7,1) = 0.7; -// Vector exb(8); -// exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2; -// exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4; -// -// vector > meas; -// meas.push_back(make_pair(_x1_, A22)); -// JacobianFactor expected(meas, exb, sigmas); -// EXPECT(assert_equal(expected,combined)); -//} -// -///* ************************************************************************* */ -//TEST(GaussianFactor, linearFactorN){ -// Matrix I = eye(2); -// vector f; -// SharedDiagonal model = sharedSigma(2,1.0); -// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, I, Vector_(2, -// 10.0, 5.0), model))); -// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, -10 * I, -// _x2_, 10 * I, Vector_(2, 1.0, -2.0), model))); -// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x2_, -10 * I, -// _x3_, 10 * I, Vector_(2, 1.5, -1.5), model))); -// f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x3_, -10 * I, -// _x4_, 10 * I, Vector_(2, 2.0, -1.0), model))); -// -// JacobianFactor combinedFactor(f); -// -// vector > combinedMeasurement; -// combinedMeasurement.push_back(make_pair(_x1_, Matrix_(8,2, -// 1.0, 0.0, -// 0.0, 1.0, -// -10.0, 0.0, -// 0.0,-10.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0))); -// combinedMeasurement.push_back(make_pair(_x2_, Matrix_(8,2, -// 0.0, 0.0, -// 0.0, 0.0, -// 10.0, 0.0, -// 0.0, 10.0, -// -10.0, 0.0, -// 0.0,-10.0, -// 0.0, 0.0, -// 0.0, 0.0))); -// combinedMeasurement.push_back(make_pair(_x3_, Matrix_(8,2, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 10.0, 0.0, -// 0.0, 10.0, -// -10.0, 0.0, -// 0.0,-10.0))); -// combinedMeasurement.push_back(make_pair(_x4_, Matrix_(8,2, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 0.0, 0.0, -// 10.0, 0.0, -// 0.0,10.0))); -// Vector b = Vector_(8, -// 10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); -// -// Vector sigmas = repeat(8,1.0); -// JacobianFactor expected(combinedMeasurement, b, sigmas); -// EXPECT(assert_equal(expected,combinedFactor)); -//} - +#endif /* ************************************************************************* */ TEST(GaussianFactor, eliminate2 ) { @@ -451,155 +168,6 @@ TEST(GaussianFactor, eliminate2 ) EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3)); } -/* ************************************************************************* */ -TEST(GaussianFactor, eliminateFrontals) -{ - // Augmented Ab test case for whole factor graph - Matrix Ab = Matrix_(14,11, - 4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1., - 9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4., - 5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0., - 5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5., - 0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4., - 0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6., - 0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6., - 0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6., - 0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0., - 0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0., - 0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3., - 0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5., - 0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6., - 0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.); - - // Create first factor (from pieces of Ab) - list > terms1; - terms1 += - make_pair(3, Matrix(ublas::project(Ab, ublas::range(0,4), ublas::range(0,2)))), - make_pair(5, ublas::project(Ab, ublas::range(0,4), ublas::range(2,4))), - make_pair(7, ublas::project(Ab, ublas::range(0,4), ublas::range(4,6))), - make_pair(9, ublas::project(Ab, ublas::range(0,4), ublas::range(6,8))), - make_pair(11, ublas::project(Ab, ublas::range(0,4), ublas::range(8,10))); - Vector b1 = ublas::project(ublas::column(Ab, 10), ublas::range(0,4)); - JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, sharedSigma(4, 0.5))); - - // Create second factor - list > terms2; - terms2 += - make_pair(5, ublas::project(Ab, ublas::range(4,8), ublas::range(2,4))), - make_pair(7, ublas::project(Ab, ublas::range(4,8), ublas::range(4,6))), - make_pair(9, ublas::project(Ab, ublas::range(4,8), ublas::range(6,8))), - make_pair(11, ublas::project(Ab, ublas::range(4,8), ublas::range(8,10))); - Vector b2 = ublas::project(ublas::column(Ab, 10), ublas::range(4,8)); - JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, sharedSigma(4, 0.5))); - - // Create third factor - list > terms3; - terms3 += - make_pair(7, ublas::project(Ab, ublas::range(8,12), ublas::range(4,6))), - make_pair(9, ublas::project(Ab, ublas::range(8,12), ublas::range(6,8))), - make_pair(11, ublas::project(Ab, ublas::range(8,12), ublas::range(8,10))); - Vector b3 = ublas::project(ublas::column(Ab, 10), ublas::range(8,12)); - JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, sharedSigma(4, 0.5))); - - // Create fourth factor - list > terms4; - terms4 += - make_pair(11, ublas::project(Ab, ublas::range(12,14), ublas::range(8,10))); - Vector b4 = ublas::project(ublas::column(Ab, 10), ublas::range(12,14)); - JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, sharedSigma(2, 0.5))); - - // Create factor graph - GaussianFactorGraph factors; - factors.push_back(factor1); - factors.push_back(factor2); - factors.push_back(factor3); - factors.push_back(factor4); - - // Create combined factor - JacobianFactor combined(*JacobianFactor::Combine(*factors.dynamicCastFactors >(), VariableSlots(factors))); - - // Copies factors as they will be eliminated in place - JacobianFactor actualFactor_QR = combined; - JacobianFactor actualFactor_Chol = combined; - - // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) - Matrix R = 2.0*Matrix_(11,11, - -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, - 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, - 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, - 0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676, - 0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277, - 0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769, - 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, - 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, - 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, - 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635); - - // Expected conditional on first variable from first 2 rows of R - Matrix R1 = ublas::project(R, ublas::range(0,2), ublas::range(0,2)); - list > cterms1; - cterms1 += - make_pair(5, ublas::project(R, ublas::range(0,2), ublas::range(2,4))), - make_pair(7, ublas::project(R, ublas::range(0,2), ublas::range(4,6))), - make_pair(9, ublas::project(R, ublas::range(0,2), ublas::range(6,8))), - make_pair(11, ublas::project(R, ublas::range(0,2), ublas::range(8,10))); - Vector d1 = ublas::project(ublas::column(R, 10), ublas::range(0,2)); - GaussianConditional::shared_ptr cond1(new GaussianConditional(3, d1, R1, cterms1, ones(2))); - - // Expected conditional on second variable from next 2 rows of R - Matrix R2 = ublas::project(R, ublas::range(2,4), ublas::range(2,4)); - list > cterms2; - cterms2 += - make_pair(7, ublas::project(R, ublas::range(2,4), ublas::range(4,6))), - make_pair(9, ublas::project(R, ublas::range(2,4), ublas::range(6,8))), - make_pair(11, ublas::project(R, ublas::range(2,4), ublas::range(8,10))); - Vector d2 = ublas::project(ublas::column(R, 10), ublas::range(2,4)); - GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2))); - - // Expected conditional on third variable from next 2 rows of R - Matrix R3 = ublas::project(R, ublas::range(4,6), ublas::range(4,6)); - list > cterms3; - cterms3 += - make_pair(9, ublas::project(R, ublas::range(4,6), ublas::range(6,8))), - make_pair(11, ublas::project(R, ublas::range(4,6), ublas::range(8,10))); - Vector d3 = ublas::project(ublas::column(R, 10), ublas::range(4,6)); - GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2))); - - // Create expected Bayes net fragment from three conditionals above - GaussianBayesNet expectedFragment; - expectedFragment.push_back(cond1); - expectedFragment.push_back(cond2); - expectedFragment.push_back(cond3); - - // Get expected matrices for remaining factor - Matrix Ae1 = ublas::project(R, ublas::range(6,10), ublas::range(6,8)); - Matrix Ae2 = ublas::project(R, ublas::range(6,10), ublas::range(8,10)); - Vector be = ublas::project(ublas::column(R, 10), ublas::range(6,10)); - - // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! - GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3); - EXPECT(assert_equal(expectedFragment, actualFragment_QR, 0.001)); - EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size())); - EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0])); - EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1])); - EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001)); - EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001)); - EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001)); - EXPECT(assert_equal(ones(4), actualFactor_QR.get_model()->sigmas(), 0.001)); - - // Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!! -// GaussianBayesNet actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactor::SOLVE_CHOLESKY); -// EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001)); -// EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size())); -// EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0])); -// EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1])); -// EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); //// -// EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001)); -// EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); //// -// EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001)); -} - /* ************************************************************************* */ TEST(GaussianFactor, default_error ) { @@ -610,28 +178,29 @@ TEST(GaussianFactor, default_error ) EXPECT(actual==0.0); } -////* ************************************************************************* */ -//TEST(GaussianFactor, eliminate_empty ) -//{ -// // create an empty factor -// JacobianFactor f; -// -// // eliminate the empty factor -// GaussianConditional::shared_ptr actualCG; -// JacobianFactor::shared_ptr actualLF(new JacobianFactor(f)); -// actualCG = actualLF->eliminateFirst(); -// -// // expected Conditional Gaussian is just a parent-less node with P(x)=1 -// GaussianConditional expectedCG(_x2_); -// -// // expected remaining factor is still empty :-) -// JacobianFactor expectedLF; -// -// // check if the result matches -// EXPECT(actualCG->equals(expectedCG)); -// EXPECT(actualLF->equals(expectedLF)); -//} +//* ************************************************************************* */ +#ifdef BROKEN +TEST(GaussianFactor, eliminate_empty ) +{ + // create an empty factor + JacobianFactor f; + // eliminate the empty factor + GaussianConditional::shared_ptr actualCG; + JacobianFactor::shared_ptr actualLF(new JacobianFactor(f)); + actualCG = actualLF->eliminateFirst(); + + // expected Conditional Gaussian is just a parent-less node with P(x)=1 + GaussianConditional expectedCG(_x2_); + + // expected remaining factor is still empty :-) + JacobianFactor expectedLF; + + // check if the result matches + EXPECT(actualCG->equals(expectedCG)); + EXPECT(actualLF->equals(expectedLF)); +} +#endif //* ************************************************************************* */ TEST(GaussianFactor, empty ) { @@ -648,29 +217,6 @@ void print(const list& i) { cout << endl; } -///* ************************************************************************* */ -//TEST(GaussianFactor, tally_separator ) -//{ -// JacobianFactor f(_x1_, eye(2), _x2_, eye(2), _l1_, eye(2), ones(2), sigma0_1); -// -// std::set act1, act2, act3; -// f.tally_separator(_x1_, act1); -// f.tally_separator(_x2_, act2); -// f.tally_separator(_l1_, act3); -// -// EXPECT(act1.size() == 2); -// EXPECT(act1.count(_x2_) == 1); -// EXPECT(act1.count(_l1_) == 1); -// -// EXPECT(act2.size() == 2); -// EXPECT(act2.count(_x1_) == 1); -// EXPECT(act2.count(_l1_) == 1); -// -// EXPECT(act3.size() == 2); -// EXPECT(act3.count(_x1_) == 1); -// EXPECT(act3.count(_x2_) == 1); -//} - /* ************************************************************************* */ TEST(GaussianFactor, CONSTRUCTOR_GaussianConditional ) { @@ -690,22 +236,6 @@ TEST(GaussianFactor, CONSTRUCTOR_GaussianConditional ) EXPECT(assert_equal(expectedLF,actualLF,1e-5)); } -///* ************************************************************************* */ -//TEST(GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained ) -//{ -// Matrix Ax = eye(2); -// Vector b = Vector_(2, 3.0, 5.0); -// SharedDiagonal noisemodel = noiseModel::Constrained::All(2); -// JacobianFactor::shared_ptr expected(new JacobianFactor(_x0_, Ax, b, noisemodel)); -// GaussianFactorGraph graph; -// graph.push_back(expected); -// -// GaussianConditional::shared_ptr conditional = GaussianSequentialSolver::EliminateUntil(graph,_x0_+1); -// JacobianFactor actual(*conditional); -// -// EXPECT(assert_equal(*expected, actual)); -//} - /* ************************************************************************* */ TEST ( JacobianFactor, constraint_eliminate1 ) { @@ -768,103 +298,6 @@ TEST ( JacobianFactor, constraint_eliminate2 ) EXPECT(assert_equal(expectedCG, *actualCG, 1e-4)); } -/* ************************************************************************* */ -TEST(GaussianFactor, permuteWithInverse) -{ - Matrix A1 = Matrix_(2,2, - 1.0, 2.0, - 3.0, 4.0); - Matrix A2 = Matrix_(2,1, - 5.0, - 6.0); - Matrix A3 = Matrix_(2,3, - 7.0, 8.0, 9.0, - 10.0, 11.0, 12.0); - Vector b = Vector_(2, 13.0, 14.0); - - Permutation inversePermutation(6); - inversePermutation[0] = 5; - inversePermutation[1] = 4; - inversePermutation[2] = 3; - inversePermutation[3] = 2; - inversePermutation[4] = 1; - inversePermutation[5] = 0; - - JacobianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0)); - GaussianFactorGraph actualFG; actualFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(actual))); - VariableIndex actualIndex(actualFG); - actual.permuteWithInverse(inversePermutation); -// actualIndex.permute(*inversePermutation.inverse()); - - JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0)); - GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected))); -// GaussianVariableIndex expectedIndex(expectedFG); - - EXPECT(assert_equal(expected, actual)); - -// // todo: fix this!!! VariableIndex should not hold slots -// for(Index j=0; j::max(); } -// } -// for(Index j=0; j::max(); } -// } -// EXPECT(assert_equal(expectedIndex, actualIndex)); -} - -///* ************************************************************************* */ -//TEST(GaussianFactor, erase) -//{ -// Vector b = Vector_(3, 1., 2., 3.); -// SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.)); -// std::list > terms; -// terms.push_back(make_pair(_x0_, eye(2))); -// terms.push_back(make_pair(_x1_, 2.*eye(2))); -// -// JacobianFactor actual(terms, b, noise); -// int erased = actual.erase_A(_x0_); -// -// LONGS_EQUAL(1, erased); -// JacobianFactor expected(_x1_, 2.*eye(2), b, noise); -// EXPECT(assert_equal(expected, actual)); -//} - -///* ************************************************************************* */ -//TEST(GaussianFactor, eliminateMatrix) -//{ -// Matrix Ab = Matrix_(3, 4, -// 1., 2., 0., 3., -// 0., 4., 5., 6., -// 0., 0., 7., 8.); -// SharedDiagonal model(Vector_(3, 0.5, 0.5, 0.5)); -// Ordering frontals; frontals += _x1_, _x2_; -// Ordering separator; separator += _x3_; -// Dimensions dimensions; -// dimensions.insert(make_pair(_x1_, 1)); -// dimensions.insert(make_pair(_x2_, 1)); -// dimensions.insert(make_pair(_x3_, 1)); -// -// JacobianFactor::shared_ptr factor; -// GaussianBayesNet bn; -// boost::tie(bn, factor) = -// JacobianFactor::eliminateMatrix(Ab, NULL, model, frontals, separator, dimensions); -// -// GaussianBayesNet bn_expected; -// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(_x1_, Vector_(1, 6.), Matrix_(1, 1, 2.), -// _x2_, Matrix_(1, 1, 4.), _x3_, Matrix_(1, 1, 0.), Vector_(1, 1.))); -// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(_x2_, Vector_(1, 12.), Matrix_(1, 1, 8.), -// _x3_, Matrix_(1, 1, 10.), Vector_(1, 1.))); -// bn_expected.push_back(conditional1); -// bn_expected.push_back(conditional2); -// EXPECT(assert_equal(bn_expected, bn)); -// -// JacobianFactor factor_expected(_x3_, Matrix_(1, 1, 14.), Vector_(1, 16.), SharedDiagonal(Vector_(1, 1.))); -// EXPECT(assert_equal(factor_expected, *factor)); -//} - - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp new file mode 100644 index 000000000..63001ea7a --- /dev/null +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -0,0 +1,536 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGaussianFactor.cpp + * @brief Unit tests for Linear Factor + * @author Christian Potthast + * @author Frank Dellaert + **/ + +#include // for operator += +using namespace boost::assign; + +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost; +namespace ublas = boost::numeric::ublas; + +static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8; + +static SharedDiagonal + sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), + constraintModel = noiseModel::Constrained::All(2); + +/* ************************************************************************* */ +#ifdef BROKEN +TEST(GaussianFactor, Combine) +{ + Matrix A00 = Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + Vector b0 = Vector_(3, 0.0, 0.0, 0.0); + Vector s0 = Vector_(3, 0.0, 0.0, 0.0); + + Matrix A10 = Matrix_(3,3, + 0.0, -2.0, -4.0, + 2.0, 0.0, 2.0, + 0.0, 0.0, -10.0); + Matrix A11 = Matrix_(3,3, + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 10.0); + Vector b1 = Vector_(3, 6.0, 2.0, 0.0); + Vector s1 = Vector_(3, 1.0, 1.0, 1.0); + + Matrix A20 = Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + Vector b2 = Vector_(3, 0.0, 0.0, 0.0); + Vector s2 = Vector_(3, 100.0, 100.0, 100.0); + + GaussianFactorGraph gfg; + gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + GaussianVariableIndex varindex(gfg); + vector factors(3); factors[0]=0; factors[1]=1; factors[2]=2; + vector variables(2); variables[0]=0; variables[1]=1; + vector > variablePositions(3); + variablePositions[0].resize(1); variablePositions[0][0]=0; + variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1; + variablePositions[2].resize(1); variablePositions[2][0]=0; + + JacobianFactor actual = *JacobianFactor::Combine(gfg, varindex, factors, variables, variablePositions); + + Matrix zero3x3 = zeros(3,3); + Matrix A0 = gtsam::stack(3, &A00, &A10, &A20); + Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3); + Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); + Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); + + JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + + EXPECT(assert_equal(expected, actual)); +} +#endif + +/* ************************************************************************* */ +TEST(GaussianFactorGraph, Combine2) +{ + Matrix A01 = Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + Vector b0 = Vector_(3, 1.5, 1.5, 1.5); + Vector s0 = Vector_(3, 1.6, 1.6, 1.6); + + Matrix A10 = Matrix_(3,3, + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 2.0); + Matrix A11 = Matrix_(3,3, + -2.0, 0.0, 0.0, + 0.0, -2.0, 0.0, + 0.0, 0.0, -2.0); + Vector b1 = Vector_(3, 2.5, 2.5, 2.5); + Vector s1 = Vector_(3, 2.6, 2.6, 2.6); + + Matrix A21 = Matrix_(3,3, + 3.0, 0.0, 0.0, + 0.0, 3.0, 0.0, + 0.0, 0.0, 3.0); + Vector b2 = Vector_(3, 3.5, 3.5, 3.5); + Vector s2 = Vector_(3, 3.6, 3.6, 3.6); + + GaussianFactorGraph gfg; + gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + JacobianFactor actual = *CombineJacobians(*gfg.dynamicCastFactors > (), VariableSlots(gfg)); + + Matrix zero3x3 = zeros(3,3); + Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); + Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); + Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); + Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); + + JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST_UNSAFE(GaussianFactor, CombineAndEliminate) +{ + Matrix A01 = Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + Vector b0 = Vector_(3, 1.5, 1.5, 1.5); + Vector s0 = Vector_(3, 1.6, 1.6, 1.6); + + Matrix A10 = Matrix_(3,3, + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 2.0); + Matrix A11 = Matrix_(3,3, + -2.0, 0.0, 0.0, + 0.0, -2.0, 0.0, + 0.0, 0.0, -2.0); + Vector b1 = Vector_(3, 2.5, 2.5, 2.5); + Vector s1 = Vector_(3, 2.6, 2.6, 2.6); + + Matrix A21 = Matrix_(3,3, + 3.0, 0.0, 0.0, + 0.0, 3.0, 0.0, + 0.0, 0.0, 3.0); + Vector b2 = Vector_(3, 3.5, 3.5, 3.5); + Vector s2 = Vector_(3, 3.6, 3.6, 3.6); + + GaussianFactorGraph gfg; + gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Matrix zero3x3 = zeros(3,3); + Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); + Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); + Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); + Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); + + JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + GaussianBayesNet expectedBN(*expectedFactor.eliminate(1)); + + GaussianBayesNet::shared_ptr actualBN; + GaussianFactor::shared_ptr actualFactor; + boost::tie(actualBN, actualFactor) = // + EliminateQR(*gfg.dynamicCastFactors > (), 1); + JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast< + JacobianFactor>(actualFactor); + + EXPECT(assert_equal(expectedBN, *actualBN)); + EXPECT(assert_equal(expectedFactor, *actualJacobian)); +} + +/* ************************************************************************* */ +#ifdef BROKEN +TEST( NonlinearFactorGraph, combine2){ + double sigma1 = 0.0957; + Matrix A11(2,2); + A11(0,0) = 1; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = 1; + Vector b(2); + b(0) = 2; b(1) = -1; + JacobianFactor::shared_ptr f1(new JacobianFactor(_x1_, A11, b*sigma1, sharedSigma(2,sigma1))); + + double sigma2 = 0.5; + A11(0,0) = 1; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = -1; + b(0) = 4 ; b(1) = -5; + JacobianFactor::shared_ptr f2(new JacobianFactor(_x1_, A11, b*sigma2, sharedSigma(2,sigma2))); + + double sigma3 = 0.25; + A11(0,0) = 1; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = -1; + b(0) = 3 ; b(1) = -88; + JacobianFactor::shared_ptr f3(new JacobianFactor(_x1_, A11, b*sigma3, sharedSigma(2,sigma3))); + + // TODO: find a real sigma value for this example + double sigma4 = 0.1; + A11(0,0) = 6; A11(0,1) = 0; + A11(1,0) = 0; A11(1,1) = 7; + b(0) = 5 ; b(1) = -6; + JacobianFactor::shared_ptr f4(new JacobianFactor(_x1_, A11*sigma4, b*sigma4, sharedSigma(2,sigma4))); + + vector lfg; + lfg.push_back(f1); + lfg.push_back(f2); + lfg.push_back(f3); + lfg.push_back(f4); + JacobianFactor combined(lfg); + + Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4); + Matrix A22(8,2); + A22(0,0) = 1; A22(0,1) = 0; + A22(1,0) = 0; A22(1,1) = 1; + A22(2,0) = 1; A22(2,1) = 0; + A22(3,0) = 0; A22(3,1) = -1; + A22(4,0) = 1; A22(4,1) = 0; + A22(5,0) = 0; A22(5,1) = -1; + A22(6,0) = 0.6; A22(6,1) = 0; + A22(7,0) = 0; A22(7,1) = 0.7; + Vector exb(8); + exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2; + exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4; + + vector > meas; + meas.push_back(make_pair(_x1_, A22)); + JacobianFactor expected(meas, exb, sigmas); + EXPECT(assert_equal(expected,combined)); +} + +/* ************************************************************************* */ +TEST(GaussianFactor, linearFactorN){ + Matrix I = eye(2); + vector f; + SharedDiagonal model = sharedSigma(2,1.0); + f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, I, Vector_(2, + 10.0, 5.0), model))); + f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, -10 * I, + _x2_, 10 * I, Vector_(2, 1.0, -2.0), model))); + f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x2_, -10 * I, + _x3_, 10 * I, Vector_(2, 1.5, -1.5), model))); + f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x3_, -10 * I, + _x4_, 10 * I, Vector_(2, 2.0, -1.0), model))); + + JacobianFactor combinedFactor(f); + + vector > combinedMeasurement; + combinedMeasurement.push_back(make_pair(_x1_, Matrix_(8,2, + 1.0, 0.0, + 0.0, 1.0, + -10.0, 0.0, + 0.0,-10.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0))); + combinedMeasurement.push_back(make_pair(_x2_, Matrix_(8,2, + 0.0, 0.0, + 0.0, 0.0, + 10.0, 0.0, + 0.0, 10.0, + -10.0, 0.0, + 0.0,-10.0, + 0.0, 0.0, + 0.0, 0.0))); + combinedMeasurement.push_back(make_pair(_x3_, Matrix_(8,2, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 10.0, 0.0, + 0.0, 10.0, + -10.0, 0.0, + 0.0,-10.0))); + combinedMeasurement.push_back(make_pair(_x4_, Matrix_(8,2, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 10.0, 0.0, + 0.0,10.0))); + Vector b = Vector_(8, + 10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); + + Vector sigmas = repeat(8,1.0); + JacobianFactor expected(combinedMeasurement, b, sigmas); + EXPECT(assert_equal(expected,combinedFactor)); +} +#endif + +/* ************************************************************************* */ +TEST(GaussianFactor, eliminateFrontals) +{ + // Augmented Ab test case for whole factor graph + Matrix Ab = Matrix_(14,11, + 4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1., + 9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4., + 5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0., + 5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5., + 0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4., + 0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6., + 0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6., + 0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6., + 0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0., + 0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0., + 0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3., + 0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5., + 0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6., + 0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.); + + // Create first factor (from pieces of Ab) + list > terms1; + terms1 += + make_pair(3, Matrix(ublas::project(Ab, ublas::range(0,4), ublas::range(0,2)))), + make_pair(5, ublas::project(Ab, ublas::range(0,4), ublas::range(2,4))), + make_pair(7, ublas::project(Ab, ublas::range(0,4), ublas::range(4,6))), + make_pair(9, ublas::project(Ab, ublas::range(0,4), ublas::range(6,8))), + make_pair(11, ublas::project(Ab, ublas::range(0,4), ublas::range(8,10))); + Vector b1 = ublas::project(ublas::column(Ab, 10), ublas::range(0,4)); + JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, sharedSigma(4, 0.5))); + + // Create second factor + list > terms2; + terms2 += + make_pair(5, ublas::project(Ab, ublas::range(4,8), ublas::range(2,4))), + make_pair(7, ublas::project(Ab, ublas::range(4,8), ublas::range(4,6))), + make_pair(9, ublas::project(Ab, ublas::range(4,8), ublas::range(6,8))), + make_pair(11, ublas::project(Ab, ublas::range(4,8), ublas::range(8,10))); + Vector b2 = ublas::project(ublas::column(Ab, 10), ublas::range(4,8)); + JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, sharedSigma(4, 0.5))); + + // Create third factor + list > terms3; + terms3 += + make_pair(7, ublas::project(Ab, ublas::range(8,12), ublas::range(4,6))), + make_pair(9, ublas::project(Ab, ublas::range(8,12), ublas::range(6,8))), + make_pair(11, ublas::project(Ab, ublas::range(8,12), ublas::range(8,10))); + Vector b3 = ublas::project(ublas::column(Ab, 10), ublas::range(8,12)); + JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, sharedSigma(4, 0.5))); + + // Create fourth factor + list > terms4; + terms4 += + make_pair(11, ublas::project(Ab, ublas::range(12,14), ublas::range(8,10))); + Vector b4 = ublas::project(ublas::column(Ab, 10), ublas::range(12,14)); + JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, sharedSigma(2, 0.5))); + + // Create factor graph + GaussianFactorGraph factors; + factors.push_back(factor1); + factors.push_back(factor2); + factors.push_back(factor3); + factors.push_back(factor4); + + // Create combined factor + JacobianFactor combined(*CombineJacobians(*factors.dynamicCastFactors > (), VariableSlots(factors))); + + // Copies factors as they will be eliminated in place + JacobianFactor actualFactor_QR = combined; + JacobianFactor actualFactor_Chol = combined; + + // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) + Matrix R = 2.0*Matrix_(11,11, + -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, + 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, + 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, + 0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676, + 0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277, + 0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769, + 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, + 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, + 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, + 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, + 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635); + + // Expected conditional on first variable from first 2 rows of R + Matrix R1 = ublas::project(R, ublas::range(0,2), ublas::range(0,2)); + list > cterms1; + cterms1 += + make_pair(5, ublas::project(R, ublas::range(0,2), ublas::range(2,4))), + make_pair(7, ublas::project(R, ublas::range(0,2), ublas::range(4,6))), + make_pair(9, ublas::project(R, ublas::range(0,2), ublas::range(6,8))), + make_pair(11, ublas::project(R, ublas::range(0,2), ublas::range(8,10))); + Vector d1 = ublas::project(ublas::column(R, 10), ublas::range(0,2)); + GaussianConditional::shared_ptr cond1(new GaussianConditional(3, d1, R1, cterms1, ones(2))); + + // Expected conditional on second variable from next 2 rows of R + Matrix R2 = ublas::project(R, ublas::range(2,4), ublas::range(2,4)); + list > cterms2; + cterms2 += + make_pair(7, ublas::project(R, ublas::range(2,4), ublas::range(4,6))), + make_pair(9, ublas::project(R, ublas::range(2,4), ublas::range(6,8))), + make_pair(11, ublas::project(R, ublas::range(2,4), ublas::range(8,10))); + Vector d2 = ublas::project(ublas::column(R, 10), ublas::range(2,4)); + GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2))); + + // Expected conditional on third variable from next 2 rows of R + Matrix R3 = ublas::project(R, ublas::range(4,6), ublas::range(4,6)); + list > cterms3; + cterms3 += + make_pair(9, ublas::project(R, ublas::range(4,6), ublas::range(6,8))), + make_pair(11, ublas::project(R, ublas::range(4,6), ublas::range(8,10))); + Vector d3 = ublas::project(ublas::column(R, 10), ublas::range(4,6)); + GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2))); + + // Create expected Bayes net fragment from three conditionals above + GaussianBayesNet expectedFragment; + expectedFragment.push_back(cond1); + expectedFragment.push_back(cond2); + expectedFragment.push_back(cond3); + + // Get expected matrices for remaining factor + Matrix Ae1 = ublas::project(R, ublas::range(6,10), ublas::range(6,8)); + Matrix Ae2 = ublas::project(R, ublas::range(6,10), ublas::range(8,10)); + Vector be = ublas::project(ublas::column(R, 10), ublas::range(6,10)); + + // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! + GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3); + EXPECT(assert_equal(expectedFragment, actualFragment_QR, 0.001)); + EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size())); + EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0])); + EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1])); + EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001)); + EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001)); + EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001)); + EXPECT(assert_equal(ones(4), actualFactor_QR.get_model()->sigmas(), 0.001)); + + // Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!! +#ifdef BROKEN + GaussianBayesNet actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactor::SOLVE_CHOLESKY); + EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001)); + EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size())); + EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0])); + EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1])); + EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); //// + EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001)); + EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); //// + EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001)); +#endif +} + +/* ************************************************************************* */ +#ifdef BROKEN +TEST(GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained ) +{ + Matrix Ax = eye(2); + Vector b = Vector_(2, 3.0, 5.0); + SharedDiagonal noisemodel = noiseModel::Constrained::All(2); + JacobianFactor::shared_ptr expected(new JacobianFactor(_x0_, Ax, b, noisemodel)); + GaussianFactorGraph graph; + graph.push_back(expected); + + GaussianConditional::shared_ptr conditional = GaussianSequentialSolver::EliminateUntil(graph,_x0_+1); + JacobianFactor actual(*conditional); + + EXPECT(assert_equal(*expected, actual)); +} +#endif + +/* ************************************************************************* */ +TEST(GaussianFactor, permuteWithInverse) +{ + Matrix A1 = Matrix_(2,2, + 1.0, 2.0, + 3.0, 4.0); + Matrix A2 = Matrix_(2,1, + 5.0, + 6.0); + Matrix A3 = Matrix_(2,3, + 7.0, 8.0, 9.0, + 10.0, 11.0, 12.0); + Vector b = Vector_(2, 13.0, 14.0); + + Permutation inversePermutation(6); + inversePermutation[0] = 5; + inversePermutation[1] = 4; + inversePermutation[2] = 3; + inversePermutation[3] = 2; + inversePermutation[4] = 1; + inversePermutation[5] = 0; + + JacobianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0)); + GaussianFactorGraph actualFG; actualFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(actual))); + VariableIndex actualIndex(actualFG); + actual.permuteWithInverse(inversePermutation); +// actualIndex.permute(*inversePermutation.inverse()); + + JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0)); + GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected))); +// GaussianVariableIndex expectedIndex(expectedFG); + + EXPECT(assert_equal(expected, actual)); + +#ifdef BROKEN + // todo: fix this!!! VariableIndex should not hold slots + for(Index j=0; j::max(); } + } + for(Index j=0; j::max(); } + } + EXPECT(assert_equal(expectedIndex, actualIndex)); +#endif +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp index ae42c472c..b92595a03 100644 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp @@ -69,7 +69,7 @@ TEST( GaussianJunctionTree, eliminate ) { GaussianFactorGraph fg = createChain(); GaussianJunctionTree junctionTree(fg); - BayesTree::sharedClique rootClique = junctionTree.eliminate(); + BayesTree::sharedClique rootClique = junctionTree.eliminate(&EliminateQR); typedef BayesTree::sharedConditional sharedConditional; Matrix two = Matrix_(1,1,2.); @@ -90,7 +90,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) GaussianFactorGraph fg = createChain(); GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(); + VectorValues actual = tree.optimize(&EliminateQR); VectorValues expected(vector(4,1)); expected[x1] = Vector_(1, 0.); expected[x2] = Vector_(1, 1.); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3a330d82a..91f39880d 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -129,11 +129,13 @@ TEST(GaussianFactor, CombineAndEliminate) JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); GaussianBayesNet expectedBN(*expectedFactor.eliminate(1)); - pair actualCholesky(HessianFactor::CombineAndEliminate( - *gfg.convertCastFactors >(), 1)); + GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky( + *gfg.convertCastFactors > (), 1); + HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< + HessianFactor>(actualCholesky.second); EXPECT(assert_equal(expectedBN, *actualCholesky.first, 1e-6)); - EXPECT(assert_equal(HessianFactor(expectedFactor), *actualCholesky.second, 1e-6)); + EXPECT(assert_equal(HessianFactor(expectedFactor), *actualFactor, 1e-6)); } /* ************************************************************************* */ @@ -177,8 +179,11 @@ TEST(GaussianFactor, eliminate2 ) HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined)); FactorGraph combinedLFG_Chol; combinedLFG_Chol.push_back(combinedLF_Chol); - pair actual_Chol = - HessianFactor::CombineAndEliminate(combinedLFG_Chol, 1); + + GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky( + combinedLFG_Chol, 1); + HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< + HessianFactor>(actual_Chol.second); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit @@ -203,7 +208,7 @@ TEST(GaussianFactor, eliminate2 ) )/sigma; Vector b1 = Vector_(2,0.0,0.894427); JacobianFactor expectedLF(1, Bl1x1, b1, repeat(2,1.0)); - EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); + EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3)); } /* ************************************************************************* */ @@ -276,11 +281,13 @@ TEST(GaussianFactor, eliminateUnsorted) { GaussianBayesNet::shared_ptr expected_bn; GaussianFactor::shared_ptr expected_factor; - boost::tie(expected_bn, expected_factor) = GaussianFactor::CombineAndEliminate(sortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY); + boost::tie(expected_bn, expected_factor) = + EliminatePreferCholesky(sortedGraph, 1); GaussianBayesNet::shared_ptr actual_bn; GaussianFactor::shared_ptr actual_factor; - boost::tie(actual_bn, actual_factor) = GaussianFactor::CombineAndEliminate(unsortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY); + boost::tie(actual_bn, actual_factor) = + EliminatePreferCholesky(unsortedGraph, 1); CHECK(assert_equal(*expected_bn, *actual_bn, 1e-10)); CHECK(assert_equal(*expected_factor, *actual_factor, 1e-10)); diff --git a/gtsam/linear/tests/timeFactorOverhead.cpp b/gtsam/linear/tests/timeFactorOverhead.cpp index 11c2c52d0..2607cab25 100644 --- a/gtsam/linear/tests/timeFactorOverhead.cpp +++ b/gtsam/linear/tests/timeFactorOverhead.cpp @@ -82,7 +82,7 @@ int main(int argc, char *argv[]) { timer.restart(); for(size_t trial=0; trialeliminate()); + GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate(&EliminateQR)); VectorValues soln(optimize(*gbn)); } blocksolve = timer.elapsed(); @@ -126,7 +126,7 @@ int main(int argc, char *argv[]) { cout.flush(); timer.restart(); for(size_t trial=0; trialeliminate()); + GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate(&EliminateQR)); VectorValues soln(optimize(*gbn)); } combsolve = timer.elapsed(); diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index ea507e3d2..9cdff6fb8 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -28,7 +28,8 @@ using namespace gtsam; /* ************************************************************************* */ template -void NonlinearISAM::update(const Factors& newFactors, const Values& initialValues) { +void NonlinearISAM::update(const Factors& newFactors, + const Values& initialValues) { if(newFactors.size() > 0) { diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 8c98ecadf..9582742fb 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -125,12 +125,12 @@ TEST( BayesTree, linear_smoother_shortcuts ) // Check the conditional P(Root|Root) GaussianBayesNet empty; GaussianISAM::sharedClique R = bayesTree.root(); - GaussianBayesNet actual1 = R->shortcut(R); + GaussianBayesNet actual1 = GaussianISAM::shortcut(R,R); CHECK(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) GaussianISAM::sharedClique C2 = bayesTree[ordering["x5"]]; - GaussianBayesNet actual2 = C2->shortcut(R); + GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R); CHECK(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root) @@ -139,7 +139,7 @@ TEST( BayesTree, linear_smoother_shortcuts ) GaussianBayesNet expected3; push_front(expected3,ordering["x5"], zero(2), eye(2)/sigma3, ordering["x6"], A56/sigma3, ones(2)); GaussianISAM::sharedClique C3 = bayesTree[ordering["x4"]]; - GaussianBayesNet actual3 = C3->shortcut(R); + GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); CHECK(assert_equal(expected3,actual3,tol)); // Check the conditional P(C4|Root) @@ -148,7 +148,7 @@ TEST( BayesTree, linear_smoother_shortcuts ) GaussianBayesNet expected4; push_front(expected4, ordering["x4"], zero(2), eye(2)/sigma4, ordering["x6"], A46/sigma4, ones(2)); GaussianISAM::sharedClique C4 = bayesTree[ordering["x3"]]; - GaussianBayesNet actual4 = C4->shortcut(R); + GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R); CHECK(assert_equal(expected4,actual4,tol)); } @@ -264,19 +264,19 @@ TEST( BayesTree, balanced_smoother_shortcuts ) // Check the conditional P(Root|Root) GaussianBayesNet empty; GaussianISAM::sharedClique R = bayesTree.root(); - GaussianBayesNet actual1 = R->shortcut(R); + GaussianBayesNet actual1 = GaussianISAM::shortcut(R,R); CHECK(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) GaussianISAM::sharedClique C2 = bayesTree[ordering["x3"]]; - GaussianBayesNet actual2 = C2->shortcut(R); + GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R); CHECK(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet[ordering["x2"]]; GaussianBayesNet expected3; expected3.push_back(p_x2_x4); GaussianISAM::sharedClique C3 = bayesTree[ordering["x1"]]; - GaussianBayesNet actual3 = C3->shortcut(R); + GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); CHECK(assert_equal(expected3,actual3,tol)); } diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 11f704779..460d888be 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -91,7 +91,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) // optimize the graph GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(); + VectorValues actual = tree.optimize(&EliminateQR); // verify VectorValues expected(vector(7,2)); // expected solution @@ -112,7 +112,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) // optimize the graph GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(); + VectorValues actual = tree.optimize(&EliminateQR); // verify VectorValues expected = createCorrectDelta(ordering); // expected solution @@ -174,7 +174,7 @@ TEST(GaussianJunctionTree, slamlike) { GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); GaussianJunctionTree gjt(linearized); - VectorValues deltaactual = gjt.optimize(); + VectorValues deltaactual = gjt.optimize(&EliminateQR); planarSLAM::Values actual = init.expmap(deltaactual, ordering); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp index 123a0de5c..a59a69dab 100644 --- a/tests/testSymbolicBayesNet.cpp +++ b/tests/testSymbolicBayesNet.cpp @@ -56,7 +56,8 @@ TEST( SymbolicBayesNet, constructor ) SymbolicFactorGraph fg(factorGraph); // eliminate it - SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); + SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate( + &EliminateSymbolic); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index 153224dc0..eb951fc16 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -143,7 +143,7 @@ TEST( SymbolicFactorGraph, eliminate ) SymbolicFactorGraph fg(factorGraph); // eliminate it - SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); + SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(&EliminateSymbolic); CHECK(assert_equal(expected,actual)); }