diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 5d2a64627..22dd0c62b 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -130,10 +130,10 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::Cliques::print(const std::string& s) const { + void BayesTree::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const { std::cout << s << ":\n"; BOOST_FOREACH(sharedClique clique, *this) - clique->printTree(); + clique->printTree("", indexFormatter); } /* ************************************************************************* */ @@ -324,14 +324,14 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::print(const std::string& s) const { + void BayesTree::print(const std::string& s, const IndexFormatter& indexFormatter) const { if (root_.use_count() == 0) { printf("WARNING: BayesTree.print encountered a forest...\n"); return; } std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl; if (nodes_.empty()) return; - root_->printTree(""); + root_->printTree("", indexFormatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index b5168567c..9c38d6496 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -70,7 +70,8 @@ namespace gtsam { // A convenience class for a list of shared cliques struct Cliques : public std::list { - void print(const std::string& s = "Cliques") const; + void print(const std::string& s = "Cliques", + const IndexFormatter& indexFormatter = &(boost::lexical_cast)) const; bool equals(const Cliques& other, double tol = 1e-9) const; }; @@ -175,7 +176,8 @@ namespace gtsam { bool equals(const BayesTree& other, double tol = 1e-9) const; /** print */ - void print(const std::string& s = "") const; + void print(const std::string& s = "", + const IndexFormatter& indexFormatter = &(boost::lexical_cast) ) const; /// @} /// @name Standard Interface diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 7e0ba6a7b..f96566536 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -50,8 +50,8 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTreeCliqueBase::print(const std::string& s) const { - conditional_->print(s); + void BayesTreeCliqueBase::print(const std::string& s, const IndexFormatter& indexFormatter) const { + conditional_->print(s, indexFormatter); } /* ************************************************************************* */ @@ -65,10 +65,10 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTreeCliqueBase::printTree(const std::string& indent) const { - asDerived(this)->print(indent); + void BayesTreeCliqueBase::printTree(const std::string& indent, const IndexFormatter& indexFormatter) const { + asDerived(this)->print(indent, indexFormatter); BOOST_FOREACH(const derived_ptr& child, children_) - child->printTree(indent+" "); + child->printTree(indent+" ", indexFormatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index e1a7395da..9a2f6c54e 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -93,10 +93,10 @@ namespace gtsam { } /** print this node */ - void print(const std::string& s = "") const; + void print(const std::string& s = "", const IndexFormatter& indexFormatter = &(boost::lexical_cast) ) const; /** print this node and entire subtree below it */ - void printTree(const std::string& indent="") const; + void printTree(const std::string& indent="", const IndexFormatter& indexFormatter = &(boost::lexical_cast) ) const; /// @} /// @name Standard Interface diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 0b5fca660..97f885596 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -23,6 +23,7 @@ #include // for noncopyable #include #include +#include #include #include @@ -98,13 +99,16 @@ public: Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); } /** Single parent */ - Conditional(KeyType key, KeyType parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); } + Conditional(KeyType key, KeyType parent) + : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); } /** Two parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); } + Conditional(KeyType key, KeyType parent1, KeyType parent2) + : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); } /** Three parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); } + Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) + : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); } /// @} /// @name Advanced Constructors @@ -126,8 +130,9 @@ public: /// @name Testable /// @{ - /** print */ - void print(const std::string& s = "Conditional") const; + /** print with optional formatter */ + void print(const std::string& s = "Conditional", + const boost::function& formatter = &(boost::lexical_cast) ) const; /** check equality */ template @@ -196,12 +201,12 @@ private: /* ************************************************************************* */ template -void Conditional::print(const std::string& s) const { +void Conditional::print(const std::string& s, const boost::function& formatter) const { std::cout << s << " P("; - BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << key; - if (nrParents()>0) std::cout << " |"; - BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << parent; - std::cout << ")" << std::endl; + BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << formatter(key); + if (nrParents()>0) std::cout << " |"; + BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << formatter(parent); + std::cout << ")" << std::endl; } } // gtsam diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index c048c8dd8..40d40a3a7 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -129,16 +129,16 @@ GaussianConditional& GaussianConditional::operator=(const GaussianConditional& r } /* ************************************************************************* */ -void GaussianConditional::print(const string &s) const +void GaussianConditional::print(const string &s, const IndexFormatter& formatter) const { cout << s << ": density on "; for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { - cout << (boost::format("[%1%]")%(*it)).str() << " "; + cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; } cout << endl; gtsam::print(Matrix(get_R()),"R"); for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { - gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(*it)).str()); + gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(formatter(*it))).str()); } gtsam::print(Vector(get_d()),"d"); gtsam::print(sigmas_,"sigmas"); diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 0dc8d771a..68b537c2d 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -138,7 +138,8 @@ public: GaussianConditional& operator=(const GaussianConditional& rhs); /** print */ - void print(const std::string& = "GaussianConditional") const; + void print(const std::string& = "GaussianConditional", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /** equals function */ bool equals(const GaussianConditional &cg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index b1fd9338e..64e4e970c 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -23,6 +23,8 @@ #include #include +#include + #include #include @@ -81,7 +83,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; // Implementing Testable interface - virtual void print(const std::string& s = "") const = 0; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = &(boost::lexical_cast)) const = 0; + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0; virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */ diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 8628c95ef..86938a9bf 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -292,11 +292,11 @@ HessianFactor& HessianFactor::operator=(const HessianFactor& rhs) { } /* ************************************************************************* */ -void HessianFactor::print(const std::string& s) const { +void HessianFactor::print(const std::string& s, const IndexFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; for(const_iterator key=this->begin(); key!=this->end(); ++key) - cout << *key << "(" << this->getDim(key) << ") "; + cout << formatter(*key) << "(" << this->getDim(key) << ") "; cout << "\n"; gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 46955d45a..92f5c06d0 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -218,7 +218,8 @@ namespace gtsam { } /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /** Compare to another factor for testing (implementing Testable) */ virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index f2cea028e..038f8c718 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -244,15 +244,15 @@ namespace gtsam { } /* ************************************************************************* */ - void JacobianFactor::print(const string& s) const { + void JacobianFactor::print(const string& s, const IndexFormatter& formatter) const { cout << s << "\n"; if (empty()) { cout << " empty, keys: "; - BOOST_FOREACH(const Index& key, keys()) { cout << key << " "; } + BOOST_FOREACH(const Index& key, keys()) { cout << formatter(key) << " "; } cout << endl; } else { for(const_iterator key=begin(); key!=end(); ++key) - cout << boost::format("A[%1%]=\n")%*key << getA(key) << endl; + cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl; cout << "b=" << getb() << endl; model_->print("model"); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 7fb4211fb..14d073576 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -151,7 +151,8 @@ namespace gtsam { } // Implementing Testable interface - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 984284ebd..00c8a026c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -254,7 +254,8 @@ public: /** Construct from an elimination result */ ISAM2Clique(const std::pair >& result) : - Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { + Base(result.first), cachedFactor_(result.second), + gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { // Compute gradient contribution const ConditionalType& conditional(*result.first); // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons @@ -278,14 +279,18 @@ public: const Vector& gradientContribution() const { return gradientContribution_; } bool equals(const This& other, double tol=1e-9) const { - return Base::equals(other) && ((!cachedFactor_ && !other.cachedFactor_) || (cachedFactor_ && other.cachedFactor_ && cachedFactor_->equals(*other.cachedFactor_, tol))); + return Base::equals(other) && + ((!cachedFactor_ && !other.cachedFactor_) + || (cachedFactor_ && other.cachedFactor_ + && cachedFactor_->equals(*other.cachedFactor_, tol))); } /** print this node */ - void print(const std::string& s = "") const { - Base::print(s); + void print(const std::string& s = "", + const IndexFormatter& formatter = &(boost::lexical_cast)) const { + Base::print(s,formatter); if(cachedFactor_) - cachedFactor_->print(s + "Cached: "); + cachedFactor_->print(s + "Cached: ", formatter); else std::cout << s << "Cached empty" << std::endl; if(gradientContribution_.rows() != 0) diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 467f7bfe3..6b5ea31bc 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -30,19 +30,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -// Create an index formatter that looks up the Key in an inverse ordering, then -// formats the key using the provided key formatter, used in saveGraph. -struct OrderingIndexFormatter { - Ordering::InvertedMap inverseOrdering; - const KeyFormatter& keyFormatter; - OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : - inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {} - string operator()(Index index) { - return keyFormatter(inverseOrdering.at(index)); - } -}; - /* ************************************************************************* */ void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const { isam_.saveGraph(s, OrderingIndexFormatter(ordering_, keyFormatter)); diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 1f2b8646e..098976a36 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -258,5 +258,17 @@ public: bool equals(const Unordered &t, double tol=0) const; }; -} +// Create an index formatter that looks up the Key in an inverse ordering, then +// formats the key using the provided key formatter, used in saveGraph. +struct OrderingIndexFormatter { + Ordering::InvertedMap inverseOrdering; + const KeyFormatter& keyFormatter; + OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : + inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {} + std::string operator()(Index index) { + return keyFormatter(inverseOrdering.at(index)); + } +}; + +} // \namespace gtsam