12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added formatting objects to linear and inference objects to allow for printing meaningful keys

release/4.3a0
Alex Cunningham 2012-06-25 21:19:38 +00:00
parent a5d60f4657
commit c7734db4fa
15 changed files with 72 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/inference/BayesTree-inl.h
View File

@ -130,10 +130,10 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s) const {
void BayesTree<CONDITIONAL,CLIQUE>::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<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::print(const std::string& s) const {
void BayesTree<CONDITIONAL,CLIQUE>::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);
}
/* ************************************************************************* */

6
gtsam/inference/BayesTree.h
View File

@ -70,7 +70,8 @@ namespace gtsam {
// A convenience class for a list of shared cliques
struct Cliques : public std::list<sharedClique> {
void print(const std::string& s = "Cliques") const;
void print(const std::string& s = "Cliques",
const IndexFormatter& indexFormatter = &(boost::lexical_cast<std::string, Index>)) const;
bool equals(const Cliques& other, double tol = 1e-9) const;
};
@ -175,7 +176,8 @@ namespace gtsam {
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& 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<std::string, Index>) ) const;
/// @}
/// @name Standard Interface

10
gtsam/inference/BayesTreeCliqueBase-inl.h
View File

@ -50,8 +50,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::print(const std::string& s) const {
conditional_->print(s);
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::print(const std::string& s, const IndexFormatter& indexFormatter) const {
conditional_->print(s, indexFormatter);
}
/* ************************************************************************* */
@ -65,10 +65,10 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::printTree(const std::string& indent) const {
asDerived(this)->print(indent);
void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::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);
}
/* ************************************************************************* */

4
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -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<std::string, Index>) ) 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<std::string, Index>) ) const;
/// @}
/// @name Standard Interface

25
gtsam/inference/Conditional.h
View File

@ -23,6 +23,7 @@
#include <boost/utility.hpp> // for noncopyable
#include <boost/foreach.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/inference/Factor.h>
@ -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<std::string(KEY)>& formatter = &(boost::lexical_cast<std::string, KEY>) ) const;
/** check equality */
template<class DERIVED>
@ -196,12 +201,12 @@ private:
/* ************************************************************************* */
template<typename KEY>
void Conditional<KEY>::print(const std::string& s) const {
void Conditional<KEY>::print(const std::string& s, const boost::function<std::string(KEY)>& 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

6
gtsam/linear/GaussianConditional.cpp
View File

@ -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");

3
gtsam/linear/GaussianConditional.h
View File

@ -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<std::string, Index>)) const;
/** equals function */
bool equals(const GaussianConditional &cg, double tol = 1e-9) const;

6
gtsam/linear/GaussianFactor.h
View File

@ -23,6 +23,8 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/IndexFactor.h>
#include <boost/lexical_cast.hpp>
#include <string>
#include <utility>
@ -81,7 +83,9 @@ namespace gtsam {
typedef boost::shared_ptr<GaussianFactor> 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<std::string, Index>)) 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) */

4
gtsam/linear/HessianFactor.cpp
View File

@ -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<Eigen::Upper>()), "Ab^T * Ab: ");
}

3
gtsam/linear/HessianFactor.h
View File

@ -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<std::string, Index>)) const;
/** Compare to another factor for testing (implementing Testable) */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;

6
gtsam/linear/JacobianFactor.cpp
View File

@ -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");
}

3
gtsam/linear/JacobianFactor.h
View File

@ -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<std::string, Index>)) const;
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */

15
gtsam/nonlinear/ISAM2.h
View File

@ -254,7 +254,8 @@ public:
/** Construct from an elimination result */
ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<ConditionalType::FactorType> >& 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<std::string, Index>)) 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)

13
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -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));

14
gtsam/nonlinear/Ordering.h
View File

@ -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