add a struct to represent vector of symbols
Yong-Dian Jian
parent
d57c4317ac
commit
f661baacbb
|
|
@ -13,7 +13,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
|
||||
// a container for all related parameters
|
||||
struct IterativeOptimizationParameters {
|
||||
|
||||
|
|
@ -22,8 +21,7 @@ namespace gtsam {
|
|||
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
|
||||
|
||||
typedef enum {
|
||||
SILENT,
|
||||
ERROR,
|
||||
SILENT, ERROR,
|
||||
} verbosityLevel;
|
||||
|
||||
public:
|
||||
|
|
@ -32,45 +30,46 @@ namespace gtsam {
|
|||
double epsilon_; // relative error
|
||||
double epsilon_abs_; // absolute error
|
||||
verbosityLevel verbosity_;
|
||||
|
||||
// specialize for spcg solver
|
||||
// typedef size_t Index;
|
||||
// typedef std::vector<Index> Spec ;
|
||||
// typedef boost::shared_ptr<Spec> sharedSpec ;
|
||||
DimSpec::shared_ptr reduce_spec_;
|
||||
DimSpec::shared_ptr skeleton_spec_;
|
||||
|
||||
public:
|
||||
IterativeOptimizationParameters() :
|
||||
maxIterations_(100),
|
||||
reset_(101),
|
||||
epsilon_(1e-5),
|
||||
epsilon_abs_(1e-5),
|
||||
verbosity_(ERROR),
|
||||
reduce_spec_(), skeleton_spec_() {}
|
||||
|
||||
IterativeOptimizationParameters(const IterativeOptimizationParameters ¶meters):
|
||||
maxIterations_(parameters.maxIterations_),
|
||||
reset_(parameters.reset_),
|
||||
epsilon_(parameters.epsilon_),
|
||||
epsilon_abs_(parameters.epsilon_abs_),
|
||||
verbosity_(parameters.verbosity_),
|
||||
reduce_spec_(parameters.reduce_spec_),
|
||||
skeleton_spec_(parameters.skeleton_spec_)
|
||||
{}
|
||||
|
||||
|
||||
IterativeOptimizationParameters
|
||||
(int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=ERROR, int reset=-1):
|
||||
maxIterations_(maxIterations), reset_(reset),
|
||||
epsilon_(epsilon), epsilon_abs_(epsilon_abs), verbosity_(verbosity) {
|
||||
if (reset_==-1) reset_ = maxIterations_ + 1 ;
|
||||
maxIterations_(100), reset_(101), epsilon_(1e-5), epsilon_abs_(1e-5),
|
||||
verbosity_(ERROR), reduce_spec_(), skeleton_spec_() {
|
||||
}
|
||||
|
||||
int maxIterations() const { return maxIterations_; }
|
||||
int reset() const { return reset_; }
|
||||
double epsilon() const { return epsilon_ ;}
|
||||
double epsilon_abs() const { return epsilon_abs_ ; }
|
||||
verbosityLevel verbosity() const { return verbosity_ ; }
|
||||
IterativeOptimizationParameters(
|
||||
const IterativeOptimizationParameters ¶meters) :
|
||||
maxIterations_(parameters.maxIterations_), reset_(parameters.reset_),
|
||||
epsilon_(parameters.epsilon_), epsilon_abs_(parameters.epsilon_abs_),
|
||||
verbosity_(parameters.verbosity_),
|
||||
reduce_spec_(parameters.reduce_spec_), skeleton_spec_(
|
||||
parameters.skeleton_spec_) {
|
||||
}
|
||||
|
||||
IterativeOptimizationParameters(int maxIterations, double epsilon,
|
||||
double epsilon_abs, verbosityLevel verbosity = ERROR, int reset = -1) :
|
||||
maxIterations_(maxIterations), reset_(reset), epsilon_(epsilon),
|
||||
epsilon_abs_(epsilon_abs), verbosity_(verbosity) {
|
||||
if (reset_ == -1)
|
||||
reset_ = maxIterations_ + 1;
|
||||
}
|
||||
|
||||
int maxIterations() const {
|
||||
return maxIterations_;
|
||||
}
|
||||
int reset() const {
|
||||
return reset_;
|
||||
}
|
||||
double epsilon() const {
|
||||
return epsilon_;
|
||||
}
|
||||
double epsilon_abs() const {
|
||||
return epsilon_abs_;
|
||||
}
|
||||
verbosityLevel verbosity() const {
|
||||
return verbosity_;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <iostream>
|
||||
#include <boost/format.hpp>
|
||||
|
|
@ -51,29 +52,53 @@ namespace gtsam {
|
|||
|
||||
// Constructors:
|
||||
|
||||
TypedSymbol():j_(0) {}
|
||||
TypedSymbol(size_t j):j_(j) {}
|
||||
TypedSymbol() :
|
||||
j_(0) {
|
||||
}
|
||||
TypedSymbol(size_t j) :
|
||||
j_(j) {
|
||||
}
|
||||
|
||||
// Get stuff:
|
||||
|
||||
size_t index() const { return j_;}
|
||||
static char chr() { 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(); }
|
||||
size_t index() const {
|
||||
return j_;
|
||||
}
|
||||
static char chr() {
|
||||
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:
|
||||
|
||||
bool operator< (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_;}
|
||||
bool operator<(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
|
||||
virtual void print(const std::string& s = "") const {
|
||||
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:
|
||||
|
||||
|
|
@ -102,21 +127,31 @@ namespace gtsam {
|
|||
|
||||
public:
|
||||
/** Default constructor */
|
||||
Symbol() : c_(0), j_(0) {}
|
||||
Symbol() :
|
||||
c_(0), j_(0) {
|
||||
}
|
||||
|
||||
/** Copy constructor */
|
||||
Symbol(const Symbol& key) : c_(key.c_), j_(key.j_) {}
|
||||
Symbol(const Symbol& key) :
|
||||
c_(key.c_), j_(key.j_) {
|
||||
}
|
||||
|
||||
/** 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 */
|
||||
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 */
|
||||
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 */
|
||||
#ifdef GTSAM_MAGIC_KEY
|
||||
|
|
@ -151,21 +186,35 @@ namespace gtsam {
|
|||
void print(const std::string& s = "") const {
|
||||
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 */
|
||||
unsigned char chr() const { return c_; }
|
||||
unsigned char chr() const {
|
||||
return c_;
|
||||
}
|
||||
|
||||
/** Retrieve key index */
|
||||
size_t index() const { return j_; }
|
||||
size_t index() const {
|
||||
return j_;
|
||||
}
|
||||
|
||||
/** 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 */
|
||||
bool operator< (const Symbol& comp) const { 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 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_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -186,7 +235,8 @@ namespace gtsam {
|
|||
|
||||
template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
|
||||
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),
|
||||
key2symbol<KEY> );
|
||||
return symbols;
|
||||
}
|
||||
|
||||
|
|
@ -201,7 +251,8 @@ namespace gtsam {
|
|||
* index
|
||||
*/
|
||||
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:
|
||||
// Label
|
||||
|
|
@ -213,23 +264,33 @@ namespace gtsam {
|
|||
|
||||
// Constructors:
|
||||
|
||||
TypedLabeledSymbol() {}
|
||||
TypedLabeledSymbol(size_t j, L label) : Base(j), label_(label) {}
|
||||
TypedLabeledSymbol() {
|
||||
}
|
||||
TypedLabeledSymbol(size_t j, L label) :
|
||||
Base(j), label_(label) {
|
||||
}
|
||||
|
||||
/** Constructor that decodes encoded labels */
|
||||
TypedLabeledSymbol(const Symbol& sym) : TypedSymbol<T,C>(0) {
|
||||
TypedLabeledSymbol(const Symbol& sym) :
|
||||
TypedSymbol<T, C> (0) {
|
||||
size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
|
||||
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 */
|
||||
TypedLabeledSymbol(const Base& key, L label) : Base(key.index()), label_(label) {}
|
||||
TypedLabeledSymbol(const Base& key, L label) :
|
||||
Base(key.index()), label_(label) {
|
||||
}
|
||||
|
||||
// Get stuff:
|
||||
|
||||
L label() const { return label_;}
|
||||
const char* c_str() const { return (std::string)(*this).c_str();}
|
||||
L label() const {
|
||||
return label_;
|
||||
}
|
||||
const char* c_str() const {
|
||||
return (std::string) (*this).c_str();
|
||||
}
|
||||
operator std::string() const {
|
||||
std::string label_s = (boost::format("%1%") % label_).str();
|
||||
return (boost::format("%c%s_%d") % C % label_s % this->j_).str();
|
||||
|
|
@ -240,7 +301,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
// 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
|
||||
|
|
@ -261,8 +324,9 @@ namespace gtsam {
|
|||
else
|
||||
return label_ < compare.label_;
|
||||
}
|
||||
bool operator== (const TypedLabeledSymbol& compare) const
|
||||
{ return this->j_==compare.j_ && label_ == compare.label_;}
|
||||
bool operator==(const TypedLabeledSymbol& compare) const {
|
||||
return this->j_ == compare.j_ && label_ == compare.label_;
|
||||
}
|
||||
int compare(const TypedLabeledSymbol& compare) const {
|
||||
if (label_ == compare.label_) // sort by label first
|
||||
return this->j_ - compare.j_;
|
||||
|
|
@ -274,8 +338,9 @@ namespace gtsam {
|
|||
void print(const std::string& s = "") const {
|
||||
std::cout << s << ": " << (std::string) (*this) << std::endl;
|
||||
}
|
||||
bool equals(const TypedLabeledSymbol& expected, double tol=0.0) const
|
||||
{ return (*this)==expected; }
|
||||
bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const {
|
||||
return (*this) == expected;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -289,5 +354,15 @@ namespace gtsam {
|
|||
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) {
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue