From 812e3277ee6de6e5314523046228c185e4582aed Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 21 Oct 2010 22:59:54 +0000 Subject: [PATCH] New linear solver interface, global inference functions removed. --- .cproject | 2 +- .project | 2 +- base/Makefile.am | 2 +- inference/BayesNet.h | 3 +- inference/BayesTree-inl.h | 144 +++---- inference/BayesTree.h | 10 +- inference/EliminationTree-inl.h | 53 +-- inference/EliminationTree.h | 17 +- inference/FactorGraph.h | 2 +- inference/ISAM-inl.h | 3 +- inference/JunctionTree-inl.h | 4 +- inference/Makefile.am | 2 +- inference/SymbolicFactorGraph.h | 2 + inference/VariableIndex.h | 48 ++- inference/inference-inl.h | 395 -------------------- inference/inference.cpp | 79 ---- inference/inference.h | 78 ---- inference/tests/testBayesTree.cpp | 113 +++--- inference/tests/testEliminationTree.cpp | 8 +- inference/tests/testJunctionTree.cpp | 2 +- inference/tests/testSymbolicFactorGraph.cpp | 31 +- linear/Factorization.h | 3 +- linear/GaussianFactorGraph.h | 6 + linear/Makefile.am | 3 + linear/SubgraphSolver-inl.h | 3 +- linear/tests/testGaussianFactor.cpp | 33 +- linear/tests/timeFactorOverhead.cpp | 7 +- linear/tests/timeSLAMlike.cpp | 5 +- myconfigure | 2 +- myconfigure.timing | 2 +- tests/testGaussianBayesNet.cpp | 5 +- tests/testGaussianFactorGraph.cpp | 122 +++--- tests/testGaussianISAM.cpp | 89 ++--- tests/testGaussianJunctionTree.cpp | 21 +- tests/testInference.cpp | 14 +- tests/testSymbolicBayesNet.cpp | 4 +- tests/testSymbolicFactorGraph.cpp | 37 +- tests/timeGaussianFactorGraph.cpp | 8 +- tests/timeLinearOnDataset.cpp | 14 +- 39 files changed, 442 insertions(+), 936 deletions(-) diff --git a/.cproject b/.cproject index c2701a85b..4407cf507 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + diff --git a/.project b/.project index 475695351..d20fd358e 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - + -j2 org.eclipse.cdt.make.core.buildCommand diff --git a/base/Makefile.am b/base/Makefile.am index 02dbf489d..1ece7c6ab 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -67,7 +67,7 @@ endif #---------------------------------------------------------------------------------------------------- TESTS = $(check_PROGRAMS) AM_DEFAULT_SOURCE_EXT = .cpp -AM_LDFLAGS = $(boost_serialization) -L$(CCOLAMDLib) -lccolamd +AM_LDFLAGS += $(boost_serialization) -L$(CCOLAMDLib) -lccolamd LDADD = libbase.la ../CppUnitLite/libCppUnitLite.a if USE_BLAS_LINUX diff --git a/inference/BayesNet.h b/inference/BayesNet.h index 103a727f3..f5b6ddc8a 100644 --- a/inference/BayesNet.h +++ b/inference/BayesNet.h @@ -94,7 +94,8 @@ namespace gtsam { /** Permute the variables in the BayesNet */ void permuteWithInverse(const Permutation& inversePermutation); - /** Permute the variables when only separator variables need to be permuted. + /** + * 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. */ diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index 31c41daaf..d8a4fea86 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -32,8 +32,10 @@ using namespace boost::assign; namespace lam = boost::lambda; +#include #include #include +#include namespace gtsam { @@ -266,9 +268,11 @@ namespace gtsam { // TODO, why do we actually return a shared pointer, why does eliminate? /* ************************************************************************* */ template - template BayesNet BayesTree::Clique::shortcut(shared_ptr R) { + + static const bool debug = false; + // A first base case is when this clique or its parent is the root, // in which case we return an empty Bayes net. @@ -279,15 +283,21 @@ namespace gtsam { return empty; } + // The root conditional + 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)); // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) - FACTORGRAPH p_Cp_R = combine(p_Fp_Sp, p_Sp_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); // Eliminate into a Bayes net with ordering designed to integrate out // any variables not in *our* separator. Variables to integrate out must be @@ -295,45 +305,48 @@ namespace gtsam { // However, an added wrinkle is that Cp might overlap with the root. // Keys corresponding to the root should not be added to the ordering at all. - typedef set, boost::fast_pool_allocator > FastJSet; - - // Get the key list Cp=Fp+Sp, which will form the basis for the integrands - vector parentKeys(parent->keys()); - FastJSet integrands(parentKeys.begin(), parentKeys.end()); - - // Start ordering with the separator - FastJSet separator(separator_.begin(), separator_.end()); - - // remove any variables in the root, after this integrands = Cp\R, ordering = S\R - BOOST_FOREACH(Index key, R->ordering()) { - integrands.erase(key); - separator.erase(key); + if(debug) { + p_R.print("p_R: "); + p_Fp_Sp.print("p_Fp_Sp: "); + p_Sp_R.print("p_Sp_R: "); } - // remove any variables in the separator, after this integrands = Cp\R\S - BOOST_FOREACH(Index key, separator_) integrands.erase(key); - - // form the ordering as [Cp\R\S S\R] - vector ordering; ordering.reserve(integrands.size() + separator.size()); - BOOST_FOREACH(Index key, integrands) ordering.push_back(key); - BOOST_FOREACH(Index key, separator) ordering.push_back(key); - - // eliminate to get marginal - typename FACTORGRAPH::variableindex_type varIndex(p_Cp_R); - Permutation toFront = Permutation::PullToFront(ordering, varIndex.size()); - Permutation::shared_ptr toFrontInverse(toFront.inverse()); - BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, p_Cp_R) { - factor->permuteWithInverse(*toFrontInverse); + // We want to factor into a conditional of the clique variables given the + // root and the marginal on the root, integrating out all other variables. + // The integrands include any parents of this clique and the variables of + // the parent clique. + vector variablesAtBack; + variablesAtBack.reserve(this->size() + R->size()); + BOOST_FOREACH(const Index separatorIndex, this->separator_) { + variablesAtBack.push_back(separatorIndex); + if(debug) cout << "At back (this): " << separatorIndex << endl; + } + BOOST_FOREACH(const sharedConditional& conditional, *R) { + variablesAtBack.push_back(conditional->key()); + if(debug) cout << "At back (root): " << conditional->key() << endl; } - varIndex.permute(toFront); - BayesNet p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex); - // remove all integrands - for(Index j=0; jback()->key() + 1); + Permutation::shared_ptr toBackInverse(toBack.inverse()); + BOOST_FOREACH(const typename CONDITIONAL::Factor::shared_ptr& factor, p_Cp_R) { + factor->permuteWithInverse(*toBackInverse); } + typename BayesNet::shared_ptr eliminated(EliminationTree::Create(p_Cp_R)->eliminate()); + + // take only the conditionals for p(S|R) + BayesNet p_S_R; + typename BayesNet::const_reverse_iterator conditional = eliminated->rbegin(); + BOOST_FOREACH(const sharedConditional& c, *R) { + (void)c; ++conditional; } + BOOST_FOREACH(const Index c, this->separator_) { + if(debug) + (*conditional)->print("Taking C|R conditional: "); + (void)c; p_S_R.push_front(*(conditional++)); } + +// for(Index j=0; j - template - FACTORGRAPH BayesTree::Clique::marginal(shared_ptr R) { + FactorGraph BayesTree::Clique::marginal(shared_ptr R) { // If we are the root, just return this root 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); p_FSR.push_front(*this); p_FSR.push_back(*R); // Find marginal on the keys we are interested in - return Inference::Marginal(FACTORGRAPH(p_FSR), keys()); + FactorGraph p_FSRfg(p_FSR); + + return *GenericSequentialSolver(p_FSR).joint(keys()); } // /* ************************************************************************* */ @@ -689,44 +703,46 @@ namespace gtsam { // First finds clique marginal then marginalizes that /* ************************************************************************* */ template - template - FACTORGRAPH BayesTree::marginal(Index key) const { + typename CONDITIONAL::Factor::shared_ptr BayesTree::marginal(Index key) const { // get clique containing key sharedClique clique = (*this)[key]; // calculate or retrieve its marginal - FACTORGRAPH cliqueMarginal = clique->marginal(root_); + FactorGraph cliqueMarginal = clique->marginal(root_); - // Reorder so that only the requested key is not eliminated - typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal); - vector keyAsVector(1); keyAsVector[0] = key; - Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size())); - Permutation::shared_ptr toBackInverse(toBack.inverse()); - varIndex.permute(toBack); - BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { - factor->permuteWithInverse(*toBackInverse); - } + return GenericSequentialSolver(cliqueMarginal).marginal(key); - // partially eliminate, remaining factor graph is requested marginal - Inference::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex); - BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { - if(factor) - factor->permuteWithInverse(toBack); - } - return cliqueMarginal; +// // Reorder so that only the requested key is not eliminated +// typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal); +// vector keyAsVector(1); keyAsVector[0] = key; +// Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size())); +// Permutation::shared_ptr toBackInverse(toBack.inverse()); +// varIndex.permute(toBack); +// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { +// factor->permuteWithInverse(*toBackInverse); +// } +// +// // partially eliminate, remaining factor graph is requested marginal +// SymbolicSequentialSolver::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex); +// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { +// if(factor) +// factor->permuteWithInverse(toBack); +// } +// return cliqueMarginal; } /* ************************************************************************* */ template - template - BayesNet BayesTree::marginalBayesNet(Index key) const { + typename BayesNet::shared_ptr BayesTree::marginalBayesNet(Index key) const { // calculate marginal as a factor graph - FACTORGRAPH fg = this->marginal(key); + typename FactorGraph::shared_ptr fg( + new FactorGraph()); + fg->push_back(this->marginal(key)); // eliminate further to Bayes net - return *Inference::Eliminate(fg); + return GenericSequentialSolver(*fg).eliminate(); } // /* ************************************************************************* */ diff --git a/inference/BayesTree.h b/inference/BayesTree.h index ffc81a4d4..1c147c7aa 100644 --- a/inference/BayesTree.h +++ b/inference/BayesTree.h @@ -107,12 +107,10 @@ namespace gtsam { /** return the conditional P(S|Root) on the separator given the root */ // TODO: create a cached version - template BayesNet shortcut(shared_ptr root); /** return the marginal P(C) of the clique */ - template - FACTORGRAPH marginal(shared_ptr root); + FactorGraph marginal(shared_ptr root); // /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ // template @@ -257,12 +255,10 @@ namespace gtsam { CliqueData getCliqueData() const; /** return marginal on any variable */ - template - FACTORGRAPH marginal(Index key) const; + typename CONDITIONAL::Factor::shared_ptr marginal(Index key) const; /** return marginal on any variable, as a Bayes Net */ - template - BayesNet marginalBayesNet(Index key) const; + typename BayesNet::shared_ptr marginalBayesNet(Index key) const; // /** return joint on two variables */ // template diff --git a/inference/EliminationTree-inl.h b/inference/EliminationTree-inl.h index c1723cf6d..70d73d5bb 100644 --- a/inference/EliminationTree-inl.h +++ b/inference/EliminationTree-inl.h @@ -21,11 +21,10 @@ namespace gtsam { /* ************************************************************************* */ template -typename EliminationTree::EliminationResult -EliminationTree::eliminate_() const { +typename EliminationTree::sharedFactor +EliminationTree::eliminate_(Conditionals& conditionals) const { - // Create the Bayes net, which will be returned to the parent. Initially empty... - BayesNet bayesNet; + set, boost::fast_pool_allocator > separator; // Create the list of factors to be eliminated, initially empty, and reserve space FactorGraph factors; @@ -36,19 +35,14 @@ EliminationTree::eliminate_() const { // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] BOOST_FOREACH(const shared_ptr& child, subTrees_) { - EliminationResult result = child->eliminate_(); - bayesNet.push_back(result.first); // Bayes net fragment added to Bayes net - factors.push_back(result.second); // Separator factor added to [factors] - } + factors.push_back(child->eliminate_(conditionals)); } // Combine all factors (from this node and from subtrees) into a joint factor sharedFactor jointFactor(FACTOR::Combine(factors, VariableSlots(factors))); + assert(jointFactor->front() == this->key_); + conditionals[this->key_] = jointFactor->eliminateFirst(); - // Eliminate the resulting joint factor and add the conditional to the bayes net - // What remains in the jointFactor will be passed to our parent node - bayesNet.push_back(jointFactor->eliminateFirst()); - - return EliminationResult(bayesNet, jointFactor); + return jointFactor; } /* ************************************************************************* */ @@ -87,18 +81,15 @@ vector EliminationTree::ComputeParents(const VariableIndex<>& str /* ************************************************************************* */ template -template +template typename EliminationTree::shared_ptr -EliminationTree::Create(const FACTORGRAPH& factorGraph) { - - // Create column structure - VariableIndex<> varIndex(factorGraph); +EliminationTree::Create(const FactorGraph& factorGraph, const VariableIndex<>& structure) { // Compute the tree structure - vector parents(ComputeParents(varIndex)); + vector parents(ComputeParents(structure)); // Number of variables - const size_t n = varIndex.size(); + const size_t n = structure.size(); static const Index none = numeric_limits::max(); @@ -112,7 +103,7 @@ EliminationTree::Create(const FACTORGRAPH& factorGraph) { } // Hang factors in right places - BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& derivedFactor, factorGraph) { + BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) { // Here we static_cast to the factor type of this EliminationTree. This // allows performing symbolic elimination on, for example, GaussianFactors. sharedFactor factor(boost::shared_static_cast(derivedFactor)); @@ -129,6 +120,14 @@ EliminationTree::Create(const FACTORGRAPH& factorGraph) { return trees.back(); } +/* ************************************************************************* */ +template +template +typename EliminationTree::shared_ptr +EliminationTree::Create(const FactorGraph& factorGraph) { + return Create(factorGraph, VariableIndex<>(factorGraph)); +} + /* ************************************************************************* */ template void EliminationTree::print(const std::string& name) const { @@ -160,8 +159,16 @@ typename EliminationTree::BayesNet::shared_ptr EliminationTree::eliminate() const { // call recursive routine - EliminationResult result = eliminate_(); - return typename BayesNet::shared_ptr(new BayesNet(result.first)); + Conditionals conditionals(this->key_ + 1); + (void)eliminate_(conditionals); + + // Add conditionals to BayesNet + typename BayesNet::shared_ptr bayesNet(new BayesNet); + BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) { + if(conditional) + bayesNet->push_back(conditional); + } + return bayesNet; } } diff --git a/inference/EliminationTree.h b/inference/EliminationTree.h index b7098558d..53f779ada 100644 --- a/inference/EliminationTree.h +++ b/inference/EliminationTree.h @@ -13,6 +13,7 @@ #include #include +#include class EliminationTreeTester; // for unit tests, see testEliminationTree @@ -35,13 +36,12 @@ private: typedef std::list > Factors; typedef std::list > SubTrees; + typedef std::vector Conditionals; Index key_; /** index associated with root */ Factors factors_; /** factors associated with root */ SubTrees subTrees_; /** sub-trees */ - typedef std::pair EliminationResult; - /** default constructor, private, as you should use Create below */ EliminationTree(Index key = 0) : key_(key) {} @@ -60,16 +60,23 @@ private: /** * Recursive routine that eliminates the factors arranged in an elimination tree */ - EliminationResult eliminate_() const; + sharedFactor eliminate_(Conditionals& conditionals) const; // Allow access to constructor and add methods for testing purposes friend class ::EliminationTreeTester; public: + /** + * Named constructor to build the elimination tree of a factor graph using + * pre-computed column structure. + */ + template + static shared_ptr Create(const FactorGraph& factorGraph, const VariableIndex<>& structure); + /** Named constructor to build the elimination tree of a factor graph */ - template - static shared_ptr Create(const FACTORGRAPH& factorGraph); + template + static shared_ptr Create(const FactorGraph& factorGraph); /** Print the tree to cout */ void print(const std::string& name = "EliminationTree: ") const; diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h index 365503141..597b5326b 100644 --- a/inference/FactorGraph.h +++ b/inference/FactorGraph.h @@ -160,7 +160,7 @@ namespace gtsam { factors_.reserve(factorGraph.size()); BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) { if(factor) - this->push_back(sharedFactor(new FACTOR(*factor))); + this->push_back(factor); else this->push_back(sharedFactor()); } diff --git a/inference/ISAM-inl.h b/inference/ISAM-inl.h index 6c69d8d93..73a8185b4 100644 --- a/inference/ISAM-inl.h +++ b/inference/ISAM-inl.h @@ -24,6 +24,7 @@ using namespace boost::assign; #include #include #include +#include namespace gtsam { @@ -52,7 +53,7 @@ namespace gtsam { factors.push_back(newFactors); // eliminate into a Bayes net - typename BayesNet::shared_ptr bayesNet = Inference::Eliminate(factors); + typename BayesNet::shared_ptr bayesNet = GenericSequentialSolver(factors).eliminate(); // insert conditionals back in, straight into the topless bayesTree typename BayesNet::const_reverse_iterator rit; diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h index 9e771c768..b32c559ba 100644 --- a/inference/JunctionTree-inl.h +++ b/inference/JunctionTree-inl.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -42,7 +42,7 @@ namespace gtsam { // Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph // -> SymbolicBayesNet -> SymbolicBayesTree tic("JT 1.1 symbolic elimination"); - SymbolicBayesNet::shared_ptr sbn = EliminationTree::Create(fg)->eliminate(); + SymbolicBayesNet::shared_ptr sbn = SymbolicSequentialSolver(fg).eliminate(); // SymbolicFactorGraph sfg(fg); // SymbolicBayesNet::shared_ptr sbn_orig = Inference::Eliminate(sfg); // assert(assert_equal(*sbn, *sbn_orig)); diff --git a/inference/Makefile.am b/inference/Makefile.am index c71122d33..f11f654ac 100644 --- a/inference/Makefile.am +++ b/inference/Makefile.am @@ -18,7 +18,7 @@ check_PROGRAMS = headers += Factor.h Factor-inl.h Conditional.h # Symbolic Inference -sources += SymbolicFactorGraph.cpp +sources += SymbolicFactorGraph.cpp SymbolicSequentialSolver.cpp check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots diff --git a/inference/SymbolicFactorGraph.h b/inference/SymbolicFactorGraph.h index 7ae803a92..308341e89 100644 --- a/inference/SymbolicFactorGraph.h +++ b/inference/SymbolicFactorGraph.h @@ -25,10 +25,12 @@ #include #include #include +#include namespace gtsam { typedef BayesNet SymbolicBayesNet; +typedef EliminationTree SymbolicEliminationTree; /** Symbolic IndexFactor Graph */ class SymbolicFactorGraph: public FactorGraph { diff --git a/inference/VariableIndex.h b/inference/VariableIndex.h index 4df1936d2..0a8637166 100644 --- a/inference/VariableIndex.h +++ b/inference/VariableIndex.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { @@ -72,6 +73,7 @@ protected: public: VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {} + template VariableIndex(const FactorGraph& factorGraph, Index nVariables); template VariableIndex(const FactorGraph& factorGraph); Index size() const { return index_.size(); } @@ -90,6 +92,8 @@ protected: VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {} void checkVar(Index variable) const { assert(variable < index_.size()); } + template void fill(const FactorGraph& factorGraph); + factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } const_factor_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } @@ -115,6 +119,27 @@ void VariableIndex::permute(const Permutation& permutation) { // this->index_[j].swap(original[permutation[j]]); } +/* ************************************************************************* */ +template +template +void VariableIndex::fill(const FactorGraph& factorGraph) { + + // Build index mapping from variable id to factor index + for(size_t fi=0; fikeys()) { + if(key < index_.size()) { + index_[key].push_back(mapped_factor_type(fi, fvari)); + ++ fvari; + ++ nEntries_; + } + } + ++ nFactors_; + } + +} + /* ************************************************************************* */ template template @@ -135,22 +160,21 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : } } - // Allocate index + // Allocate array index_.container().resize(maxVar+1); index_.permutation() = Permutation::Identity(maxVar+1); - // Build index mapping from variable id to factor index - for(size_t fi=0; fikeys()) { - index_[key].push_back(mapped_factor_type(fi, fvari)); - ++ fvari; - ++ nEntries_; - } - ++ nFactors_; - } + fill(factorGraph); } + +} + +/* ************************************************************************* */ +template +template +VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) : + indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { + fill(factorGraph); } /* ************************************************************************* */ diff --git a/inference/inference-inl.h b/inference/inference-inl.h index 08322b8c3..d3c431ffe 100644 --- a/inference/inference-inl.h +++ b/inference/inference-inl.h @@ -39,296 +39,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -template -inline typename FACTORGRAPH::bayesnet_type::shared_ptr Inference::Eliminate(const FACTORGRAPH& factorGraph) { - - // Create a copy of the factor graph to eliminate in-place - FACTORGRAPH eliminationGraph(factorGraph); - typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph); - - return Eliminate(eliminationGraph, variableIndex); -} - -/* ************************************************************************* */ -//template -//BayesNet::shared_ptr Inference::EliminateSymbolic(const FactorGraph& factorGraph) { -// -// // Create a copy of the factor graph to eliminate in-place -// FactorGraph eliminationGraph(factorGraph); -// VariableIndex<> variableIndex(eliminationGraph); -// -// typename BayesNet::shared_ptr bayesnet(new BayesNet()); -// -// // Eliminate variables one-by-one, updating the eliminated factor graph and -// // the variable index. -// for(Index var = 0; var < variableIndex.size(); ++var) { -// Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var)); -// if(conditional) // Will be NULL if the variable did not appear in the factor graph. -// bayesnet->push_back(conditional); -// } -// -// return bayesnet; -//} - -/* ************************************************************************* */ -template -inline typename FACTORGRAPH::bayesnet_type::shared_ptr -Inference::Eliminate(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex) { - - return EliminateUntil(factorGraph, variableIndex.size(), variableIndex); -} - -/* ************************************************************************* */ -template -inline typename FACTORGRAPH::bayesnet_type::shared_ptr -Inference::EliminateUntil(const FACTORGRAPH& factorGraph, Index bound) { - - // Create a copy of the factor graph to eliminate in-place - FACTORGRAPH eliminationGraph(factorGraph); - typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph); - - return EliminateUntil(eliminationGraph, bound, variableIndex); -} - -/* ************************************************************************* */ -template -typename FACTORGRAPH::bayesnet_type::shared_ptr -Inference::EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex) { - - typename FACTORGRAPH::bayesnet_type::shared_ptr bayesnet(new typename FACTORGRAPH::bayesnet_type); - - // Eliminate variables one-by-one, updating the eliminated factor graph and - // the variable index. - for(Index var = 0; var < bound; ++var) { - typename FACTORGRAPH::bayesnet_type::sharedConditional conditional(EliminateOne(factorGraph, variableIndex, var)); - if(conditional) // Will be NULL if the variable did not appear in the factor graph. - bayesnet->push_back(conditional); - } - - return bayesnet; -} - -/* ************************************************************************* */ -template -typename FACTORGRAPH::bayesnet_type::sharedConditional -Inference::EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var) { - - /* This function performs symbolic elimination of a variable, comprising - * combining involved factors (analogous to "assembly" in SPQR) followed by - * eliminating to an upper-trapezoidal factor using spqr_front. This - * function performs the bookkeeping necessary for high performance. - * - * When combining factors, variables are merge sorted so that they remain - * in elimination order in the combined factor. GaussianFactor combines - * rows such that the row index after the last structural non-zero in each - * column increases monotonically (referred to as the "staircase" pattern in - * SPQR). The variable ordering is passed into the factor's Combine(...) - * function, which does the work of actually building the combined factor - * (for a GaussianFactor this assembles the augmented matrix). - * - * Next, this function calls the factor's eliminateFirst() function, which - * factorizes the factor into a conditional on the first variable and a - * factor on the remaining variables. In addition, this function updates the - * bookkeeping of the pattern of structural non-zeros. The GaussianFactor - * calls spqr_front during eliminateFirst(), which reduces its matrix to - * upper-trapezoidal form. - * - * Returns NULL if the variable does not appear in factorGraph. - */ - - tic("EliminateOne"); - - // Get the factors involving the eliminated variable - typename FACTORGRAPH::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]); - typedef typename FACTORGRAPH::variableindex_type::mapped_factor_type mapped_factor_type; - - if(!varIndexEntry.empty()) { - - vector removedFactors(varIndexEntry.size()); - transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(), - boost::lambda::bind(&FACTORGRAPH::variableindex_type::mapped_factor_type::factorIndex, boost::lambda::_1)); - - // The new joint factor will be the last one in the factor graph - size_t jointFactorIndex = factorGraph.size(); - - static const bool debug = false; - - if(debug) { - cout << "Eliminating " << var; - factorGraph.print(" from graph: "); - cout << removedFactors.size() << " factors to remove" << endl; - } - - // Compute the involved keys, uses the variableIndex to mark whether each - // key has been added yet, but the positions stored in the variableIndex are - // from the unsorted positions and will be fixed later. - tic("EliminateOne: Find involved vars"); - map, boost::fast_pool_allocator > > involvedKeys; // Variable and original order as discovered - BOOST_FOREACH(size_t removedFactorI, removedFactors) { - if(debug) cout << removedFactorI << " is involved" << endl; - // If the factor has not previously been removed - if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) { - // Loop over the variables involved in the removed factor to update the - // variable index and joint factor positions of each variable. - BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) { - // Mark the new joint factor as involving each variable in the removed factor. - assert(!variableIndex[involvedVariable].empty()); - if(variableIndex[involvedVariable].back().factorIndex != jointFactorIndex) { - if(debug) cout << " pulls in variable " << involvedVariable << endl; - size_t varpos = involvedKeys.size(); - variableIndex[involvedVariable].push_back(mapped_factor_type(jointFactorIndex, varpos)); -#ifndef NDEBUG - bool inserted = -#endif - involvedKeys.insert(make_pair(involvedVariable, varpos)).second; - assert(inserted); - } else if(debug) - cout << " involves variable " << involvedVariable << " which was previously discovered" << endl; - } - } - } - toc("EliminateOne: Find involved vars"); - if(debug) cout << removedFactors.size() << " factors to remove" << endl; - - // Compute the permutation to go from the original varpos to the sorted - // joint factor varpos - if(debug) cout << "Sorted keys:"; - tic("EliminateOne: Sort involved vars"); - vector varposPermutation(involvedKeys.size(), numeric_limits::max()); - vector sortedKeys(involvedKeys.size()); - { - size_t sortedVarpos = 0; - const map, boost::fast_pool_allocator > >& involvedKeysC(involvedKeys); - for(map, boost::fast_pool_allocator > >::const_iterator key_pos=involvedKeysC.begin(); key_pos!=involvedKeysC.end(); ++key_pos) { - sortedKeys[sortedVarpos] = key_pos->first; - assert(varposPermutation[key_pos->second] == numeric_limits::max()); - varposPermutation[key_pos->second] = sortedVarpos; - if(debug) cout << " " << key_pos->first << " (" << key_pos->second << "->" << sortedVarpos << ") "; - ++ sortedVarpos; - } - } - toc("EliminateOne: Sort involved vars"); - if(debug) cout << endl; - - assert(sortedKeys.front() == var); - if(debug) cout << removedFactors.size() << " factors to remove" << endl; - - // Fix the variable positions in the variableIndex - tic("EliminateOne: Fix varIndex"); - for(size_t sortedPos=0; sortedPos" << sortedPos << endl; - variableIndex[var].back().variablePosition = sortedPos; - } - toc("EliminateOne: Fix varIndex"); - - // Fill in the jointFactorPositions - tic("EliminateOne: Fill jointFactorPositions"); - vector removedFactorIdxs; - removedFactorIdxs.reserve(removedFactors.size()); - vector > jointFactorPositions; - jointFactorPositions.reserve(removedFactors.size()); - if(debug) cout << removedFactors.size() << " factors to remove" << endl; - BOOST_FOREACH(size_t removedFactorI, removedFactors) { - if(debug) cout << "Fixing variable positions for factor " << removedFactorI << endl; - // If the factor has not previously been removed - if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) { - - // Allocate space - jointFactorPositions.push_back(vector()); - vector& jointFactorPositionsCur(jointFactorPositions.back()); - jointFactorPositionsCur.reserve(factorGraph[removedFactorI]->keys().size()); - removedFactorIdxs.push_back(removedFactorI); - - // Loop over the variables involved in the removed factor to update the - // variable index and joint factor positions of each variable. - BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) { - // Mark the new joint factor as involving each variable in the removed factor - assert(!variableIndex[involvedVariable].empty()); - assert(variableIndex[involvedVariable].back().factorIndex == jointFactorIndex); - const size_t varpos = variableIndex[involvedVariable].back().variablePosition; - jointFactorPositionsCur.push_back(varpos); - if(debug) cout << "Variable " << involvedVariable << " from factor " << removedFactorI; - if(debug) cout << " goes in position " << varpos << " of the joint factor" << endl; - assert(sortedKeys[varpos] == involvedVariable); - } - } - } - toc("EliminateOne: Fill jointFactorPositions"); - - // Join the factors and eliminate the variable from the joint factor - tic("EliminateOne: Combine"); - typename FACTORGRAPH::sharedFactor jointFactor( - FACTORGRAPH::Factor::Combine( - factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions)); - toc("EliminateOne: Combine"); - - // Remove the original factors - BOOST_FOREACH(size_t removedFactorI, removedFactors) { - if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) - factorGraph.remove(removedFactorI); - } - - typename FACTORGRAPH::bayesnet_type::sharedConditional conditional; - tic("EliminateOne: eliminateFirst"); - conditional = jointFactor->eliminateFirst(); // Eliminate the first variable in-place - toc("EliminateOne: eliminateFirst"); - tic("EliminateOne: store eliminated"); - variableIndex[sortedKeys.front()].pop_back(); // Unmark the joint factor from involving the eliminated variable - factorGraph.push_back(jointFactor); // Put the eliminated factor into the factor graph - toc("EliminateOne: store eliminated"); - - toc("EliminateOne"); - - return conditional; - - } else { // varIndexEntry.empty() - toc("EliminateOne"); - return typename FACTORGRAPH::bayesnet_type::sharedConditional(); - } -} - -/* ************************************************************************* */ -template -FACTORGRAPH Inference::Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables) { - - // Compute a COLAMD permutation with the marginal variables constrained to the end - typename FACTORGRAPH::variableindex_type varIndex(factorGraph); - Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(varIndex, variables)); - Permutation::shared_ptr permutationInverse(permutation->inverse()); - - // Copy and permute the factors - varIndex.permute(*permutation); - FACTORGRAPH eliminationGraph; eliminationGraph.reserve(factorGraph.size()); - BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, factorGraph) { - typename FACTORGRAPH::sharedFactor permFactor(new typename FACTORGRAPH::Factor(*factor)); - permFactor->permuteWithInverse(*permutationInverse); - eliminationGraph.push_back(permFactor); - } - - // Eliminate all variables - typename FACTORGRAPH::bayesnet_type::shared_ptr bn(Inference::Eliminate(eliminationGraph, varIndex)); - - // The last conditionals in the eliminated BayesNet contain the marginal for - // the variables we want. Undo the permutation as we add the marginal - // factors. - FACTORGRAPH marginal; marginal.reserve(variables.size()); - typename FACTORGRAPH::bayesnet_type::const_reverse_iterator conditional = bn->rbegin(); - for(Index j=0; jpermuteWithInverse(*permutation); - marginal.push_back(factor); - assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end()); - } - - // Undo the permutation - return marginal; -} - /* ************************************************************************* */ template Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast) { @@ -398,109 +108,4 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& va return permutation; } - -// /* ************************************************************************* */ -// /* eliminate one node from the factor graph */ -// /* ************************************************************************* */ -// template -// boost::shared_ptr eliminateOne(FactorGraph& graph, Index key) { -// -// // combine the factors of all nodes connected to the variable to be eliminated -// // if no factors are connected to key, returns an empty factor -// boost::shared_ptr joint_factor = removeAndCombineFactors(graph,key); -// -// // eliminate that joint factor -// boost::shared_ptr factor; -// boost::shared_ptr conditional; -// boost::tie(conditional, factor) = joint_factor->eliminate(key); -// -// // add new factor on separator back into the graph -// if (!factor->empty()) graph.push_back(factor); -// -// // return the conditional Gaussian -// return conditional; -// } -// -// /* ************************************************************************* */ -// // This doubly templated function is generic. There is a GaussianFactorGraph -// // version that returns a more specific GaussianBayesNet. -// // Note, you will need to include this file to instantiate the function. -// /* ************************************************************************* */ -// template -// BayesNet eliminate(FactorGraph& factorGraph, const Ordering& ordering) -// { -// BayesNet bayesNet; // empty -// -// BOOST_FOREACH(Index key, ordering) { -// boost::shared_ptr cg = eliminateOne(factorGraph,key); -// bayesNet.push_back(cg); -// } -// -// return bayesNet; -// } - -// /* ************************************************************************* */ -// template -// pair< BayesNet, FactorGraph > -// factor(const BayesNet& bn, const Ordering& keys) { -// // Convert to factor graph -// FactorGraph factorGraph(bn); -// -// // Get the keys of all variables and remove all keys we want the marginal for -// Ordering ord = bn.ordering(); -// BOOST_FOREACH(Index key, keys) ord.remove(key); // TODO: O(n*k), faster possible? -// -// // eliminate partially, -// BayesNet conditional = eliminate(factorGraph,ord); -// -// // at this moment, the factor graph only encodes P(keys) -// return make_pair(conditional,factorGraph); -// } -// -// /* ************************************************************************* */ -// template -// FactorGraph marginalize(const BayesNet& bn, const Ordering& keys) { -// -// // factor P(X,Y) as P(X|Y)P(Y), where Y corresponds to keys -// pair< BayesNet, FactorGraph > factors = -// gtsam::factor(bn,keys); -// -// // throw away conditional, return marginal P(Y) -// return factors.second; -// } - - /* ************************************************************************* */ -// pair marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key) { -// -// // todo: this does not use colamd! -// -// list ord; -// BOOST_FOREACH(const Symbol& k, fg.keys()) { -// if(k != key) -// ord.push_back(k); -// } -// Ordering ordering(ord); -// -// // Now make another factor graph where we eliminate all the other variables -// GaussianFactorGraph marginal(fg); -// marginal.eliminate(ordering); -// -// GaussianFactor::shared_ptr factor; -// for(size_t i=0; ikeys().size() != 1 || factor->keys().front() != key) -// throw runtime_error("Didn't get the right marginal!"); -// -// VectorValues mean_cfg(marginal.optimize(Ordering(key))); -// Matrix A(factor->get_A(key)); -// -// return make_pair(mean_cfg[key], inverse(prod(trans(A), A))); -// } - - /* ************************************************************************* */ - } // namespace gtsam diff --git a/inference/inference.cpp b/inference/inference.cpp index 616ddb1d2..d9dbac9ae 100644 --- a/inference/inference.cpp +++ b/inference/inference.cpp @@ -22,83 +22,4 @@ namespace gtsam { -/* ************************************************************************* */ -//Conditional::shared_ptr -//Inference::EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, Index var) { -// -// tic("EliminateOne"); -// -// // Get the factors involving the eliminated variable -// VariableIndex<>::mapped_type& varIndexEntry(variableIndex[var]); -// typedef VariableIndex<>::mapped_factor_type mapped_factor_type; -// -// if(!varIndexEntry.empty()) { -// -// vector removedFactors(varIndexEntry.size()); -// transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(), -// boost::lambda::bind(&VariableIndex<>::mapped_factor_type::factorIndex, boost::lambda::_1)); -// -// // The new joint factor will be the last one in the factor graph -// size_t jointFactorIndex = factorGraph.size(); -// -// static const bool debug = false; -// -// if(debug) { -// cout << "Eliminating " << var; -// factorGraph.print(" from graph: "); -// cout << removedFactors.size() << " factors to remove" << endl; -// } -// -// // Compute the involved keys, uses the variableIndex to mark whether each -// // key has been added yet, but the positions stored in the variableIndex are -// // from the unsorted positions and will be fixed later. -// tic("EliminateOne: Find involved vars"); -// typedef set, boost::fast_pool_allocator > InvolvedKeys; -// InvolvedKeys involvedKeys; -// BOOST_FOREACH(size_t removedFactorI, removedFactors) { -// if(debug) cout << removedFactorI << " is involved" << endl; -// // If the factor has not previously been removed -// if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) { -// // Loop over the variables involved in the removed factor to update the -// // variable index and joint factor positions of each variable. -// BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) { -// if(debug) cout << " pulls in variable " << involvedVariable << endl; -// // Mark the new joint factor as involving each variable in the removed factor. -// assert(!variableIndex[involvedVariable].empty()); -// involvedKeys.insert(involvedVariable); -// } -// } -// -// // Remove the original factor -// factorGraph.remove(removedFactorI); -// } -// -// // We need only mark the next variable to be eliminated as involved with the joint factor -// if(involvedKeys.size() > 1) { -// InvolvedKeys::const_iterator next = involvedKeys.begin(); ++ next; -// variableIndex[*next].push_back(mapped_factor_type(jointFactorIndex,0)); -// } -// toc("EliminateOne: Find involved vars"); -// if(debug) cout << removedFactors.size() << " factors to remove" << endl; -// -// // Join the factors and eliminate the variable from the joint factor -// tic("EliminateOne: Combine"); -// Conditional::shared_ptr conditional = Conditional::FromRange(involvedKeys.begin(), involvedKeys.end(), 1); -// Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents())); -// toc("EliminateOne: Combine"); -// -// tic("EliminateOne: store eliminated"); -// factorGraph.push_back(eliminated); // Put the eliminated factor into the factor graph -// toc("EliminateOne: store eliminated"); -// -// toc("EliminateOne"); -// -// return conditional; -// -// } else { // varIndexEntry.empty() -// toc("EliminateOne"); -// return Conditional::shared_ptr(); -// } -//} - } diff --git a/inference/inference.h b/inference/inference.h index 1dc408155..f8951cbeb 100644 --- a/inference/inference.h +++ b/inference/inference.h @@ -36,74 +36,6 @@ namespace gtsam { public: - /** - * Eliminate a factor graph in its natural ordering, i.e. eliminating - * variables in order starting from 0. - */ - template - static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(const FACTORGRAPH& factorGraph); - - /** - * Eliminate a factor graph in its natural ordering, i.e. eliminating - * variables in order starting from 0. Special fast version for symbolic - * elimination. - */ -// template -// static BayesNet::shared_ptr EliminateSymbolic(const FactorGraph& factorGraph); - - /** - * Eliminate a factor graph in its natural ordering, i.e. eliminating - * variables in order starting from 0. Uses an existing - * variable index instead of building one from scratch. - */ - template - static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate( - FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex); - - /** - * Partially eliminate a factor graph, up to but not including the given - * variable. - */ - template - static typename FACTORGRAPH::bayesnet_type::shared_ptr - EliminateUntil(const FACTORGRAPH& factorGraph, Index bound); - - /** - * Partially eliminate a factor graph, up to but not including the given - * variable. Use an existing variable index instead of building one from - * scratch. - */ - template - static typename FACTORGRAPH::bayesnet_type::shared_ptr - EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex); - - /** - * Eliminate a single variable, updating an existing factor graph and - * variable index. - */ - template - static typename FACTORGRAPH::bayesnet_type::sharedConditional - EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var); - - /** - * Eliminate a single variable, updating an existing factor graph and - * variable index. This is a specialized faster version for purely - * symbolic factor graphs. - */ -// static boost::shared_ptr -// EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, Index var); - - /** - * Eliminate all variables except the specified ones. Internally this - * permutes these variables to the end of the ordering, eliminates all - * other variables, and then undoes the permutation. This is - * inefficient if multiple marginals are needed - in that case use the - * BayesTree which supports efficiently computing marginals for multiple - * variables. - */ - template - static FACTORGRAPH Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables); - /** * Compute a permutation (variable ordering) using colamd */ @@ -112,16 +44,6 @@ namespace gtsam { template static boost::shared_ptr PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast); -// /** -// * Join several factors into one. This involves determining the set of -// * shared variables and the correct variable positions in the new joint -// * factor. -// */ -// template -// static typename FACTORGRAPH::shared_factor Combine(const FACTORGRAPH& factorGraph, -// InputIterator indicesBegin, InputIterator indicesEnd); - - }; // ELIMINATE: FACTOR GRAPH -> BAYES NET diff --git a/inference/tests/testBayesTree.cpp b/inference/tests/testBayesTree.cpp index 7c8416d0c..fc3186247 100644 --- a/inference/tests/testBayesTree.cpp +++ b/inference/tests/testBayesTree.cpp @@ -26,7 +26,7 @@ using namespace boost::assign; #include #include #include -#include +#include using namespace gtsam; @@ -333,62 +333,61 @@ TEST( BayesTree, removeTop3 ) CHECK(orphans.size() == 0); } -/* ************************************************************************* */ -/** - * x2 - x3 - x4 - x5 - * | / \ | - * x1 / \ x6 - */ -TEST( BayesTree, insert ) -{ - // construct bayes tree by split the graph along the separator x3 - x4 - const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5; - SymbolicFactorGraph fg1, fg2, fg3; - fg1.push_factor(_x3_, _x4_); - fg2.push_factor(_x1_, _x2_); - fg2.push_factor(_x2_, _x3_); - fg2.push_factor(_x1_, _x3_); - fg3.push_factor(_x5_, _x4_); - fg3.push_factor(_x6_, _x5_); - fg3.push_factor(_x6_, _x4_); - -// Ordering ordering1; ordering1 += _x3_, _x4_; -// Ordering ordering2; ordering2 += _x1_, _x2_; -// Ordering ordering3; ordering3 += _x6_, _x5_; - - BayesNet bn1, bn2, bn3; - bn1 = *Inference::EliminateUntil(fg1, _x4_+1); - bn2 = *Inference::EliminateUntil(fg2, _x2_+1); - bn3 = *Inference::EliminateUntil(fg3, _x5_+1); - - // insert child cliques - SymbolicBayesTree actual; - list children; - SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children); - SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children); - - // insert root clique - children.push_back(r1); - children.push_back(r2); - actual.insert(bn1, children, true); - - // traditional way - SymbolicFactorGraph fg; - fg.push_factor(_x3_, _x4_); - fg.push_factor(_x1_, _x2_); - fg.push_factor(_x2_, _x3_); - fg.push_factor(_x1_, _x3_); - fg.push_factor(_x5_, _x4_); - fg.push_factor(_x6_, _x5_); - fg.push_factor(_x6_, _x4_); - -// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; - BayesNet bn; - bn = *Inference::Eliminate(fg); - SymbolicBayesTree expected(bn); - CHECK(assert_equal(expected, actual)); - -} +///* ************************************************************************* */ +///** +// * x2 - x3 - x4 - x5 +// * | / \ | +// * x1 / \ x6 +// */ +//TEST( BayesTree, insert ) +//{ +// // construct bayes tree by split the graph along the separator x3 - x4 +// const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5; +// SymbolicFactorGraph fg1, fg2, fg3; +// fg1.push_factor(_x3_, _x4_); +// fg2.push_factor(_x1_, _x2_); +// fg2.push_factor(_x2_, _x3_); +// fg2.push_factor(_x1_, _x3_); +// fg3.push_factor(_x5_, _x4_); +// fg3.push_factor(_x6_, _x5_); +// fg3.push_factor(_x6_, _x4_); +// +//// Ordering ordering1; ordering1 += _x3_, _x4_; +//// Ordering ordering2; ordering2 += _x1_, _x2_; +//// Ordering ordering3; ordering3 += _x6_, _x5_; +// +// BayesNet bn1, bn2, bn3; +// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1); +// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1); +// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1); +// +// // insert child cliques +// SymbolicBayesTree actual; +// list children; +// SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children); +// SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children); +// +// // insert root clique +// children.push_back(r1); +// children.push_back(r2); +// actual.insert(bn1, children, true); +// +// // traditional way +// SymbolicFactorGraph fg; +// fg.push_factor(_x3_, _x4_); +// fg.push_factor(_x1_, _x2_); +// fg.push_factor(_x2_, _x3_); +// fg.push_factor(_x1_, _x3_); +// fg.push_factor(_x5_, _x4_); +// fg.push_factor(_x6_, _x5_); +// fg.push_factor(_x6_, _x4_); +// +//// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; +// BayesNet bn(*SymbolicSequentialSolver(fg).eliminate()); +// SymbolicBayesTree expected(bn); +// CHECK(assert_equal(expected, actual)); +// +//} /* ************************************************************************* */ int main() { diff --git a/inference/tests/testEliminationTree.cpp b/inference/tests/testEliminationTree.cpp index ed2a0ac86..c4c26d289 100644 --- a/inference/tests/testEliminationTree.cpp +++ b/inference/tests/testEliminationTree.cpp @@ -9,13 +9,11 @@ #include #include -#include +#include using namespace gtsam; using namespace std; -typedef EliminationTree SymbolicEliminationTree; - struct EliminationTreeTester { // build hardcoded tree static SymbolicEliminationTree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) { @@ -76,10 +74,10 @@ TEST(EliminationTree, eliminate ) IndexConditional::shared_ptr c4(new IndexConditional(4)); SymbolicBayesNet expected; - expected.push_back(c3); expected.push_back(c0); expected.push_back(c1); expected.push_back(c2); + expected.push_back(c3); expected.push_back(c4); // Create factor graph @@ -91,7 +89,7 @@ TEST(EliminationTree, eliminate ) fg.push_factor(3, 4); // eliminate - SymbolicBayesNet actual = *SymbolicEliminationTree::Create(fg)->eliminate(); + SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); CHECK(assert_equal(expected,actual)); } diff --git a/inference/tests/testJunctionTree.cpp b/inference/tests/testJunctionTree.cpp index 62502904c..c3ec4dff7 100644 --- a/inference/tests/testJunctionTree.cpp +++ b/inference/tests/testJunctionTree.cpp @@ -83,7 +83,7 @@ TEST( JunctionTree, eliminate) SymbolicJunctionTree jt(fg); SymbolicBayesTree::sharedClique actual = jt.eliminate(); - BayesNet bn = *Inference::Eliminate(fg); + BayesNet bn(*SymbolicSequentialSolver(fg).eliminate()); SymbolicBayesTree expected(bn); // cout << "BT from JT:\n"; diff --git a/inference/tests/testSymbolicFactorGraph.cpp b/inference/tests/testSymbolicFactorGraph.cpp index 41b215c37..8450db72a 100644 --- a/inference/tests/testSymbolicFactorGraph.cpp +++ b/inference/tests/testSymbolicFactorGraph.cpp @@ -24,7 +24,7 @@ using namespace boost::assign; #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -33,21 +33,20 @@ static const Index vx2 = 0; static const Index vx1 = 1; static const Index vl1 = 2; -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, EliminateOne ) -{ - // create a test graph - SymbolicFactorGraph fg; - fg.push_factor(vx2, vx1); - - VariableIndex<> variableIndex(fg); - Inference::EliminateOne(fg, variableIndex, vx2); - SymbolicFactorGraph expected; - expected.push_back(boost::shared_ptr()); - expected.push_factor(vx1); - - CHECK(assert_equal(expected, fg)); -} +///* ************************************************************************* */ +//TEST( SymbolicFactorGraph, EliminateOne ) +//{ +// // create a test graph +// SymbolicFactorGraph fg; +// fg.push_factor(vx2, vx1); +// +// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1); +// SymbolicFactorGraph expected; +// expected.push_back(boost::shared_ptr()); +// expected.push_factor(vx1); +// +// CHECK(assert_equal(expected, fg)); +//} /* ************************************************************************* */ TEST( SymbolicFactorGraph, constructFromBayesNet ) diff --git a/linear/Factorization.h b/linear/Factorization.h index acf8bb29f..0f6848519 100644 --- a/linear/Factorization.h +++ b/linear/Factorization.h @@ -22,6 +22,7 @@ #include #include +#include #include namespace gtsam { @@ -47,7 +48,7 @@ namespace gtsam { * the resulted linear system */ VectorValues optimize(GaussianFactorGraph& fg) const { - return gtsam::optimize(*Inference::Eliminate(fg)); + return *GaussianSequentialSolver(fg).optimize(); } /** diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 9697d8cf3..307b79d9f 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -51,6 +51,12 @@ namespace gtsam { */ GaussianFactorGraph(const GaussianBayesNet& CBN); + /** Constructor from a factor graph of GaussianFactor or a derived type */ + template + GaussianFactorGraph(const FactorGraph& fg) { + push_back(fg); + } + /** Add a null factor */ void add(const Vector& b) { push_back(sharedFactor(new GaussianFactor(b))); diff --git a/linear/Makefile.am b/linear/Makefile.am index da26fd224..7cd462e62 100644 --- a/linear/Makefile.am +++ b/linear/Makefile.am @@ -21,6 +21,9 @@ check_PROGRAMS += tests/testVectorValues #sources += VectorMap.cpp VectorBTree.cpp #check_PROGRAMS += tests/testVectorMap tests/testVectorBTree +# Solvers +sources += GaussianSequentialSolver.cpp + # Gaussian Factor Graphs headers += GaussianFactorSet.h Factorization.h sources += GaussianFactor.cpp GaussianFactorGraph.cpp diff --git a/linear/SubgraphSolver-inl.h b/linear/SubgraphSolver-inl.h index 4d101de86..22b452b40 100644 --- a/linear/SubgraphSolver-inl.h +++ b/linear/SubgraphSolver-inl.h @@ -24,6 +24,7 @@ #include #include #include +#include using namespace std; @@ -69,7 +70,7 @@ namespace gtsam { SubgraphPreconditioner::sharedValues xbar; #else GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!! - SubgraphPreconditioner::sharedBayesNet Rc1 = Inference::Eliminate(sacrificialAb1) ; + SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree::Create(sacrificialAb1)->eliminate(); SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); #endif // TODO: there does not seem to be a good reason to have Ab1_ diff --git a/linear/tests/testGaussianFactor.cpp b/linear/tests/testGaussianFactor.cpp index 950852283..c12855540 100644 --- a/linear/tests/testGaussianFactor.cpp +++ b/linear/tests/testGaussianFactor.cpp @@ -34,7 +34,7 @@ using namespace boost::assign; #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -610,22 +610,21 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional ) CHECK(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); - GaussianFactor::shared_ptr expected(new GaussianFactor(_x0_, Ax, b, noisemodel)); - GaussianFactorGraph graph; - graph.push_back(expected); - - GaussianVariableIndex<> index(graph); - GaussianConditional::shared_ptr conditional = Inference::EliminateOne(graph,index,_x0_); - GaussianFactor actual(*conditional); - - CHECK(assert_equal(*expected, actual)); -} +///* ************************************************************************* */ +//TEST( GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained ) +//{ +// Matrix Ax = eye(2); +// Vector b = Vector_(2, 3.0, 5.0); +// SharedDiagonal noisemodel = noiseModel::Constrained::All(2); +// GaussianFactor::shared_ptr expected(new GaussianFactor(_x0_, Ax, b, noisemodel)); +// GaussianFactorGraph graph; +// graph.push_back(expected); +// +// GaussianConditional::shared_ptr conditional = GaussianSequentialSolver::EliminateUntil(graph,_x0_+1); +// GaussianFactor actual(*conditional); +// +// CHECK(assert_equal(*expected, actual)); +//} /* ************************************************************************* */ TEST ( GaussianFactor, constraint_eliminate1 ) diff --git a/linear/tests/timeFactorOverhead.cpp b/linear/tests/timeFactorOverhead.cpp index 0e2237c6c..b0440b08b 100644 --- a/linear/tests/timeFactorOverhead.cpp +++ b/linear/tests/timeFactorOverhead.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -27,6 +28,8 @@ using namespace gtsam; using namespace std; +typedef EliminationTree GaussianEliminationTree; + static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); int main(int argc, char *argv[]) { @@ -80,7 +83,7 @@ int main(int argc, char *argv[]) { timer.restart(); for(size_t trial=0; trialeliminate()); VectorValues soln(optimize(*gbn)); } blocksolve = timer.elapsed(); @@ -124,7 +127,7 @@ int main(int argc, char *argv[]) { cout.flush(); timer.restart(); for(size_t trial=0; trialeliminate()); VectorValues soln(optimize(*gbn)); } combsolve = timer.elapsed(); diff --git a/linear/tests/timeSLAMlike.cpp b/linear/tests/timeSLAMlike.cpp index bc4a0ff5c..4fd31b3f2 100644 --- a/linear/tests/timeSLAMlike.cpp +++ b/linear/tests/timeSLAMlike.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,8 @@ using namespace gtsam; using namespace std; using namespace boost::lambda; +typedef EliminationTree GaussianEliminationTree; + static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); bool _pair_compare(const pair& a1, const pair& a2) { return a1.first < a2.first; } @@ -116,7 +119,7 @@ int main(int argc, char *argv[]) { timer.restart(); for(size_t trial=0; trialeliminate()); VectorValues soln(optimize(*gbn)); } blocksolve = timer.elapsed(); diff --git a/myconfigure b/myconfigure index 9612c7752..5e1ec71a0 100755 --- a/myconfigure +++ b/myconfigure @@ -1,4 +1,4 @@ -../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CPPFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install +../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" CPPFLAGS="-D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install #../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CXXFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-fno-inline -g -DNDEBUG -O3" CFLAGS="-fno-inline -g -DNDEBUG -O3" LDFLAGS="-fno-inline -g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-DNDEBUG -O3" CFLAGS="-g -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack diff --git a/myconfigure.timing b/myconfigure.timing index 68c701104..6ae568444 100755 --- a/myconfigure.timing +++ b/myconfigure.timing @@ -1,2 +1,2 @@ -../configure --prefix=$HOME/borg --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack +../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack #../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CXXFLAGS="-DNDEBUG -g" CFLAGS="-DNDEBUG -g" LDFLAGS="-DNDEBUG -g" --disable-static --enable-blas --enable-lapack diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp index d68407f19..aee70af30 100644 --- a/tests/testGaussianBayesNet.cpp +++ b/tests/testGaussianBayesNet.cpp @@ -34,7 +34,7 @@ using namespace boost::assign; #include #include -#include +#include #include using namespace std; @@ -114,8 +114,7 @@ TEST( GaussianBayesNet, optimize2 ) fg.add(_x_, -eye(1), _z_, eye(1), 2*ones(1), noise); - GaussianBayesNet cbn = *Inference::Eliminate(fg); - VectorValues actual = optimize(cbn); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); VectorValues expected(vector(3,1)); expected[_x_] = Vector_(1,1.); diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index efb5fbcc0..5f3c66e09 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -36,8 +36,7 @@ using namespace boost::assign; #include #include //#include -#include -#include +#include using namespace gtsam; using namespace example; @@ -195,55 +194,52 @@ TEST( GaussianFactorGraph, error ) // CHECK(assert_equal(expected,*actual)); //} -/* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_x1 ) -{ - Ordering ordering; ordering += "x1","l1","x2"; - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianVariableIndex<> varindex(fg); - GaussianConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, 0); - - // create expected Conditional Gaussian - Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); - GaussianConditional expected(ordering["x1"],15*d,R11,ordering["l1"],S12,ordering["x2"],S13,sigma); - - CHECK(assert_equal(expected,*actual,tol)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_x2 ) -{ - Ordering ordering; ordering += "x2","l1","x1"; - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianVariableIndex<> varindex(fg); - GaussianConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, 0); - - // create expected Conditional Gaussian - double sig = 0.0894427; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); - GaussianConditional expected(ordering["x2"],d,R11,ordering["l1"],S12,ordering["x1"],S13,sigma); - - CHECK(assert_equal(expected,*actual,tol)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_l1 ) -{ - Ordering ordering; ordering += "l1","x1","x2"; - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianVariableIndex<> varindex(fg); - GaussianConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, 0); - - // create expected Conditional Gaussian - double sig = sqrt(2)/10.; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); - GaussianConditional expected(ordering["l1"],d,R11,ordering["x1"],S12,ordering["x2"],S13,sigma); - - CHECK(assert_equal(expected,*actual,tol)); -} +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, eliminateOne_x1 ) +//{ +// Ordering ordering; ordering += "x1","l1","x2"; +// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); +// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); +// +// // create expected Conditional Gaussian +// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; +// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); +// GaussianConditional expected(ordering["x1"],15*d,R11,ordering["l1"],S12,ordering["x2"],S13,sigma); +// +// CHECK(assert_equal(expected,*actual,tol)); +//} +// +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, eliminateOne_x2 ) +//{ +// Ordering ordering; ordering += "x2","l1","x1"; +// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); +// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); +// +// // create expected Conditional Gaussian +// double sig = 0.0894427; +// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; +// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); +// GaussianConditional expected(ordering["x2"],d,R11,ordering["l1"],S12,ordering["x1"],S13,sigma); +// +// CHECK(assert_equal(expected,*actual,tol)); +//} +// +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, eliminateOne_l1 ) +//{ +// Ordering ordering; ordering += "l1","x1","x2"; +// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); +// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); +// +// // create expected Conditional Gaussian +// double sig = sqrt(2)/10.; +// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; +// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); +// GaussianConditional expected(ordering["l1"],d,R11,ordering["x1"],S12,ordering["x2"],S13,sigma); +// +// CHECK(assert_equal(expected,*actual,tol)); +//} ///* ************************************************************************* */ //TEST( GaussianFactorGraph, eliminateOne_x1_fast ) @@ -311,10 +307,10 @@ TEST( GaussianFactorGraph, eliminateAll ) // Check one ordering GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering); - GaussianBayesNet actual = *Inference::Eliminate(fg1); + GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate(); CHECK(assert_equal(expected,actual,tol)); - GaussianBayesNet actualET = *EliminationTree::Create(fg1)->eliminate(); + GaussianBayesNet actualET = *GaussianSequentialSolver(fg1).eliminate(); CHECK(assert_equal(expected,actualET,tol)); } @@ -370,7 +366,7 @@ TEST( GaussianFactorGraph, copying ) GaussianFactorGraph copy = actual; // now eliminate the copy - GaussianBayesNet actual1 = *Inference::Eliminate(copy); + GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate(); // Create the same graph, but not by copying GaussianFactorGraph expected = createGaussianFactorGraph(ordering); @@ -446,11 +442,11 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) GaussianFactorGraph fg = createGaussianFactorGraph(ord); // render with a given ordering - GaussianBayesNet CBN = *Inference::Eliminate(fg); + GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate(); // True GaussianFactorGraph GaussianFactorGraph fg2(CBN); - GaussianBayesNet CBN2 = *Inference::Eliminate(fg2); + GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate(); CHECK(assert_equal(CBN,CBN2)); // // Base FactorGraph only @@ -490,14 +486,12 @@ TEST( GaussianFactorGraph, optimize ) GaussianFactorGraph fg = createGaussianFactorGraph(ord); // optimize the graph - VectorValues actual = optimize(*Inference::Eliminate(fg)); - VectorValues actualET = optimize(*EliminationTree::Create(fg)->eliminate()); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); // verify VectorValues expected = createCorrectDelta(ord); CHECK(assert_equal(expected,actual)); - CHECK(assert_equal(expected,actualET)); } ///* ************************************************************************* */ @@ -759,7 +753,7 @@ TEST( GaussianFactorGraph, elimination ) fg.add(ord["x2"], Ap, b, sigma); // Eliminate - GaussianBayesNet bayesNet = *Inference::Eliminate(fg); + GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); // Check sigma DOUBLES_EQUAL(1.0,bayesNet[ord["x2"]]->get_sigmas()(0),1e-5); @@ -785,13 +779,11 @@ TEST( GaussianFactorGraph, constrained_simple ) GaussianFactorGraph fg = createSimpleConstraintGraph(); // eliminate and solve - VectorValues actual = optimize(*Inference::Eliminate(fg)); - VectorValues actualET = optimize(*EliminationTree::Create(fg)->eliminate()); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); // verify VectorValues expected = createSimpleConstraintValues(); CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(expected, actualET)); } /* ************************************************************************* */ @@ -801,7 +793,7 @@ TEST( GaussianFactorGraph, constrained_single ) GaussianFactorGraph fg = createSingleConstraintGraph(); // eliminate and solve - VectorValues actual = optimize(*Inference::Eliminate(fg)); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); // verify VectorValues expected = createSingleConstraintValues(); @@ -831,7 +823,7 @@ TEST( GaussianFactorGraph, constrained_multi1 ) GaussianFactorGraph fg = createMultiConstraintGraph(); // eliminate and solve - VectorValues actual = optimize(*Inference::Eliminate(fg)); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); // verify VectorValues expected = createMultiConstraintValues(); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 9b61a8646..6e059921b 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -27,6 +27,7 @@ using namespace boost::assign; #include #include #include +#include #include using namespace std; @@ -59,7 +60,7 @@ TEST( ISAM, iSAM_smoother ) } // Create expected Bayes Tree by solving smoother with "natural" ordering - GaussianISAM expected(*Inference::Eliminate(smoother)); + GaussianISAM expected(*GaussianSequentialSolver(smoother).eliminate()); // Check whether BayesTree is correct CHECK(assert_equal(expected, actual)); @@ -113,7 +114,7 @@ TEST( BayesTree, linear_smoother_shortcuts ) boost::tie(smoother, ordering) = createSmoother(7); // eliminate using the "natural" ordering - GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother); + GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); // Create the Bayes tree GaussianISAM bayesTree(chordalBayesNet); @@ -122,12 +123,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 = R->shortcut(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 = C2->shortcut(R); CHECK(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root) @@ -136,7 +137,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 = C3->shortcut(R); CHECK(assert_equal(expected3,actual3,tol)); // Check the conditional P(C4|Root) @@ -145,7 +146,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 = C4->shortcut(R); CHECK(assert_equal(expected4,actual4,tol)); } @@ -176,7 +177,7 @@ TEST( BayesTree, balanced_smoother_marginals ) GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree - GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother); + GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); VectorValues expectedSolution(7, 2); expectedSolution.makeZero(); @@ -191,28 +192,28 @@ TEST( BayesTree, balanced_smoother_marginals ) // Check marginal on x1 GaussianBayesNet expected1 = simpleGaussian(ordering["x1"], zero(2), sigmax1); - GaussianBayesNet actual1 = bayesTree.marginalBayesNet(ordering["x1"]); + GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering["x1"]); CHECK(assert_equal(expected1,actual1,tol)); // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically GaussianBayesNet expected2 = simpleGaussian(ordering["x2"], zero(2), sigx2); - GaussianBayesNet actual2 = bayesTree.marginalBayesNet(ordering["x2"]); + GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering["x2"]); CHECK(assert_equal(expected2,actual2,tol)); // FAILS // Check marginal on x3 GaussianBayesNet expected3 = simpleGaussian(ordering["x3"], zero(2), sigmax3); - GaussianBayesNet actual3 = bayesTree.marginalBayesNet(ordering["x3"]); + GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering["x3"]); CHECK(assert_equal(expected3,actual3,tol)); // Check marginal on x4 GaussianBayesNet expected4 = simpleGaussian(ordering["x4"], zero(2), sigmax4); - GaussianBayesNet actual4 = bayesTree.marginalBayesNet(ordering["x4"]); + GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering["x4"]); CHECK(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) GaussianBayesNet expected7 = simpleGaussian(ordering["x7"], zero(2), sigmax7); - GaussianBayesNet actual7 = bayesTree.marginalBayesNet(ordering["x7"]); + GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering["x7"]); CHECK(assert_equal(expected7,actual7,tol)); } @@ -225,56 +226,56 @@ TEST( BayesTree, balanced_smoother_shortcuts ) GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree - GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother); + GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); GaussianISAM bayesTree(chordalBayesNet); // Check the conditional P(Root|Root) GaussianBayesNet empty; GaussianISAM::sharedClique R = bayesTree.root(); - GaussianBayesNet actual1 = R->shortcut(R); + GaussianBayesNet actual1 = R->shortcut(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 = C2->shortcut(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 = C3->shortcut(R); CHECK(assert_equal(expected3,actual3,tol)); } -/* ************************************************************************* */ -TEST( BayesTree, balanced_smoother_clique_marginals ) -{ - // Create smoother with 7 nodes - Ordering ordering; - ordering += "x1","x3","x5","x7","x2","x6","x4"; - GaussianFactorGraph smoother = createSmoother(7, ordering).first; - - // Create the Bayes tree - GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother); - GaussianISAM bayesTree(chordalBayesNet); - - // Check the clique marginal P(C3) - double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! - GaussianBayesNet expected = simpleGaussian(ordering["x2"],zero(2),sigmax2_alt); - push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2)); - GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]]; - GaussianFactorGraph marginal = C3->marginal(R); - GaussianVariableIndex<> varIndex(marginal); - Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); - Permutation toFrontInverse(*toFront.inverse()); - varIndex.permute(toFront); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { - factor->permuteWithInverse(toFrontInverse); } - GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex); - actual.permuteWithInverse(toFront); - CHECK(assert_equal(expected,actual,tol)); -} +///* ************************************************************************* */ +//TEST( BayesTree, balanced_smoother_clique_marginals ) +//{ +// // Create smoother with 7 nodes +// Ordering ordering; +// ordering += "x1","x3","x5","x7","x2","x6","x4"; +// GaussianFactorGraph smoother = createSmoother(7, ordering).first; +// +// // Create the Bayes tree +// GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); +// GaussianISAM bayesTree(chordalBayesNet); +// +// // Check the clique marginal P(C3) +// double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! +// GaussianBayesNet expected = simpleGaussian(ordering["x2"],zero(2),sigmax2_alt); +// push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2)); +// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]]; +// GaussianFactorGraph marginal = C3->marginal(R); +// GaussianVariableIndex<> varIndex(marginal); +// Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); +// Permutation toFrontInverse(*toFront.inverse()); +// varIndex.permute(toFront); +// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { +// factor->permuteWithInverse(toFrontInverse); } +// GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex); +// actual.permuteWithInverse(toFront); +// CHECK(assert_equal(expected,actual,tol)); +//} /* ************************************************************************* */ // SL-FIX TEST( BayesTree, balanced_smoother_joint ) diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 494b5cd40..f8b618ffd 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -31,6 +31,7 @@ using namespace boost::assign; #include #include +#include #include #include @@ -69,15 +70,15 @@ TEST( GaussianJunctionTree, constructor2 ) list::const_iterator child1it = child0it; ++child1it; GaussianJunctionTree::sharedClique child0 = *child0it; GaussianJunctionTree::sharedClique child1 = *child1it; - CHECK(assert_equal(frontal2, child1->frontal)); - CHECK(assert_equal(sep2, child1->separator)); - LONGS_EQUAL(4, child1->size()); - CHECK(assert_equal(frontal3, child1->children().front()->frontal)); - CHECK(assert_equal(sep3, child1->children().front()->separator)); - LONGS_EQUAL(2, child1->children().front()->size()); - CHECK(assert_equal(frontal4, child0->frontal)); - CHECK(assert_equal(sep4, child0->separator)); - LONGS_EQUAL(2, child0->size()); + CHECK(assert_equal(frontal2, child0->frontal)); + CHECK(assert_equal(sep2, child0->separator)); + LONGS_EQUAL(4, child0->size()); + CHECK(assert_equal(frontal3, child0->children().front()->frontal)); + CHECK(assert_equal(sep3, child0->children().front()->separator)); + LONGS_EQUAL(2, child0->children().front()->size()); + CHECK(assert_equal(frontal4, child1->frontal)); + CHECK(assert_equal(sep4, child1->separator)); + LONGS_EQUAL(2, child1->size()); } /* ************************************************************************* */ @@ -176,7 +177,7 @@ TEST(GaussianJunctionTree, slamlike) { VectorValues deltaactual = gjt.optimize(); planarSLAM::Values actual = init.expmap(deltaactual, ordering); - GaussianBayesNet gbn = *Inference::Eliminate(linearized); + GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); VectorValues delta = optimize(gbn); planarSLAM::Values expected = init.expmap(delta, ordering); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index bcc8ce972..1ba047eec 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -20,7 +20,7 @@ #define GTSAM_MAGIC_KEY #include -#include +#include using namespace std; using namespace gtsam; @@ -40,10 +40,10 @@ TEST(GaussianFactorGraph, createSmoother) LONGS_EQUAL(5,fg2.size()); // eliminate - list x3var; x3var.push_back(ordering["x3"]); - list x1var; x1var.push_back(ordering["x1"]); - GaussianBayesNet p_x3 = *Inference::Eliminate(Inference::Marginal(fg2, x3var)); - GaussianBayesNet p_x1 = *Inference::Eliminate(Inference::Marginal(fg2, x1var)); + vector x3var; x3var.push_back(ordering["x3"]); + vector x1var; x1var.push_back(ordering["x1"]); + GaussianBayesNet p_x3 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x3var)).eliminate(); + GaussianBayesNet p_x1 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x1var)).eliminate(); CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry } @@ -52,8 +52,8 @@ TEST( Inference, marginals ) { // create and marginalize a small Bayes net on "x" GaussianBayesNet cbn = createSmallGaussianBayesNet(); - list xvar; xvar.push_back(0); - GaussianBayesNet actual = *Inference::Eliminate(Inference::Marginal(GaussianFactorGraph(cbn), xvar)); + vector xvar; xvar.push_back(0); + GaussianBayesNet actual = *GaussianSequentialSolver(*GaussianSequentialSolver(GaussianFactorGraph(cbn)).joint(xvar)).eliminate(); // expected is just scalar Gaussian on x GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2)); diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp index f03a4adc8..db023713b 100644 --- a/tests/testSymbolicBayesNet.cpp +++ b/tests/testSymbolicBayesNet.cpp @@ -25,7 +25,7 @@ using namespace boost::assign; #include #include -#include +#include #include using namespace std; @@ -56,7 +56,7 @@ TEST( SymbolicBayesNet, constructor ) SymbolicFactorGraph fg(factorGraph); // eliminate it - SymbolicBayesNet actual = *Inference::Eliminate(fg); + SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index bfee66673..804b0c759 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -26,7 +26,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include using namespace std; @@ -106,23 +106,22 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // CHECK(assert_equal(expected,*actual)); //} -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, eliminateOne ) -{ - Ordering o; o += "x1","l1","x2"; - // create a test graph - GaussianFactorGraph factorGraph = createGaussianFactorGraph(o); - SymbolicFactorGraph fg(factorGraph); - - // eliminate - VariableIndex<> varindex(fg); - IndexConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, o["x1"]); - - // create expected symbolic IndexConditional - IndexConditional expected(o["x1"],o["l1"],o["x2"]); - - CHECK(assert_equal(expected,*actual)); -} +///* ************************************************************************* */ +//TEST( SymbolicFactorGraph, eliminateOne ) +//{ +// Ordering o; o += "x1","l1","x2"; +// // create a test graph +// GaussianFactorGraph factorGraph = createGaussianFactorGraph(o); +// SymbolicFactorGraph fg(factorGraph); +// +// // eliminate +// IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o["x1"]+1); +// +// // create expected symbolic IndexConditional +// IndexConditional expected(o["x1"],o["l1"],o["x2"]); +// +// CHECK(assert_equal(expected,*actual)); +//} /* ************************************************************************* */ TEST( SymbolicFactorGraph, eliminate ) @@ -144,7 +143,7 @@ TEST( SymbolicFactorGraph, eliminate ) SymbolicFactorGraph fg(factorGraph); // eliminate it - SymbolicBayesNet actual = *Inference::Eliminate(fg); + SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); CHECK(assert_equal(expected,actual)); } diff --git a/tests/timeGaussianFactorGraph.cpp b/tests/timeGaussianFactorGraph.cpp index bc2113df2..36a751ee9 100644 --- a/tests/timeGaussianFactorGraph.cpp +++ b/tests/timeGaussianFactorGraph.cpp @@ -22,7 +22,7 @@ #include // for operator += in Ordering #include #include -#include +#include using namespace std; using namespace gtsam; @@ -35,7 +35,7 @@ double timeKalmanSmoother(int T) { pair smoother_ordering = createSmoother(T); GaussianFactorGraph& smoother(smoother_ordering.first); clock_t start = clock(); - optimize(*Inference::Eliminate(smoother)); + GaussianSequentialSolver(smoother).optimize(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; @@ -50,7 +50,7 @@ double timePlanarSmoother(int N, bool old = true) { // Ordering& ordering(pg.get<1>()); // VectorValues& config(pg.get<2>()); clock_t start = clock(); - optimize(*Inference::Eliminate(fg)); + GaussianSequentialSolver(fg).optimize(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; @@ -65,7 +65,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) { // Ordering& ordering(pg.get<1>()); // VectorValues& config(pg.get<2>()); clock_t start = clock(); - Inference::Eliminate(fg); + GaussianSequentialSolver(fg).eliminate(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; diff --git a/tests/timeLinearOnDataset.cpp b/tests/timeLinearOnDataset.cpp index 1be19d4d9..f9e9ebc33 100644 --- a/tests/timeLinearOnDataset.cpp +++ b/tests/timeLinearOnDataset.cpp @@ -35,22 +35,22 @@ int main(int argc, char *argv[]) { pair, shared_ptr > data = load2D(dataset(datasetname)); - tic("Z 1 order"); + tic_("Z 1 order"); Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second)); - toc("Z 1 order"); + toc_("Z 1 order"); - tic("Z 2 linearize"); + tic_("Z 2 linearize"); GaussianFactorGraph::shared_ptr gfg(data.first->linearize(*data.second, *ordering)); - toc("Z 2 linearize"); + toc_("Z 2 linearize"); for(size_t trial = 0; trial < 100; ++trial) { - tic("Z 3 solve"); + tic_("Z 3 solve"); GaussianJunctionTree gjt(*gfg); VectorValues soln(gjt.optimize()); - toc("Z 3 solve"); + toc_("Z 3 solve"); - tictoc_print(); + tictoc_print_(); } return 0;