diff --git a/cpp/Key.h b/cpp/Key.h index f9ae51604..b12793ab7 100644 --- a/cpp/Key.h +++ b/cpp/Key.h @@ -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 { - private: + protected: unsigned char c_; size_t j_; @@ -163,6 +182,10 @@ namespace gtsam { template Symbol(const TypedSymbol& symbol): c_(C), j_(symbol.index()) {} + /** Casting constructor from TypedLabeledSymbol */ + template + Symbol(const TypedLabeledSymbol& 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 { +// 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 +// LabeledSymbol(const TypedLabeledSymbol& 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 +// 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 diff --git a/cpp/testKey.cpp b/cpp/testKey.cpp index 238793c9f..867cc2dd2 100644 --- a/cpp/testKey.cpp +++ b/cpp/testKey.cpp @@ -31,7 +31,7 @@ TEST ( TypedSymbol, basic_operations ) { CHECK(key3 < key4); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST ( TypedLabledSymbol, basic_operations ) { typedef TypedLabeledSymbol RobotKey; @@ -54,6 +54,72 @@ TEST ( TypedLabledSymbol, basic_operations ) { CHECK(key5 < key6); } +/* ************************************************************************* * +TEST ( TypedLabledSymbol, encoding ) { + typedef TypedLabeledSymbol 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 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 Key1; + typedef TypedSymbol 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); } /* ************************************************************************* */ +