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

static chr() function for TypedSymbol, and another version of Symbol (don't use)

release/4.3a0
Alex Cunningham 2010-02-09 03:59:45 +00:00
parent 0c74e0c309
commit 942e2b9c6d
2 changed files with 162 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
cpp/Key.h
View File

@ -46,6 +46,7 @@ namespace gtsam {
// 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(); }
@ -57,7 +58,7 @@ namespace gtsam {
int compare(const TypedSymbol& compare) const {return j_-compare.j_;}
// 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;
}
bool equals(const TypedSymbol& expected, double tol=0.0) const { return (*this)==expected; }
@ -105,6 +106,21 @@ namespace gtsam {
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 bits of the index
*
* STILL IN TESTING - DO NOT USE!!
*/
size_t encode() {
//short label = label_;
return this->j_; // + (label << 32);
}
// logic:
bool operator< (const TypedLabeledSymbol& compare) const {
@ -123,6 +139,9 @@ namespace gtsam {
}
// 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; }
@ -145,7 +164,7 @@ namespace gtsam {
* safe, so cannot be used with any Nonlinear* classes.
*/
class Symbol : Testable<Symbol> {
private:
protected:
unsigned char c_;
size_t j_;
@ -163,6 +182,10 @@ namespace gtsam {
template<class T, char C>
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()) {}
/** "Magic" key casting constructor from string */
#ifdef GTSAM_MAGIC_KEY
Symbol(const std::string& str) {
@ -193,7 +216,7 @@ namespace gtsam {
#endif
// Testable Requirements
void print(const std::string& s) const {
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; }
@ -235,5 +258,73 @@ namespace gtsam {
return symbols;
}
/**
* Alternate version of Symbol for the Labeled typed keys
* It is assumed that all Label types have a means to convert to
* an unsigned int in the Symbol form.
*
* At the moment, this is mostly useless due to the hardcoding of
* the key in gaussian factors
*/
// class LabeledSymbol : public Symbol, Testable<LabeledSymbol> {
// protected:
// 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_);
// }
// };
} // namespace gtsam

69
cpp/testKey.cpp
View File

@ -31,7 +31,7 @@ TEST ( TypedSymbol, basic_operations ) {
CHECK(key3 < key4);
}
/* ************************************************************************* *
/* ************************************************************************* */
TEST ( TypedLabledSymbol, basic_operations ) {
typedef TypedLabeledSymbol<Pose3, 'x', int> RobotKey;
@ -54,6 +54,72 @@ TEST ( TypedLabledSymbol, basic_operations ) {
CHECK(key5 < key6);
}
/* ************************************************************************* *
TEST ( TypedLabledSymbol, encoding ) {
typedef TypedLabeledSymbol<Pose3, 'x', char> RobotKey;
cout << "short : " << sizeof(short) << " size_t: " << sizeof(size_t) << endl;
cout << "unsigned int : " << sizeof(unsigned int) << endl;
RobotKey key1(37, 'A');
size_t index = key1.index();
size_t modifier = 65;
modifier = modifier << sizeof(size_t) * 4;
index += modifier;
cout << "index: " << index << " modifier: " << modifier << endl;
// short encoded = 65;
// size_t modifier = encoded << 32;
// size_t index = 37 + encoded;
// Symbol act(key1), exp('x', index);
// CHECK(assert_equal(exp, act));
}
/* ************************************************************************* *
TEST ( TypedLabledSymbol, symbol_translation ) {
typedef TypedLabeledSymbol<Pose3, 'x', char> Key;
Key key1(0, 'A'),
key2(1, 'A'),
key3(0, 'B'),
key4(1, 'B');
LabeledSymbol act1(key1), act2(key2), act3(key3), act4(key4);
LabeledSymbol exp1('x', 0, 'A'),
exp2('x', 1, 'A'),
exp3('x', 0, 'B'),
exp4('x', 1, 'B');
CHECK(assert_equal(exp1, act1));
CHECK(assert_equal(exp2, act2));
CHECK(assert_equal(exp3, act3));
CHECK(assert_equal(exp4, act4));
}
/* ************************************************************************* *
TEST ( TypedLabledSymbol, symbol_comparison ) {
typedef TypedLabeledSymbol<Pose3, 'x', char> Key1;
typedef TypedSymbol<Pose3, 'x'> Key2;
Key1 key1(0, 'A'),
key2(1, 'A'),
key3(0, 'B'),
key4(1, 'B');
Key2 key5(0), key6(1);
LabeledSymbol act1(key1), act2(key2), act3(key3), act4(key4);
CHECK(act1 != act2);
CHECK(act1 != act3);
CHECK(act1 != act4);
CHECK(act1 == act1);
CHECK(act1 < act2);
CHECK(act1 < act3);
}
/* ************************************************************************* */
TEST ( Key, keys2symbols )
{
@ -69,3 +135,4 @@ TEST ( Key, keys2symbols )
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */