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

add a struct to represent vector of symbols

release/4.3a0
Yong-Dian Jian 2010-12-01 22:39:03 +00:00
parent d57c4317ac
commit f661baacbb
2 changed files with 330 additions and 256 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
gtsam/linear/IterativeOptimizationParameters.h
View File

@ -13,64 +13,63 @@
namespace gtsam { namespace gtsam {
// a container for all related parameters
struct IterativeOptimizationParameters {
// a container for all related parameters public:
struct IterativeOptimizationParameters {
public: typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr ;
typedef enum { typedef enum {
SILENT, SILENT, ERROR,
ERROR,
} verbosityLevel; } verbosityLevel;
public: public:
int maxIterations_; int maxIterations_;
int reset_ ; // number of iterations before reset, for cg and gmres int reset_; // number of iterations before reset, for cg and gmres
double epsilon_; // relative error double epsilon_; // relative error
double epsilon_abs_; // absolute error double epsilon_abs_; // absolute error
verbosityLevel verbosity_; verbosityLevel verbosity_;
DimSpec::shared_ptr reduce_spec_;
DimSpec::shared_ptr skeleton_spec_;
// specialize for spcg solver public:
// typedef size_t Index; IterativeOptimizationParameters() :
// typedef std::vector<Index> Spec ; maxIterations_(100), reset_(101), epsilon_(1e-5), epsilon_abs_(1e-5),
// typedef boost::shared_ptr<Spec> sharedSpec ; verbosity_(ERROR), reduce_spec_(), skeleton_spec_() {
DimSpec::shared_ptr reduce_spec_ ; }
DimSpec::shared_ptr skeleton_spec_ ;
public: IterativeOptimizationParameters(
IterativeOptimizationParameters(): const IterativeOptimizationParameters &parameters) :
maxIterations_(100), maxIterations_(parameters.maxIterations_), reset_(parameters.reset_),
reset_(101), epsilon_(parameters.epsilon_), epsilon_abs_(parameters.epsilon_abs_),
epsilon_(1e-5), verbosity_(parameters.verbosity_),
epsilon_abs_(1e-5), reduce_spec_(parameters.reduce_spec_), skeleton_spec_(
verbosity_(ERROR), parameters.skeleton_spec_) {
reduce_spec_(), skeleton_spec_() {} }
IterativeOptimizationParameters(const IterativeOptimizationParameters &parameters): IterativeOptimizationParameters(int maxIterations, double epsilon,
maxIterations_(parameters.maxIterations_), double epsilon_abs, verbosityLevel verbosity = ERROR, int reset = -1) :
reset_(parameters.reset_), maxIterations_(maxIterations), reset_(reset), epsilon_(epsilon),
epsilon_(parameters.epsilon_), epsilon_abs_(epsilon_abs), verbosity_(verbosity) {
epsilon_abs_(parameters.epsilon_abs_), if (reset_ == -1)
verbosity_(parameters.verbosity_), reset_ = maxIterations_ + 1;
reduce_spec_(parameters.reduce_spec_), }
skeleton_spec_(parameters.skeleton_spec_)
{}
int maxIterations() const {
IterativeOptimizationParameters return maxIterations_;
(int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=ERROR, int reset=-1): }
maxIterations_(maxIterations), reset_(reset), int reset() const {
epsilon_(epsilon), epsilon_abs_(epsilon_abs), verbosity_(verbosity) { return reset_;
if (reset_==-1) reset_ = maxIterations_ + 1 ; }
} double epsilon() const {
return epsilon_;
int maxIterations() const { return maxIterations_; } }
int reset() const { return reset_; } double epsilon_abs() const {
double epsilon() const { return epsilon_ ;} return epsilon_abs_;
double epsilon_abs() const { return epsilon_abs_ ; } }
verbosityLevel verbosity() const { return verbosity_ ; } verbosityLevel verbosity() const {
}; return verbosity_;
}
};
} }

487
gtsam/nonlinear/Key.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <vector>
#include <list> #include <list>
#include <iostream> #include <iostream>
#include <boost/format.hpp> #include <boost/format.hpp>
@ -33,261 +34,335 @@
namespace gtsam { namespace gtsam {
/** /**
* TypedSymbol key class is templated on * TypedSymbol key class is templated on
* 1) the type T it is supposed to retrieve, for extra type checking * 1) the type T it is supposed to retrieve, for extra type checking
* 2) the character constant used for its string representation * 2) the character constant used for its string representation
*/ */
template <class T, char C> template<class T, char C>
class TypedSymbol : Testable<TypedSymbol<T,C> > { class TypedSymbol: Testable<TypedSymbol<T, C> > {
protected: protected:
size_t j_; size_t j_;
public: public:
// typedefs // typedefs
typedef T Value; typedef T Value;
// Constructors: // Constructors:
TypedSymbol():j_(0) {} TypedSymbol() :
TypedSymbol(size_t j):j_(j) {} j_(0) {
}
TypedSymbol(size_t j) :
j_(j) {
}
// Get stuff: // Get stuff:
size_t index() const { return j_;} size_t index() const {
static char chr() { return C; } return j_;
const char* c_str() const { return (std::string)(*this).c_str();} }
operator std::string() const { return (boost::format("%c%d") % C % j_).str(); } static char chr() {
std::string latex() const { return (boost::format("%c_{%d}") % C % j_).str(); } return C;
}
const char* c_str() const {
return (std::string) (*this).c_str();
}
operator std::string() const {
return (boost::format("%c%d") % C % j_).str();
}
std::string latex() const {
return (boost::format("%c_{%d}") % C % j_).str();
}
// logic: // logic:
bool operator< (const TypedSymbol& compare) const { return j_<compare.j_;} bool operator<(const TypedSymbol& compare) const {
bool operator== (const TypedSymbol& compare) const { return j_==compare.j_;} return j_ < compare.j_;
bool operator!= (const TypedSymbol& compare) const { return j_!=compare.j_;} }
int compare(const TypedSymbol& compare) const {return j_-compare.j_;} bool operator==(const TypedSymbol& compare) const {
return j_ == compare.j_;
}
bool operator!=(const TypedSymbol& compare) const {
return j_ != compare.j_;
}
int compare(const TypedSymbol& compare) const {
return j_ - compare.j_;
}
// Testable Requirements // Testable Requirements
virtual void print(const std::string& s="") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string)(*this) << std::endl; std::cout << s << ": " << (std::string) (*this) << std::endl;
} }
bool equals(const TypedSymbol& expected, double tol=0.0) const { return (*this)==expected; } bool equals(const TypedSymbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(j_); ar & BOOST_SERIALIZATION_NVP(j_);
} }
}; };
/** forward declaration to avoid circular dependencies */ /** forward declaration to avoid circular dependencies */
template<class T, char C, typename L> template<class T, char C, typename L>
class TypedLabeledSymbol; class TypedLabeledSymbol;
/** /**
* Character and index key used in VectorValues, GaussianFactorGraph, * Character and index key used in VectorValues, GaussianFactorGraph,
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
* keys when linearizing a nonlinear factor graph. This key is not type * keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes. * safe, so cannot be used with any Nonlinear* classes.
*/ */
class Symbol : Testable<Symbol> { class Symbol: Testable<Symbol> {
protected: protected:
unsigned char c_; unsigned char c_;
size_t j_; size_t j_;
public: public:
/** Default constructor */ /** Default constructor */
Symbol() : c_(0), j_(0) {} Symbol() :
c_(0), j_(0) {
}
/** Copy constructor */ /** Copy constructor */
Symbol(const Symbol& key) : c_(key.c_), j_(key.j_) {} Symbol(const Symbol& key) :
c_(key.c_), j_(key.j_) {
}
/** Constructor */ /** Constructor */
Symbol(unsigned char c, size_t j): c_(c), j_(j) {} Symbol(unsigned char c, size_t j) :
c_(c), j_(j) {
}
/** Casting constructor from TypedSymbol */ /** Casting constructor from TypedSymbol */
template<class T, char C> template<class T, char C>
Symbol(const TypedSymbol<T,C>& symbol): c_(C), j_(symbol.index()) {} Symbol(const TypedSymbol<T, C>& symbol) :
c_(C), j_(symbol.index()) {
}
/** Casting constructor from TypedLabeledSymbol */ /** Casting constructor from TypedLabeledSymbol */
template<class T, char C, typename L> template<class T, char C, typename L>
Symbol(const TypedLabeledSymbol<T,C,L>& symbol): c_(C), j_(symbol.encode()) {} Symbol(const TypedLabeledSymbol<T, C, L>& symbol) :
c_(C), j_(symbol.encode()) {
}
/** "Magic" key casting constructor from string */ /** "Magic" key casting constructor from string */
#ifdef GTSAM_MAGIC_KEY #ifdef GTSAM_MAGIC_KEY
Symbol(const std::string& str) { Symbol(const std::string& str) {
if(str.length() < 1) if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'"); throw std::invalid_argument("Cannot parse string key '" + str + "'");
else { else {
const char *c_str = str.c_str(); const char *c_str = str.c_str();
c_ = c_str[0]; c_ = c_str[0];
if(str.length() > 1) if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1); j_ = boost::lexical_cast<size_t>(c_str+1);
else else
j_ = 0; j_ = 0;
} }
} }
Symbol(const char *c_str) { Symbol(const char *c_str) {
std::string str(c_str); std::string str(c_str);
if(str.length() < 1) if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'"); throw std::invalid_argument("Cannot parse string key '" + str + "'");
else { else {
c_ = c_str[0]; c_ = c_str[0];
if(str.length() > 1) if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1); j_ = boost::lexical_cast<size_t>(c_str+1);
else else
j_ = 0; j_ = 0;
} }
} }
#endif #endif
// Testable Requirements // Testable Requirements
void print(const std::string& s="") const { void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string)(*this) << std::endl; std::cout << s << ": " << (std::string) (*this) << std::endl;
} }
bool equals(const Symbol& expected, double tol=0.0) const { return (*this)==expected; } bool equals(const Symbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
/** Retrieve key character */ /** Retrieve key character */
unsigned char chr() const { return c_; } unsigned char chr() const {
return c_;
}
/** Retrieve key index */ /** Retrieve key index */
size_t index() const { return j_; } size_t index() const {
return j_;
}
/** Create a string from the key */ /** Create a string from the key */
operator std::string() const { return str(boost::format("%c%d") % c_ % j_); } operator std::string() const {
return str(boost::format("%c%d") % c_ % j_);
}
/** Comparison for use in maps */ /** Comparison for use in maps */
bool operator< (const Symbol& comp) const { return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); } bool operator<(const Symbol& comp) const {
bool operator== (const Symbol& comp) const { return comp.c_ == c_ && comp.j_ == j_; } return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
bool operator!= (const Symbol& comp) const { return comp.c_ != c_ || comp.j_ != j_; } }
bool operator==(const Symbol& comp) const {
return comp.c_ == c_ && comp.j_ == j_;
}
bool operator!=(const Symbol& comp) const {
return comp.c_ != c_ || comp.j_ != j_;
}
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(j_); ar & BOOST_SERIALIZATION_NVP(j_);
} }
}; };
// Conversion utilities // Conversion utilities
template<class KEY> Symbol key2symbol(KEY key) { template<class KEY> Symbol key2symbol(KEY key) {
return Symbol(key); return Symbol(key);
} }
template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) { template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
std::list<Symbol> symbols; std::list<Symbol> symbols;
std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), key2symbol<KEY> ); std::transform(keys.begin(), keys.end(), std::back_inserter(symbols),
return symbols; key2symbol<KEY> );
} return symbols;
}
/** /**
* TypedLabeledSymbol is a variation of the TypedSymbol that allows * TypedLabeledSymbol is a variation of the TypedSymbol that allows
* for a runtime label to be placed on the label, so as to express * for a runtime label to be placed on the label, so as to express
* "Pose 5 for robot 3" * "Pose 5 for robot 3"
* Labels should be kept to base datatypes (int, char, etc) to * Labels should be kept to base datatypes (int, char, etc) to
* minimize cost of comparisons * minimize cost of comparisons
* *
* The labels will be compared first when comparing Keys, followed by the * The labels will be compared first when comparing Keys, followed by the
* index * index
*/ */
template <class T, char C, typename L> template<class T, char C, typename L>
class TypedLabeledSymbol : public TypedSymbol<T, C>, Testable<TypedLabeledSymbol<T,C,L> > { class TypedLabeledSymbol: public TypedSymbol<T, C> , Testable<
TypedLabeledSymbol<T, C, L> > {
protected: protected:
// Label // Label
L label_; L label_;
public: public:
typedef TypedSymbol<T, C> Base; typedef TypedSymbol<T, C> Base;
// Constructors: // Constructors:
TypedLabeledSymbol() {} TypedLabeledSymbol() {
TypedLabeledSymbol(size_t j, L label) : Base(j), label_(label) {} }
TypedLabeledSymbol(size_t j, L label) :
Base(j), label_(label) {
}
/** Constructor that decodes encoded labels */ /** Constructor that decodes encoded labels */
TypedLabeledSymbol(const Symbol& sym) : TypedSymbol<T,C>(0) { TypedLabeledSymbol(const Symbol& sym) :
size_t shift = (sizeof(size_t)-sizeof(short)) * 8; TypedSymbol<T, C> (0) {
this->j_ = (sym.index() << shift) >> shift; // truncate upper bits size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
label_ = (L) (sym.index() >> shift); // remove lower bits this->j_ = (sym.index() << shift) >> shift; // truncate upper bits
} label_ = (L) (sym.index() >> shift); // remove lower bits
}
/** Constructor to upgrade an existing typed label with a label */ /** Constructor to upgrade an existing typed label with a label */
TypedLabeledSymbol(const Base& key, L label) : Base(key.index()), label_(label) {} TypedLabeledSymbol(const Base& key, L label) :
Base(key.index()), label_(label) {
}
// Get stuff: // Get stuff:
L label() const { return label_;} L label() const {
const char* c_str() const { return (std::string)(*this).c_str();} return label_;
operator std::string() const { }
std::string label_s = (boost::format("%1%") % label_).str(); const char* c_str() const {
return (boost::format("%c%s_%d") % C % label_s % this->j_).str(); return (std::string) (*this).c_str();
} }
std::string latex() const { operator std::string() const {
std::string label_s = (boost::format("%1%") % label_).str(); std::string label_s = (boost::format("%1%") % label_).str();
return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str(); return (boost::format("%c%s_%d") % C % label_s % this->j_).str();
} }
std::string latex() const {
std::string label_s = (boost::format("%1%") % label_).str();
return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
}
// Needed for conversion to LabeledSymbol // Needed for conversion to LabeledSymbol
size_t convertLabel() const { return label_; } size_t convertLabel() const {
return label_;
}
/** /**
* Encoding two numbers into a single size_t for conversion to Symbol * Encoding two numbers into a single size_t for conversion to Symbol
* Stores the label in the upper bytes of the index * Stores the label in the upper bytes of the index
*/ */
size_t encode() const { size_t encode() const {
short label = (short) label_; //bound size of label to 2 bytes short label = (short) label_; //bound size of label to 2 bytes
size_t shift = (sizeof(size_t)-sizeof(short)) * 8; size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
size_t modifier = ((size_t) label) << shift; size_t modifier = ((size_t) label) << shift;
return this->j_ + modifier; return this->j_ + modifier;
} }
// logic: // logic:
bool operator< (const TypedLabeledSymbol& compare) const { bool operator<(const TypedLabeledSymbol& compare) const {
if (label_ == compare.label_) // sort by label first if (label_ == compare.label_) // sort by label first
return this->j_<compare.j_; return this->j_ < compare.j_;
else else
return label_<compare.label_; return label_ < compare.label_;
} }
bool operator== (const TypedLabeledSymbol& compare) const bool operator==(const TypedLabeledSymbol& compare) const {
{ return this->j_==compare.j_ && label_ == compare.label_;} return this->j_ == compare.j_ && label_ == compare.label_;
int compare(const TypedLabeledSymbol& compare) const { }
if (label_ == compare.label_) // sort by label first int compare(const TypedLabeledSymbol& compare) const {
return this->j_-compare.j_; if (label_ == compare.label_) // sort by label first
else return this->j_ - compare.j_;
return label_-compare.label_; else
} return label_ - compare.label_;
}
// Testable Requirements // Testable Requirements
void print(const std::string& s="") const { void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string)(*this) << std::endl; std::cout << s << ": " << (std::string) (*this) << std::endl;
} }
bool equals(const TypedLabeledSymbol& expected, double tol=0.0) const bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const {
{ return (*this)==expected; } return (*this) == expected;
}
private: private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
typedef TypedSymbol<T, C> Base;
ar & boost::serialization::make_nvp("TypedLabeledSymbol",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(label_);
}
};
struct SymbolSpec: public std::vector<Symbol> {
public:
typedef std::vector<Symbol> Base;
typedef boost::shared_ptr<SymbolSpec> shared_ptr;
SymbolSpec(const size_t n) :
Base(n) {
}
};
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
typedef TypedSymbol<T,C> Base;
ar & boost::serialization::make_nvp("TypedLabeledSymbol",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(label_);
}
};
} // namespace gtsam } // namespace gtsam