diff --git a/.cproject b/.cproject index 5d28b6cdb..f47fbdce0 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - install true true @@ -308,7 +307,6 @@ make - check true true @@ -316,7 +314,7 @@ make --k + check true false @@ -324,7 +322,6 @@ make - testSimpleCamera.run true true @@ -340,6 +337,7 @@ make + testVSLAMFactor.run true true @@ -347,7 +345,6 @@ make - testCalibratedCamera.run true true @@ -355,6 +352,7 @@ make + testConditionalGaussian.run true true @@ -362,7 +360,6 @@ make - testPose2.run true true @@ -378,7 +375,6 @@ make - testRot3.run true true @@ -386,6 +382,7 @@ make + testNonlinearOptimizer.run true true @@ -393,7 +390,6 @@ make - testLinearFactor.run true true @@ -401,7 +397,6 @@ make - testConstrainedNonlinearFactorGraph.run true true @@ -409,7 +404,6 @@ make - testLinearFactorGraph.run true true @@ -417,6 +411,7 @@ make + testNonlinearFactorGraph.run true true @@ -424,7 +419,6 @@ make - testPose3.run true true @@ -432,6 +426,7 @@ make + testConstrainedLinearFactorGraph.run true true @@ -439,6 +434,7 @@ make + testVectorConfig.run true true @@ -446,6 +442,7 @@ make + testPoint2.run true true @@ -453,7 +450,6 @@ make - testNonlinearFactor.run true true @@ -461,7 +457,6 @@ make - timeLinearFactor.run true true @@ -469,7 +464,6 @@ make - timeLinearFactorGraph.run true true @@ -477,7 +471,6 @@ make - testGaussianBayesNet.run true true @@ -485,6 +478,7 @@ make + testBayesTree.run true false @@ -492,7 +486,6 @@ make - testSymbolicBayesNet.run true false @@ -500,6 +493,7 @@ make + testSymbolicFactorGraph.run true false @@ -507,7 +501,6 @@ make - testVector.run true true @@ -515,7 +508,6 @@ make - testMatrix.run true true @@ -523,6 +515,7 @@ make + install true true @@ -530,6 +523,7 @@ make + clean true true @@ -537,6 +531,7 @@ make + check true true diff --git a/cpp/BayesNet-inl.h b/cpp/BayesNet-inl.h index 3743d4c96..fa9116ea5 100644 --- a/cpp/BayesNet-inl.h +++ b/cpp/BayesNet-inl.h @@ -23,7 +23,7 @@ namespace gtsam { void BayesNet::print(const string& s) const { cout << s << ":\n"; std::string key; - BOOST_FOREACH(conditional_ptr conditional,conditionals_) + BOOST_FOREACH(sharedConditional conditional,conditionals_) conditional->print("Node[" + conditional->key() + "]"); } @@ -38,26 +38,17 @@ namespace gtsam { template Ordering BayesNet::ordering() const { Ordering ord; - BOOST_FOREACH(conditional_ptr conditional,conditionals_) + BOOST_FOREACH(sharedConditional conditional,conditionals_) ord.push_back(conditional->key()); return ord; } /* ************************************************************************* */ - // predicate to check whether a conditional has the sought key - template - class HasKey { - const string& key_; - public: - HasKey(const std::string& key):key_(key) {} - bool operator()(const boost::shared_ptr& conditional) { - return (conditional->key()==key_); - } - }; template - boost::shared_ptr BayesNet::operator[](const std::string& key) const { - const_iterator it = find_if(conditionals_.begin(),conditionals_.end(),HasKey(key)); + typename BayesNet::sharedConditional + BayesNet::operator[](const std::string& key) const { + const_iterator it = find_if(conditionals_.begin(),conditionals_.end(),onKey(key)); if (it == conditionals_.end()) throw(invalid_argument( "BayesNet::operator['"+key+"']: not found")); return *it; diff --git a/cpp/BayesNet.h b/cpp/BayesNet.h index 9f41c6c90..92f776f50 100644 --- a/cpp/BayesNet.h +++ b/cpp/BayesNet.h @@ -31,8 +31,8 @@ namespace gtsam { public: /** We store shared pointers to Conditional densities */ - typedef typename boost::shared_ptr conditional_ptr; - typedef typename std::list Conditionals; + typedef typename boost::shared_ptr sharedConditional; + typedef typename std::list Conditionals; typedef typename Conditionals::const_iterator const_iterator; typedef typename Conditionals::const_reverse_iterator const_reverse_iterator; @@ -55,12 +55,12 @@ namespace gtsam { bool equals(const BayesNet& other, double tol = 1e-9) const; /** push_back: use reverse topological sort (i.e. parents last / elimination order) */ - inline void push_back(const boost::shared_ptr& conditional) { + inline void push_back(const sharedConditional& conditional) { conditionals_.push_back(conditional); } /** push_front: use topological sort (i.e. parents first / reverse elimination order) */ - inline void push_front(const boost::shared_ptr& conditional) { + inline void push_front(const sharedConditional& conditional) { conditionals_.push_front(conditional); } @@ -73,9 +73,9 @@ namespace gtsam { Ordering ordering() const; /** SLOW O(n) random access to Conditional by key */ - conditional_ptr operator[](const std::string& key) const; + sharedConditional operator[](const std::string& key) const; - inline conditional_ptr back() { return conditionals_.back(); } + inline sharedConditional back() { return conditionals_.back(); } /** return iterators. FD: breaks encapsulation? */ inline const_iterator const begin() const {return conditionals_.begin();} diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 6a507e9de..536de14ab 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -14,7 +14,7 @@ namespace gtsam { /* ************************************************************************* */ template - BayesTree::Clique::Clique(const boost::shared_ptr& conditional) { + BayesTree::Clique::Clique(const sharedConditional& conditional) { separator_ = conditional->parents(); this->push_back(conditional); } @@ -91,8 +91,7 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::insert - (const boost::shared_ptr& conditional) + void BayesTree::insert(const sharedConditional& conditional) { // get key and parents string key = conditional->key(); @@ -129,7 +128,8 @@ namespace gtsam { /* ************************************************************************* */ template template - boost::shared_ptr BayesTree::marginal(const string& key) const { + typename BayesTree::sharedConditional + BayesTree::marginal(const string& key) const { // get clique containing key, and remove all factors below key sharedClique clique = (*this)[key]; @@ -160,9 +160,7 @@ namespace gtsam { ordering.reverse(); // eliminate to get marginal - boost::shared_ptr > bayesNet; - typename boost::shared_ptr > chordalBayesNet = - graph.eliminate(bayesNet,ordering); + sharedBayesNet chordalBayesNet = eliminate(graph,ordering); return chordalBayesNet->back(); // the root is the marginal } diff --git a/cpp/Conditional.h b/cpp/Conditional.h index 03e229c9c..dba0a68d0 100644 --- a/cpp/Conditional.h +++ b/cpp/Conditional.h @@ -49,4 +49,17 @@ namespace gtsam { /** return the number of parents */ virtual std::size_t nrParents() const = 0; }; + + // predicate to check whether a conditional has the sought key + template + class onKey { + const std::string& key_; + public: + onKey(const std::string& key):key_(key) {} + bool operator()(const typename Conditional::shared_ptr& conditional) { + return (conditional->key()==key_); + } + }; + + } diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index 13b2e8e65..544ab5b87 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -83,7 +83,8 @@ GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Order } else { - ConditionalGaussian::shared_ptr cg = eliminateOne(key); + ConditionalGaussian::shared_ptr cg = + eliminateOne(*this,key); cbn->push_back(cg); } } diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 74353239a..f17355125 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -29,7 +29,7 @@ FactorGraph::FactorGraph(const BayesNet& bayesNet) { typename BayesNet::const_iterator it = bayesNet.begin(); for(; it != bayesNet.end(); it++) { - typename boost::shared_ptr::shared_ptr factor(new Factor(*it)); + sharedFactor factor(new Factor(*it)); push_back(factor); } } @@ -56,7 +56,7 @@ bool FactorGraph::equals /** check whether the factors_ are the same */ for (size_t i = 0; i < factors_.size(); i++) { // TODO: Doesn't this force order of factor insertion? - shared_factor f1 = factors_[i], f2 = fg.factors_[i]; + sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; if (f1 == NULL && f2 == NULL) continue; if (f1 == NULL || f2 == NULL) return false; if (!f1->equals(*f2, tol)) return false; @@ -75,7 +75,7 @@ size_t FactorGraph::nrFactors() const { /* ************************************************************************* */ template -void FactorGraph::push_back(shared_factor factor) { +void FactorGraph::push_back(sharedFactor factor) { factors_.push_back(factor); // add the actual factor if (factor==NULL) return; @@ -184,11 +184,11 @@ list FactorGraph::factors(const string& key) const { template vector > FactorGraph::findAndRemoveFactors(const string& key) { - vector > found; + vector found; Indices::iterator it = indices_.find(key); if (it == indices_.end()) - throw(std::invalid_argument + throw(invalid_argument ("FactorGraph::findAndRemoveFactors invalid key: " + key)); list *indices_ptr; // pointer to indices list in indices_ map @@ -206,14 +206,13 @@ FactorGraph::findAndRemoveFactors(const string& key) { /* find factors and remove them from the factor graph: O(n) */ /* ************************************************************************* */ template -boost::shared_ptr +typename FactorGraph::sharedFactor FactorGraph::removeAndCombineFactors(const string& key) { bool verbose = false; if (verbose) cout << "FactorGraph::removeAndCombineFactors" << endl; - typedef typename boost::shared_ptr shared_factor; - vector found = findAndRemoveFactors(key); - shared_factor new_factor(new Factor(found)); + vector found = findAndRemoveFactors(key); + sharedFactor new_factor(new Factor(found)); if (verbose) cout << "FactorGraph::removeAndCombineFactors done" << endl; return new_factor; } @@ -221,21 +220,20 @@ FactorGraph::removeAndCombineFactors(const string& key) /* ************************************************************************* */ /* eliminate one node from the factor graph */ /* ************************************************************************* */ -template -template -boost::shared_ptr FactorGraph::eliminateOne(const std::string& key) { +template +boost::shared_ptr eliminateOne(FactorGraph& graph, const string& 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 - shared_factor joint_factor = removeAndCombineFactors(key); + boost::shared_ptr joint_factor = graph.removeAndCombineFactors(key); // eliminate that joint factor - shared_factor 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()) push_back(factor); + if (!factor->empty()) graph.push_back(factor); // return the conditional Gaussian return conditional; @@ -245,16 +243,16 @@ boost::shared_ptr FactorGraph::eliminateOne(const std::stri // This doubly templated function is generic. There is a LinearFactorGraph // version that returns a more specific GaussianBayesNet. // Note, you will need to include this file to instantiate the function. +// TODO: get rid of summy argument /* ************************************************************************* */ -template -template +template boost::shared_ptr > -FactorGraph::eliminate(boost::shared_ptr > xxx, const Ordering& ordering) +eliminate(FactorGraph& factorGraph, const Ordering& ordering) { boost::shared_ptr > bayesNet (new BayesNet()); // empty BOOST_FOREACH(string key, ordering) { - boost::shared_ptr cg = eliminateOne(key); + boost::shared_ptr cg = eliminateOne(factorGraph,key); bayesNet->push_back(cg); } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 4baff5a58..5b00a3bdb 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -30,13 +30,13 @@ namespace gtsam { */ template class FactorGraph: public Testable > { public: - typedef typename boost::shared_ptr shared_factor; - typedef typename std::vector::iterator iterator; - typedef typename std::vector::const_iterator const_iterator; + typedef typename boost::shared_ptr sharedFactor; + typedef typename std::vector::iterator iterator; + typedef typename std::vector::const_iterator const_iterator; protected: /** Collection of factors */ - std::vector factors_; + std::vector factors_; /** For each variable a list of factor indices connected to it */ typedef std::map > Indices; @@ -73,7 +73,7 @@ namespace gtsam { } /** Get a specific factor by index */ - inline shared_factor operator[](size_t i) const { + inline sharedFactor operator[](size_t i) const { return factors_[i]; } @@ -84,7 +84,7 @@ namespace gtsam { size_t nrFactors() const; /** Add a factor */ - void push_back(shared_factor factor); + void push_back(sharedFactor factor); /** * Compute colamd ordering @@ -102,29 +102,14 @@ namespace gtsam { * from the factor graph * @param key the key for the given node */ - std::vector findAndRemoveFactors(const std::string& key); + std::vector findAndRemoveFactors(const std::string& key); /** * extract and combine all the factors that involve a given node * @param key the key for the given node * @return the combined linear factor */ - shared_factor removeAndCombineFactors(const std::string& key); - - /** - * Eliminate a single node yielding a Conditional - * Eliminates the factors from the factor graph through findAndRemoveFactors - * and adds a new factor on the separator to the factor graph - */ - template - boost::shared_ptr eliminateOne(const std::string& key); - - /** - * eliminate factor graph using the given (not necessarily complete) - * ordering, yielding a chordal Bayes net and (partially eliminated) FG - */ - template - boost::shared_ptr > eliminate(boost::shared_ptr > bayesNet, const Ordering& ordering); + sharedFactor removeAndCombineFactors(const std::string& key); private: @@ -136,5 +121,24 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(indices_); } }; // FactorGraph + + /** doubly templated functions */ + + /** + * Eliminate a single node yielding a Conditional + * Eliminates the factors from the factor graph through findAndRemoveFactors + * and adds a new factor on the separator to the factor graph + */ + template + boost::shared_ptr eliminateOne(FactorGraph& factorGraph, const std::string& key); + + /** + * eliminate factor graph using the given (not necessarily complete) + * ordering, yielding a chordal Bayes net and (partially eliminated) FG + */ + template + boost::shared_ptr > + eliminate(FactorGraph& factorGraph, const Ordering& ordering); + } // namespace gtsam diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index ace8f4cda..476678140 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -34,7 +34,7 @@ LinearFactorGraph::LinearFactorGraph(const GaussianBayesNet& CBN) : set LinearFactorGraph::find_separator(const string& key) const { set separator; - BOOST_FOREACH(shared_factor factor,factors_) + BOOST_FOREACH(sharedFactor factor,factors_) factor->tally_separator(key,separator); return separator; @@ -46,7 +46,8 @@ LinearFactorGraph::eliminate(const Ordering& ordering) { GaussianBayesNet::shared_ptr chordalBayesNet (new GaussianBayesNet()); // empty BOOST_FOREACH(string key, ordering) { - ConditionalGaussian::shared_ptr cg = eliminateOne(key); + ConditionalGaussian::shared_ptr cg = + eliminateOne(*this, key); chordalBayesNet->push_back(cg); } return chordalBayesNet; @@ -90,7 +91,7 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1, /* ************************************************************************* */ Dimensions LinearFactorGraph::dimensions() const { Dimensions result; - BOOST_FOREACH(shared_factor factor,factors_) { + BOOST_FOREACH(sharedFactor factor,factors_) { Dimensions vs = factor->dimensions(); string key; int dim; FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim)); @@ -112,7 +113,7 @@ LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const { FOREACH_PAIR(key,dim,vs) { Matrix A = eye(dim); Vector b = zero(dim); - shared_factor prior(new LinearFactor(key,A,b, sigma)); + sharedFactor prior(new LinearFactor(key,A,b, sigma)); result.push_back(prior); } return result; @@ -123,7 +124,7 @@ pair LinearFactorGraph::matrix(const Ordering& ordering) const { // get all factors LinearFactorSet found; - BOOST_FOREACH(shared_factor factor,factors_) + BOOST_FOREACH(sharedFactor factor,factors_) found.push_back(factor); // combine them @@ -145,7 +146,7 @@ Matrix LinearFactorGraph::sparse(const Ordering& ordering) const { // Collect the I,J,S lists for all factors int row_index = 0; - BOOST_FOREACH(shared_factor factor,factors_) { + BOOST_FOREACH(sharedFactor factor,factors_) { // get sparse lists for the factor list i1,j1; diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index d36588b3d..6c8306d63 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -25,7 +25,8 @@ namespace gtsam { SymbolicBayesNet::shared_ptr bayesNet (new SymbolicBayesNet()); BOOST_FOREACH(string key, ordering) { - SymbolicConditional::shared_ptr conditional = eliminateOne(key); + SymbolicConditional::shared_ptr conditional = + eliminateOne(*this,key); bayesNet->push_back(conditional); } diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp index 2fdca4070..70ca52df8 100644 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -256,7 +256,8 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint ) CHECK(fg.nrFactors() == 0); // eliminate the linear factor - ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne("z"); + ConditionalGaussian::shared_ptr cg3 = + eliminateOne(fg,"z"); CHECK(cg3->nrParents() == 0); CHECK(fg.size() == 0); diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index ee14ad79a..90bec931a 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -191,7 +191,7 @@ TEST( LinearFactorGraph, eliminateOne_x1 ) { LinearFactorGraph fg = createLinearFactorGraph(); ConditionalGaussian::shared_ptr actual = - fg.eliminateOne("x1"); + eliminateOne(fg,"x1"); // create expected Conditional Gaussian Matrix R11 = Matrix_(2,2, @@ -220,7 +220,7 @@ TEST( LinearFactorGraph, eliminateOne_x2 ) { LinearFactorGraph fg = createLinearFactorGraph(); ConditionalGaussian::shared_ptr actual = - fg.eliminateOne("x2"); + eliminateOne(fg,"x2"); // create expected Conditional Gaussian Matrix R11 = Matrix_(2,2, @@ -248,7 +248,7 @@ TEST( LinearFactorGraph, eliminateOne_l1 ) { LinearFactorGraph fg = createLinearFactorGraph(); ConditionalGaussian::shared_ptr actual = - fg.eliminateOne("l1"); + eliminateOne(fg,"l1"); // create expected Conditional Gaussian Matrix R11 = Matrix_(2,2, @@ -383,8 +383,8 @@ TEST( LinearFactorGraph, matrix ) ); Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5); - EQUALITY(A,A1); // currently fails - CHECK(b==b1); // currently fails + EQUALITY(A,A1); + CHECK(b==b1); } /* ************************************************************************* */ @@ -400,9 +400,10 @@ TEST( LinearFactorGraph, sparse ) Matrix ijs = fg.sparse(ord); EQUALITY(ijs, Matrix_(3, 14, - +1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8.,7.,8., - +5., 6., 1., 2., 5., 6., 3.,4., 5., 6., 1., 2.,3.,4., - 10.,10., 10.,10.,-10.,-10., 5.,5.,-5.,-5., -5.,-5.,5.,5.)); + // f(x1) f(x2,x1) f(l1,x1) f(x2,l1) + +1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8.,7.,8., + +5., 6., 1., 2., 5., 6., 3.,4., 5., 6., 1., 2.,3.,4., + 10.,10., 10.,10.,-10.,-10., 5.,5.,-5.,-5., -5.,-5.,5.,5.)); } /* ************************************************************************* */ @@ -422,9 +423,8 @@ TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet ) // Base FactorGraph only FactorGraph fg3(*CBN); - boost::shared_ptr > dummy; boost::shared_ptr > CBN3 = - fg3.eliminate(dummy,ord); + eliminate(fg3,ord); CHECK(CBN->equals(*CBN3)); } diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index 060273f88..4ce1258d4 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -112,7 +112,7 @@ TEST( LinearFactorGraph, eliminateOne ) // eliminate SymbolicConditional::shared_ptr actual = - fg.eliminateOne("x1"); + eliminateOne(fg,"x1"); // create expected symbolic Conditional SymbolicConditional expected("x1","l1","x2");