diff --git a/gtsam.h b/gtsam.h index d16ad9837..d3909fcba 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1450,6 +1450,32 @@ size_t symbol(char chr, size_t index); char symbolChr(size_t key); size_t symbolIndex(size_t key); +// Key utilities +gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); +gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); +bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); + +// Default keyformatter +void printKeySet(const gtsam::KeySet& keys); +void printKeySet(const gtsam::KeySet& keys, string s); + +#include +class LabeledSymbol { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol& key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; + + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + + void print(string s) const; +}; + #include class Ordering { // Standard Constructors and Named Constructors diff --git a/gtsam/nonlinear/LabeledSymbol.cpp b/gtsam/nonlinear/LabeledSymbol.cpp new file mode 100644 index 000000000..4a792edb9 --- /dev/null +++ b/gtsam/nonlinear/LabeledSymbol.cpp @@ -0,0 +1,191 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LabeledSymbol.h + * @date Jan 12, 2010 + * @author: Alex Cunningham + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace gtsam { + +using namespace std; + +/* ************************************************************************* */ +LabeledSymbol::LabeledSymbol() +: c_(0), label_(0), j_(0) {} + +/* ************************************************************************* */ +LabeledSymbol::LabeledSymbol(const LabeledSymbol& key) +: c_(key.c_), label_(key.label_), j_(key.j_) {} + +/* ************************************************************************* */ +LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, size_t j) +: c_(c), label_(label), j_(j) {} + +/* ************************************************************************* */ +LabeledSymbol::LabeledSymbol(gtsam::Key key) { + const size_t keyBits = sizeof(gtsam::Key) * 8; + const size_t chrBits = sizeof(unsigned char) * 8; + const size_t lblBits = sizeof(unsigned char) * 8; + const size_t indexBits = keyBits - chrBits - lblBits; + const gtsam::Key chrMask = gtsam::Key(std::numeric_limits::max()) << (indexBits + lblBits); + const gtsam::Key lblMask = gtsam::Key(std::numeric_limits::max()) << indexBits; + const gtsam::Key indexMask = ~(chrMask | lblMask); + c_ = (unsigned char)((key & chrMask) >> (indexBits + lblBits)); + label_ = (unsigned char)((key & lblMask) >> indexBits); + j_ = key & indexMask; +} + +/* ************************************************************************* */ +LabeledSymbol::operator gtsam::Key() const { + const size_t keyBits = sizeof(gtsam::Key) * 8; + const size_t chrBits = sizeof(unsigned char) * 8; + const size_t lblBits = sizeof(unsigned char) * 8; + const size_t indexBits = keyBits - chrBits - lblBits; + const gtsam::Key chrMask = gtsam::Key(std::numeric_limits::max()) << (indexBits + lblBits); + const gtsam::Key lblMask = gtsam::Key(std::numeric_limits::max()) << indexBits; + const gtsam::Key indexMask = ~(chrMask | lblMask); + if(j_ > indexMask) + throw std::invalid_argument("Symbol index is too large"); + gtsam::Key key = (gtsam::Key(c_) << (indexBits + lblBits)) | (gtsam::Key(label_) << indexBits) | j_; + return key; +} + +/* ************************************************************************* */ +void LabeledSymbol::print(const std::string& s) const { + std::cout << s << ": " << (std::string) (*this) << std::endl; +} + +/* ************************************************************************* */ +LabeledSymbol::operator std::string() const { + return str(boost::format("%c%c%d") % c_ % label_ % j_); +} + +/* ************************************************************************* */ +bool LabeledSymbol::operator<(const LabeledSymbol& comp) const { + return c_ < comp.c_ + || (comp.c_ == c_ && label_ < comp.label_) + || (comp.c_ == c_ && comp.label_ == label_ && j_ < comp.j_); +} + +/* ************************************************************************* */ +bool LabeledSymbol::operator==(const LabeledSymbol& comp) const { + return comp.c_ == c_ && comp.label_ == label_ && comp.j_ == j_; +} + +/* ************************************************************************* */ +bool LabeledSymbol::operator!=(const LabeledSymbol& comp) const { + return comp.c_ != c_ || comp.label_ != label_ || comp.j_ != j_; +} + +/* ************************************************************************* */ +bool LabeledSymbol::operator==(gtsam::Key comp) const { + return comp == (gtsam::Key)(*this); +} + +/* ************************************************************************* */ +bool LabeledSymbol::operator!=(gtsam::Key comp) const { + return comp != (gtsam::Key)(*this); +} + +/* ************************************************************************* */ +boost::function LabeledSymbol::TypeTest(unsigned char c) { + namespace bl = boost::lambda; + return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c; +} + +/* ************************************************************************* */ +boost::function LabeledSymbol::LabelTest(unsigned char label) { + namespace bl = boost::lambda; + return bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; +} + +/* ************************************************************************* */ +boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { + namespace bl = boost::lambda; + return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c && + bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; +} + +/* ************************************************************************* */ +std::string _multirobotKeyFormatter(gtsam::Key key) { + const LabeledSymbol asLabeledSymbol(key); + if(asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) + return (std::string)asLabeledSymbol; + + const gtsam::Symbol asSymbol(key); + if (asLabeledSymbol.chr() > 0) + return (std::string)asSymbol; + else + return boost::lexical_cast(key); +} + +/* ************************************************************************* */ +void printKeySet(const gtsam::KeySet& keys, const std::string& s, const KeyFormatter& keyFormatter) { + cout << s << " "; + if (keys.empty()) + cout << "(none)" << endl; + else { + BOOST_FOREACH(const gtsam::Key& key, keys) + cout << keyFormatter(key) << " "; + cout << endl; + } +} + +/* ************************************************************************* */ +gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) { + gtsam::KeySet intersection; + if (keysA.empty() || keysB.empty()) + return intersection; + BOOST_FOREACH(const gtsam::Key& key, keysA) + if (keysB.count(key)) + intersection.insert(key); + return intersection; +} + +/* ************************************************************************* */ +bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) { + if (keysA.empty() || keysB.empty()) + return false; + BOOST_FOREACH(const gtsam::Key& key, keysA) + if (keysB.count(key)) + return true; + return false; +} + +/* ************************************************************************* */ +gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) { + if (keysA.empty() || keysB.empty()) + return keysA; + + gtsam::KeySet difference; + BOOST_FOREACH(const gtsam::Key& key, keysA) + if (!keysB.count(key)) + difference.insert(key); + return difference; +} + +} // \namespace gtsam + diff --git a/gtsam/nonlinear/LabeledSymbol.h b/gtsam/nonlinear/LabeledSymbol.h new file mode 100644 index 000000000..e30b66f79 --- /dev/null +++ b/gtsam/nonlinear/LabeledSymbol.h @@ -0,0 +1,147 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LabeledSymbol.h + * @date Jan 12, 2010 + * @author: Alex Cunningham + * @author: Frank Dellaert + * @author: Richard Roberts + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Customized version of gtsam::Symbol for multi-robot use + */ +class LabeledSymbol { +protected: + unsigned char c_, label_; + size_t j_; + +public: + /** Default constructor */ + LabeledSymbol(); + + /** Copy constructor */ + LabeledSymbol(const LabeledSymbol& key); + + /** Constructor */ + LabeledSymbol(unsigned char c, unsigned char label, size_t j); + + /** Constructor that decodes an integer gtsam::Key */ + LabeledSymbol(gtsam::Key key); + + /** Cast to integer */ + operator gtsam::Key() const; + + // Testable Requirements + void print(const std::string& s = "") const; + + bool equals(const LabeledSymbol& expected, double tol = 0.0) const { + return (*this) == expected; + } + + /** return the integer version */ + gtsam::Key key() const { return (gtsam::Key) *this; } + + /** Retrieve label character */ + inline unsigned char label() const { return label_; } + + /** Retrieve key character */ + inline unsigned char chr() const { return c_; } + + /** Retrieve key index */ + inline size_t index() const { return j_; } + + /** Create a string from the key */ + operator std::string() const; + + /** Comparison for use in maps */ + bool operator<(const LabeledSymbol& comp) const; + bool operator==(const LabeledSymbol& comp) const; + bool operator==(gtsam::Key comp) const; + bool operator!=(const LabeledSymbol& comp) const; + bool operator!=(gtsam::Key comp) const; + + /** Return a filter function that returns true when evaluated on a gtsam::Key whose + * character (when converted to a LabeledSymbol) matches \c c. Use this with the + * Values::filter() function to retrieve all key-value pairs with the + * requested character. + */ + + // Checks only the type + static boost::function TypeTest(unsigned char c); + + // Checks only the robot ID (label_) + static boost::function LabelTest(unsigned char label); + + // Checks both type and the robot ID + static boost::function TypeLabelTest(unsigned char c, unsigned char label); + + // Converts to upper/lower versions of labels + LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } + LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); } + + // Create a new symbol with a different value + LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); } + LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); } + +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(label_); + ar & BOOST_SERIALIZATION_NVP(j_); + } +}; // \class LabeledSymbol + +// Helper function for Multi-robot Key Formatter +std::string _multirobotKeyFormatter(gtsam::Key key); + +/** + * A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain + * integer keys. This keyformatter will need to be passed in to override the default + * formatter in print functions. + * + * Checks for LabeledSymbol, Symbol and then plain keys, in order. + */ +static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter; + +/// Version of orderingIndexFormatter using multi-robot formatter +struct MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter { + MultiRobotLinearFormatter(const gtsam::Ordering& ordering) + : gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {} +}; + +/// Utility function to print sets of keys with optional prefix +void printKeySet(const KeySet& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Computes the intersection between two sets +gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); + +/// Checks if an intersection exists - faster checking size of above +bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); + +/// Computes a difference between sets, so result is those that are in A, but not B +gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); + +} // \namespace gtsam + diff --git a/gtsam/nonlinear/tests/testLabeledSymbol.cpp b/gtsam/nonlinear/tests/testLabeledSymbol.cpp new file mode 100644 index 000000000..edc4b77af --- /dev/null +++ b/gtsam/nonlinear/tests/testLabeledSymbol.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testLabeledSymbol.cpp + * @author Alex Cunningham + */ + +#include // for operator += +using namespace boost::assign; + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) { + LabeledSymbol expected('x', 'A', 4); + Key key(expected); + LabeledSymbol actual(key); + + EXPECT(assert_equal(expected, actual)) +} + +/* ************************************************************************* */ +TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { + + // Test encoding of LabeledSymbol <-> size_t <-> string + // Encoding scheme: + // Top 8 bits: variable type (255 possible values) - zero will not process + // Next 8 high bits: variable type (255 possible values) + // TODO: use fewer bits for type - only 4 or 5 should be necessary - will need more decoding + + if(sizeof(Key) == 8) { + LabeledSymbol symbol(0x78, 0x41, 5); + Key key = 0x7841000000000005; + string str = "xA5"; + + EXPECT_LONGS_EQUAL(key, (Key)symbol); + EXPECT(assert_equal(str, MultiRobotKeyFormatter(symbol))); + EXPECT(assert_equal(symbol, LabeledSymbol(key))); + } else if(sizeof(Key) == 4) { + LabeledSymbol symbol(0x78, 0x41, 5); + Key key = 0x78410005; + string str = "xA5"; + + EXPECT_LONGS_EQUAL(key, (Key)symbol); + EXPECT(assert_equal(str, DefaultKeyFormatter(symbol))); + EXPECT(assert_equal(symbol, LabeledSymbol(key))); + } +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ +