Added a constructor for TypedLabeledSymbol from Symbol that decodes the index to split out a label
Alex Cunningham
parent
d0aed559b5
commit
dde0626e5c
234
cpp/Key.h
234
cpp/Key.h
|
|
@ -73,89 +73,9 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/** forward declaration to avoid circular dependencies */
|
||||||
* TypedLabeledSymbol is a variation of the TypedSymbol that allows
|
template<class T, char C, typename L>
|
||||||
* for a runtime label to be placed on the label, so as to express
|
class TypedLabeledSymbol;
|
||||||
* "Pose 5 for robot 3"
|
|
||||||
* Labels should be kept to base datatypes (int, char, etc) to
|
|
||||||
* minimize cost of comparisons
|
|
||||||
*
|
|
||||||
* The labels will be compared first when comparing Keys, followed by the
|
|
||||||
* index
|
|
||||||
*/
|
|
||||||
template <class T, char C, typename L>
|
|
||||||
class TypedLabeledSymbol : public TypedSymbol<T, C>, Testable<TypedLabeledSymbol<T,C,L> > {
|
|
||||||
|
|
||||||
protected:
|
|
||||||
// Label
|
|
||||||
L label_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
// Constructors:
|
|
||||||
|
|
||||||
TypedLabeledSymbol() {}
|
|
||||||
TypedLabeledSymbol(size_t j, L label):TypedSymbol<T,C>(j), label_(label) {}
|
|
||||||
|
|
||||||
// Get stuff:
|
|
||||||
|
|
||||||
L label() const { return label_;}
|
|
||||||
const char* c_str() const { return (std::string)(*this).c_str();}
|
|
||||||
operator std::string() const
|
|
||||||
{ return (boost::format("%c%label%d") % C % label_ % this->j_).str(); }
|
|
||||||
std::string latex() const
|
|
||||||
{ return (boost::format("%c%label_{%d}") % C % label_ % this->j_).str(); }
|
|
||||||
|
|
||||||
// Needed for conversion to LabeledSymbol
|
|
||||||
size_t convertLabel() const { return label_; }
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Encoding two numbers into a single size_t for conversion to Symbol
|
|
||||||
* Stores the label in the upper bytes of the index
|
|
||||||
*/
|
|
||||||
size_t encode() const {
|
|
||||||
short label = (short) label_; //bound size of label to 2 bytes
|
|
||||||
size_t shift = (sizeof(size_t)-sizeof(short)) * 8;
|
|
||||||
size_t modifier = ((size_t) label) << shift;
|
|
||||||
return this->j_ + modifier;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// logic:
|
|
||||||
|
|
||||||
bool operator< (const TypedLabeledSymbol& compare) const {
|
|
||||||
if (label_ == compare.label_) // sort by label first
|
|
||||||
return this->j_<compare.j_;
|
|
||||||
else
|
|
||||||
return 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_;
|
|
||||||
else
|
|
||||||
return label_-compare.label_;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Testable Requirements
|
|
||||||
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; }
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class Archive>
|
|
||||||
void serialize(Archive & ar, const unsigned int version) {
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(this->j_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(label_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Character and index key used in VectorConfig, GaussianFactorGraph,
|
* Character and index key used in VectorConfig, GaussianFactorGraph,
|
||||||
|
|
@ -259,72 +179,92 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Alternate version of Symbol for the Labeled typed keys
|
* TypedLabeledSymbol is a variation of the TypedSymbol that allows
|
||||||
* It is assumed that all Label types have a means to convert to
|
* for a runtime label to be placed on the label, so as to express
|
||||||
* an unsigned int in the Symbol form.
|
* "Pose 5 for robot 3"
|
||||||
|
* Labels should be kept to base datatypes (int, char, etc) to
|
||||||
|
* minimize cost of comparisons
|
||||||
*
|
*
|
||||||
* At the moment, this is mostly useless due to the hardcoding of
|
* The labels will be compared first when comparing Keys, followed by the
|
||||||
* the key in gaussian factors
|
* index
|
||||||
*/
|
*/
|
||||||
// class LabeledSymbol : public Symbol, Testable<LabeledSymbol> {
|
template <class T, char C, typename L>
|
||||||
// protected:
|
class TypedLabeledSymbol : public TypedSymbol<T, C>, Testable<TypedLabeledSymbol<T,C,L> > {
|
||||||
// size_t label_;
|
|
||||||
//
|
|
||||||
// public:
|
|
||||||
// /** Default constructor */
|
|
||||||
// LabeledSymbol() : Symbol(0, 0), label_(0) {}
|
|
||||||
//
|
|
||||||
// /** Copy constructor */
|
|
||||||
// LabeledSymbol(const LabeledSymbol& key) : Symbol(key.c_, key.j_), label_(key.label_) {}
|
|
||||||
//
|
|
||||||
// /** Constructor */
|
|
||||||
// LabeledSymbol(unsigned char c, size_t j, size_t label): Symbol(c, j), label_(label) {}
|
|
||||||
//
|
|
||||||
// /** Casting constructor from TypedSymbol */
|
|
||||||
// template<class T, char C, typename L>
|
|
||||||
// LabeledSymbol(const TypedLabeledSymbol<T,C,L>& symbol)
|
|
||||||
// : Symbol(C, symbol.index()), label_(symbol.convertLabel()) {}
|
|
||||||
//
|
|
||||||
// // Testable Requirements
|
|
||||||
// void print(const std::string& s="") const {
|
|
||||||
// std::cout << s << ": " << (std::string)(*this) << std::endl;
|
|
||||||
// }
|
|
||||||
// bool equals(const LabeledSymbol& expected, double tol=0.0) const { return (*this)==expected; }
|
|
||||||
//
|
|
||||||
// /** Retrieve key character */
|
|
||||||
// unsigned char chr() const { return c_; }
|
|
||||||
//
|
|
||||||
// /** Retrieve key index */
|
|
||||||
// size_t index() const { return j_; }
|
|
||||||
//
|
|
||||||
// /** Retrieve label number */
|
|
||||||
// size_t label() const { return label_; }
|
|
||||||
//
|
|
||||||
// /** Create a string from the key */
|
|
||||||
// operator std::string() const { return str(boost::format("%c%d_%d") % c_ % j_ % label_); }
|
|
||||||
//
|
|
||||||
// /** Comparison for use in maps */
|
|
||||||
// bool operator< (const LabeledSymbol& comp) const {
|
|
||||||
// return c_ < comp.c_ ||
|
|
||||||
// (comp.c_ == c_ && label_ < comp.label_) ||
|
|
||||||
// (comp.c_ == c_ && label_ == comp.label_ && j_ < comp.j_);
|
|
||||||
// }
|
|
||||||
// bool operator== (const LabeledSymbol& comp) const
|
|
||||||
// { return comp.c_ == c_ && comp.j_ == j_ && label_ == comp.label_; }
|
|
||||||
// bool operator!= (const LabeledSymbol& comp) const
|
|
||||||
// { return comp.c_ != c_ || comp.j_ != j_ || comp.label_ != label_; }
|
|
||||||
//
|
|
||||||
// private:
|
|
||||||
//
|
|
||||||
// /** Serialization function */
|
|
||||||
// friend class boost::serialization::access;
|
|
||||||
// template<class Archive>
|
|
||||||
// void serialize(Archive & ar, const unsigned int version) {
|
|
||||||
// ar & BOOST_SERIALIZATION_NVP(c_);
|
|
||||||
// ar & BOOST_SERIALIZATION_NVP(j_);
|
|
||||||
// ar & BOOST_SERIALIZATION_NVP(label_);
|
|
||||||
// }
|
|
||||||
// };
|
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// Label
|
||||||
|
L label_;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
// Constructors:
|
||||||
|
|
||||||
|
TypedLabeledSymbol() {}
|
||||||
|
TypedLabeledSymbol(size_t j, L label):TypedSymbol<T,C>(j), label_(label) {}
|
||||||
|
|
||||||
|
/** Constructor that decodes encoded labels */
|
||||||
|
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
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get stuff:
|
||||||
|
|
||||||
|
L label() const { return label_;}
|
||||||
|
const char* c_str() const { return (std::string)(*this).c_str();}
|
||||||
|
operator std::string() const
|
||||||
|
{ return (boost::format("%c%label%d") % C % label_ % this->j_).str(); }
|
||||||
|
std::string latex() const
|
||||||
|
{ return (boost::format("%c%label_{%d}") % C % label_ % this->j_).str(); }
|
||||||
|
|
||||||
|
// Needed for conversion to LabeledSymbol
|
||||||
|
size_t convertLabel() const { return label_; }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Encoding two numbers into a single size_t for conversion to Symbol
|
||||||
|
* Stores the label in the upper bytes of the index
|
||||||
|
*/
|
||||||
|
size_t encode() const {
|
||||||
|
short label = (short) label_; //bound size of label to 2 bytes
|
||||||
|
size_t shift = (sizeof(size_t)-sizeof(short)) * 8;
|
||||||
|
size_t modifier = ((size_t) label) << shift;
|
||||||
|
return this->j_ + modifier;
|
||||||
|
}
|
||||||
|
|
||||||
|
// logic:
|
||||||
|
|
||||||
|
bool operator< (const TypedLabeledSymbol& compare) const {
|
||||||
|
if (label_ == compare.label_) // sort by label first
|
||||||
|
return this->j_<compare.j_;
|
||||||
|
else
|
||||||
|
return 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_;
|
||||||
|
else
|
||||||
|
return label_-compare.label_;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Testable Requirements
|
||||||
|
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; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class Archive>
|
||||||
|
void serialize(Archive & ar, const unsigned int version) {
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(this->j_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(label_);
|
||||||
|
}
|
||||||
|
};
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -77,8 +77,10 @@ TEST ( TypedLabledSymbol, encoding ) {
|
||||||
// check casting
|
// check casting
|
||||||
Symbol act2 = (Symbol) key1;
|
Symbol act2 = (Symbol) key1;
|
||||||
CHECK(assert_equal(exp, act2));
|
CHECK(assert_equal(exp, act2));
|
||||||
}
|
|
||||||
|
|
||||||
|
// decode
|
||||||
|
CHECK(assert_equal(key1, RobotKey(act1)));
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* *
|
/* ************************************************************************* *
|
||||||
TEST ( TypedLabledSymbol, symbol_translation ) {
|
TEST ( TypedLabledSymbol, symbol_translation ) {
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue