diff --git a/.cproject b/.cproject index a071e0c00..7c1c35ba1 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + check true true @@ -307,7 +308,6 @@ make - testSimpleCamera.run true true @@ -315,7 +315,6 @@ make - testCal3_S2.run true true @@ -323,6 +322,7 @@ make + testVSLAMFactor.run true true @@ -330,7 +330,6 @@ make - testCalibratedCamera.run true true @@ -338,6 +337,7 @@ make + testConditionalGaussian.run true true @@ -345,14 +345,22 @@ make - testPose2.run true true true + +make + +testFGConfig.run +true +true +true + make + install true true @@ -360,6 +368,7 @@ make + clean true true @@ -367,6 +376,7 @@ make + check true true diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index 512a5cb43..25e36f413 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -91,14 +91,14 @@ DeltaFunction::shared_ptr ConstrainedLinearFactorGraph::eliminate_one_eq(const s // remove all unary linear factors on this node vector newfactors; - BOOST_FOREACH(LinearFactor::shared_ptr f, factors) + BOOST_FOREACH(LinearFactor::shared_ptr f, factors_) { if (f->size() != 1 || !f->involves(key)) { newfactors.push_back(f); } } - factors = newfactors; + factors_ = newfactors; // combine the linear factors connected to equality node boost::shared_ptr joint_factor = combine_factors(key); @@ -146,7 +146,7 @@ FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering){ void ConstrainedLinearFactorGraph::print(const std::string& s) const { cout << "ConstrainedFactorGraph: " << s << endl; - BOOST_FOREACH(LinearFactor::shared_ptr f, factors) + BOOST_FOREACH(LinearFactor::shared_ptr f, factors_) { f->print(); } @@ -208,7 +208,7 @@ Ordering ConstrainedLinearFactorGraph::getOrdering() const LinearFactorGraph ConstrainedLinearFactorGraph::convert() const { LinearFactorGraph ret; - BOOST_FOREACH(LinearFactor::shared_ptr f, factors) + BOOST_FOREACH(LinearFactor::shared_ptr f, factors_) { ret.push_back(f); } diff --git a/cpp/ConstrainedLinearFactorGraph.h b/cpp/ConstrainedLinearFactorGraph.h index 59943b410..518a89ffd 100644 --- a/cpp/ConstrainedLinearFactorGraph.h +++ b/cpp/ConstrainedLinearFactorGraph.h @@ -56,15 +56,14 @@ public: return eq_factors.end(); } - /** clear the factor graph - reimplemented to include equality factors */ + /** clear the factor graph - re-implemented to include equality factors */ void clear(){ - factors.clear(); - node_to_factors_.clear(); + factors_.clear(); eq_factors.clear(); } - /** size - reimplemented to include the equality factors */ - inline size_t size() const { return factors.size() + eq_factors.size(); } + /** size - reimplemented to include the equality factors_ */ + inline size_t size() const { return factors_.size() + eq_factors.size(); } /** Check equality - checks equality constraints as well*/ bool equals(const LinearFactorGraph& fg, double tol=1e-9) const; diff --git a/cpp/ConstrainedNonlinearFactorGraph.cpp b/cpp/ConstrainedNonlinearFactorGraph.cpp index 49e30d98e..dc00abce3 100644 --- a/cpp/ConstrainedNonlinearFactorGraph.cpp +++ b/cpp/ConstrainedNonlinearFactorGraph.cpp @@ -28,7 +28,7 @@ ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FG ConstrainedLinearFactorGraph ret; // linearize all nonlinear factors - for(const_iterator factor=factors.begin(); factorlinearize(config); ret.push_back(lf); } @@ -45,10 +45,9 @@ ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FG NonlinearFactorGraph ConstrainedNonlinearFactorGraph::convert() const { NonlinearFactorGraph ret; - BOOST_FOREACH(boost::shared_ptr f, factors) - { + BOOST_FOREACH(boost::shared_ptr f, factors_) ret.push_back(f); - } + return ret; } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 13016db9f..fb900ad26 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -9,104 +9,106 @@ #pragma once -#include -#include -#include #include +#include +#include + #include "Factor.h" #include "FGConfig.h" namespace gtsam { - - /** - * A factor graph is a bipartite graph with factor nodes connected to variable nodes. - * In this class, however, only factor nodes are kept around. - */ - template class FactorGraph - { - public: - typedef typename boost::shared_ptr shared_factor; - typedef typename std::vector::iterator iterator; - typedef typename std::vector::const_iterator const_iterator; - protected: - /** Collection of factors */ - std::vector factors; - std::map > node_to_factors_; + /** + * A factor graph is a bipartite graph with factor nodes connected to variable nodes. + * In this class, however, only factor nodes are kept around. + */ + template class FactorGraph { + public: + typedef typename boost::shared_ptr shared_factor; + typedef typename std::vector::iterator iterator; + typedef typename std::vector::const_iterator const_iterator; - public: + protected: + /** Collection of factors */ + std::vector factors_; - /** get the factors to a specific node */ - const std::list& get_factors_to_node(const std::string& key) const { - return node_to_factors_[key]; - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(factors_); + } - /** STL like, return the iterator pointing to the first factor */ - const_iterator begin() const { - return factors.begin(); - } + public: - /** STL like, return the iterator pointing to the last factor */ - const_iterator end() const { - return factors.end(); - } + /** STL like, return the iterator pointing to the first factor */ + const_iterator begin() const { + return factors_.begin(); + } - /** clear the factor graph */ - void clear(){ - factors.clear(); - node_to_factors_.clear(); - } + /** STL like, return the iterator pointing to the last factor */ + const_iterator end() const { + return factors_.end(); + } - /** Get a specific factor by index */ - shared_factor operator[](size_t i) const { - return factors[i]; - } + /** clear the factor graph */ + void clear() { + factors_.clear(); + } - /** return the numbers of the factors in the factor graph */ - inline size_t size() const { return factors.size(); } + /** Get a specific factor by index */ + shared_factor operator[](size_t i) const { + return factors_[i]; + } - /** Add a factor */ - void push_back(shared_factor ptr_f) {factors.push_back(ptr_f);} - - /** unnormalized error */ - double error(const FGConfig& c) const { - double total_error = 0.; - /** iterate over all the factors to accumulate the log probabilities */ - for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++) - total_error += (*factor)->error(c); + /** return the numbers of the factors_ in the factor graph */ + inline size_t size() const { + return factors_.size(); + } - return total_error; - } + /** Add a factor */ + void push_back(shared_factor ptr_f) { + factors_.push_back(ptr_f); + } - /** Unnormalized probability. O(n) */ - double probPrime(const FGConfig& c) const { - return exp(-0.5*error(c)); - } + /** unnormalized error */ + double error(const FGConfig& c) const { + double total_error = 0.; + /** iterate over all the factors_ to accumulate the log probabilities */ + for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) + total_error += (*factor)->error(c); - /** print out graph */ - void print(const std::string& s = "FactorGraph") const{ - std::cout << s << std::endl; - printf("size: %d\n", (int)size()); - for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++) - (*factor)->print(); - } + return total_error; + } - /** Check equality */ - bool equals(const FactorGraph& fg, double tol=1e-9) const - { - /** check whether the two factor graphs have the same number of factors */ - if( factors.size() != fg.size() ) goto fail; - - /** check whether the factors are the same */ - for(size_t i=0;iequals(*fg.factors[i], tol) ) goto fail; //TODO: Doesn't this force order of factor insertion? - return true; + /** Unnormalized probability. O(n) */ + double probPrime(const FGConfig& c) const { + return exp(-0.5 * error(c)); + } - fail: - print(); - fg.print(); - return false; - } - }; + /** print out graph */ + void print(const std::string& s = "FactorGraph") const { + std::cout << s << std::endl; + printf("size: %d\n", (int) size()); + for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) + (*factor)->print(); + } + + /** Check equality */ + bool equals(const FactorGraph& fg, double tol = 1e-9) const { + /** check whether the two factor graphs have the same number of factors_ */ + if (factors_.size() != fg.size()) goto fail; + + /** 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? + if (!factors_[i]->equals(*fg.factors_[i], tol)) goto fail; + return true; + + fail: print(); + fg.print(); + return false; + } + }; } // namespace gtsam diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index c4cc3d75d..68b948ef5 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -40,8 +40,8 @@ void LinearFactorGraph::setCBN(const ChordalBayesNet& CBN) set LinearFactorGraph::find_separator(const string& key) const { set separator; - BOOST_FOREACH(shared_factor factor,factors) - factor->tally_separator(key,separator); + BOOST_FOREACH(shared_factor factor,factors_) + factor->tally_separator(key,separator); return separator; } @@ -54,10 +54,10 @@ LinearFactorGraph::find_factors_and_remove(const string& key) { LinearFactorSet found; - for(iterator factor=factors.begin(); factor!=factors.end(); ) + for(iterator factor=factors_.begin(); factor!=factors_.end(); ) if ((*factor)->involves(key)) { found.insert(*factor); - factor = factors.erase(factor); + factor = factors_.erase(factor); } else { factor++; // important, erase will have effect of ++ } @@ -130,7 +130,7 @@ LinearFactorGraph::eliminate(const Ordering& ordering) // after eliminate, only one zero indegree factor should remain // TODO: this check needs to exist - verify that unit tests work when this check is in place /* - if (factors.size() != 1) { + if (factors_.size() != 1) { print(); throw(invalid_argument("LinearFactorGraph::eliminate: graph not empty after eliminate, ordering incomplete?")); } @@ -156,7 +156,7 @@ FGConfig LinearFactorGraph::optimize(const Ordering& ordering) /** combine two factor graphs */ /* ************************************************************************* */ void LinearFactorGraph::combine(const LinearFactorGraph &lfg){ - for(const_iterator factor=lfg.factors.begin(); factor!=lfg.factors.end(); factor++){ + for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){ push_back(*factor); } } @@ -170,9 +170,9 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1, // create new linear factor graph equal to the first one LinearFactorGraph fg = lfg1; - // add the second factors in the graph - for (const_iterator factor = lfg2.factors.begin(); factor - != lfg2.factors.end(); factor++) { + // add the second factors_ in the graph + for (const_iterator factor = lfg2.factors_.begin(); factor + != lfg2.factors_.end(); factor++) { fg.push_back(*factor); } @@ -184,7 +184,7 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1, // find all variables and their dimensions VariableSet LinearFactorGraph::variables() const { VariableSet result; - BOOST_FOREACH(shared_factor factor,factors) { + BOOST_FOREACH(shared_factor factor,factors_) { VariableSet vs = factor->variables(); BOOST_FOREACH(Variable v,vs) result.insert(v); } @@ -217,7 +217,7 @@ pair LinearFactorGraph::matrix(const Ordering& ordering) const { // get all factors LinearFactorSet found; - BOOST_FOREACH(shared_factor factor,factors) + BOOST_FOREACH(shared_factor factor,factors_) found.insert(factor); // combine them diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 7dbe52a23..1b2e77f55 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -15,6 +15,7 @@ #include #include +#include #include "Factor.h" #include "Matrix.h" @@ -37,6 +38,18 @@ namespace gtsam { */ class NonlinearFactor : public Factor { + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { +// ar & boost::serialization::base_object(*this); // TODO: needed ? + ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(sigma_); + ar & BOOST_SERIALIZATION_NVP(keys_); + } + protected: Vector z_; // measurement @@ -45,9 +58,12 @@ namespace gtsam { public: + /** Default constructor, with easily identifiable bogus values */ + NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {} + /** Constructor */ - NonlinearFactor(const Vector& z, // the measurement - const double sigma); // the variance + NonlinearFactor(const Vector& z, // the measurement + const double sigma); // the variance /** Vector of errors */ virtual Vector error_vector(const FGConfig& c) const = 0; diff --git a/cpp/NonlinearFactorGraph.cpp b/cpp/NonlinearFactorGraph.cpp index da065d3f8..2a9ff459e 100644 --- a/cpp/NonlinearFactorGraph.cpp +++ b/cpp/NonlinearFactorGraph.cpp @@ -27,7 +27,7 @@ LinearFactorGraph NonlinearFactorGraph::linearize(const FGConfig& config) const LinearFactorGraph linearFG; // linearize all factors - for(const_iterator factor=factors.begin(); factorlinearize(config); linearFG.push_back(lf); } @@ -70,7 +70,7 @@ bool check_convergence (double relativeErrorTreshold, FGConfig NonlinearFactorGraph::iterate (const FGConfig& config, const Ordering& ordering) const { - LinearFactorGraph linear = linearize(config); // linearize all factors + LinearFactorGraph linear = linearize(config); // linearize all factors_ return linear.optimize(ordering); } diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index 90ead895c..8de58df57 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -10,17 +10,27 @@ #pragma once -#include "LinearFactorGraph.h" +#include +#include #include "FactorGraph.h" #include "NonlinearFactor.h" +#include "LinearFactorGraph.h" #include "ChordalBayesNet.h" -#include namespace gtsam { /** Factor Graph Constsiting of non-linear factors */ class NonlinearFactorGraph : public FactorGraph { +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::base_object< FactorGraph >(*this); + } + public: // internal, exposed for testing only, doc in .cpp file FGConfig iterate(const FGConfig& config, const Ordering& ordering) const;