From 6c665b818bffac765c7d5efe52918f7ee2f67631 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 14 Jun 2019 15:01:33 -0400 Subject: [PATCH 1/3] Streaming Key values with a custom formatter --- gtsam/inference/Key.cpp | 40 ++++++++++++++++++++++++- gtsam/inference/Key.h | 30 +++++++++++++++++++ gtsam/inference/tests/testKey.cpp | 50 ++++++++++++++++++++++++------- 3 files changed, 109 insertions(+), 11 deletions(-) diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index f257274410..dd433ff094 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -56,7 +56,7 @@ string _multirobotKeyFormatter(Key key) { /* ************************************************************************* */ template -static void Print(const CONTAINER& keys, const string& s, +void Print(const CONTAINER& keys, const string& s, const KeyFormatter& keyFormatter) { cout << s << " "; if (keys.empty()) @@ -83,6 +83,44 @@ void PrintKeySet(const KeySet& keys, const string& s, const KeyFormatter& keyFormatter) { Print(keys, s, keyFormatter); } + +/* ************************************************************************* */ +// Access to custom stream property. +void *&key_formatter::property(ios_base &s) { + static int kUniqueIndex = ios_base::xalloc(); + return s.pword(kUniqueIndex); +} + +/* ************************************************************************* */ +// Store pointer to formatter in property. +void key_formatter::set_property(ios_base &s, const KeyFormatter &f) { + property(s) = (void *)(&f); +} + +/* ************************************************************************* */ +// Get pointer to formatter from property. +KeyFormatter *key_formatter::get_property(ios_base &s) { + return (KeyFormatter *)(property(s)); +} + +/* ************************************************************************* */ +// Stream operator that will take a key_formatter and set the stream property. +ostream &operator<<(ostream &os, const key_formatter &m) { + key_formatter::set_property(os, m.formatter_); + return os; +} + +/* ************************************************************************* */ +// Stream operator that takes a StreamedKey and properly formats it +ostream &operator<<(ostream &os, const StreamedKey &streamedKey) { + const KeyFormatter *formatter = key_formatter::get_property(os); + if (formatter == nullptr) { + formatter = &DefaultKeyFormatter; + } + os << (*formatter)(streamedKey.key_); + return (os); +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index d400a33c06..ae3f3844b2 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -27,6 +27,8 @@ #include +#include + namespace gtsam { /// Typedef for a function to format a key, i.e. to convert it to a string @@ -52,6 +54,34 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter; +/// To use the key_formatter on Keys, they must be wrapped in a StreamedKey. +struct StreamedKey { + const Key &key_; + explicit StreamedKey(const Key &key) : key_(key) {} + friend std::ostream &operator<<(std::ostream &, const StreamedKey &); +}; + +/** + * Output stream manipulator that will format gtsam::Keys according to the given + * KeyFormatter, as long as Key values are wrapped in a gtsam::StreamedKey. + * LabeledSymbol and Symbol values do not have to be wrapped. + * usage: + * Key key = LabeledSymbol('x', 'A', 5); // cast to key type + * cout << key_formatter(MultiRobotKeyFormatter) << StreamedKey(key); + */ +class key_formatter { + public: + explicit key_formatter(KeyFormatter v) : formatter_(v) {} + friend std::ostream &operator<<(std::ostream &, const key_formatter &); + friend std::ostream &operator<<(std::ostream &, const StreamedKey &); + + private: + KeyFormatter formatter_; + static void *&property(std::ios_base &s); + static void set_property(std::ios_base &s, const KeyFormatter &f); + static KeyFormatter *get_property(std::ios_base &s); +}; + /// Define collection type once and for all - also used in wrappers typedef FastVector KeyVector; diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 93a161ccde..fcdb5709b0 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -22,6 +22,9 @@ #include #include // for operator += + +#include + using namespace boost::assign; using namespace std; using namespace gtsam; @@ -41,17 +44,15 @@ TEST(Key, KeySymbolConversion) { template Key KeyTestValue(); -template<> -Key KeyTestValue<8>() -{ +template <> +Key KeyTestValue<8>() { return 0x6100000000000005; -}; +} -template<> -Key KeyTestValue<4>() -{ +template <> +Key KeyTestValue<4>() { return 0x61000005; -}; +} /* ************************************************************************* */ TEST(Key, KeySymbolEncoding) { @@ -68,12 +69,41 @@ TEST(Key, KeySymbolEncoding) { /* ************************************************************************* */ TEST(Key, ChrTest) { - Key key = Symbol('c',3); + Symbol key('c', 3); EXPECT(Symbol::ChrTest('c')(key)); EXPECT(!Symbol::ChrTest('d')(key)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// A custom (nonsensical) formatter. +string myFormatter(Key key) { + return "special"; +} + +TEST(Key, Formatting) { + Symbol key('c', 3); + EXPECT("c3" == DefaultKeyFormatter(key)); + + // Try streaming keys, should be default-formatted. + stringstream ss; + ss << StreamedKey(key); + EXPECT("c3" == ss.str()); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(myFormatter) << StreamedKey(key); + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(DefaultKeyFormatter) << StreamedKey(key); + EXPECT("c3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From 4e2713026cb12d7de65422621d8d33a589ee556b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 14 Jun 2019 15:31:08 -0400 Subject: [PATCH 2/3] Streaming for Symbols --- gtsam/inference/Symbol.cpp | 5 +++ gtsam/inference/Symbol.h | 3 ++ gtsam/inference/tests/testSymbol.cpp | 50 ++++++++++++++++++++++++++++ 3 files changed, 58 insertions(+) create mode 100644 gtsam/inference/tests/testSymbol.cpp diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index ccabcb07e0..5e41b3eac4 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -66,5 +66,10 @@ boost::function Symbol::ChrTest(unsigned char c) { return bind(&Symbol::chr, bind(make, _1)) == c; } +std::ostream &operator<<(std::ostream &os, const Symbol &symbol) { + os << StreamedKey(symbol); + return os; +} + } // namespace gtsam diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 8e22202ed9..86574f70d4 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -112,6 +112,9 @@ class GTSAM_EXPORT Symbol { */ static boost::function ChrTest(unsigned char c); + /// Output stream operator that can be used with key_formatter (see Key.h). + friend std::ostream &operator<<(std::ostream &, const Symbol &); + private: /** Serialization function */ diff --git a/gtsam/inference/tests/testSymbol.cpp b/gtsam/inference/tests/testSymbol.cpp new file mode 100644 index 0000000000..43a0e219a1 --- /dev/null +++ b/gtsam/inference/tests/testSymbol.cpp @@ -0,0 +1,50 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSymbol.cpp + * @author Frank Dellaert + */ + +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// A custom (nonsensical) formatter. +string myFormatter(Key key) { + return "special"; +} + +TEST(Symbol, Formatting) { + Symbol symbol('c', 3); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(myFormatter) << symbol; + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(MultiRobotKeyFormatter) << symbol; + EXPECT("c3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 86b4be3304dbf12053349125de083fee6c783f69 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 14 Jun 2019 15:31:23 -0400 Subject: [PATCH 3/3] Streaming for LabeledSymbols --- gtsam/inference/LabeledSymbol.cpp | 7 ++++++ gtsam/inference/LabeledSymbol.h | 7 +++++- gtsam/inference/tests/testLabeledSymbol.cpp | 25 ++++++++++++++++++++- 3 files changed, 37 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 0d9c35d2c7..6d34883fa3 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -121,6 +121,13 @@ boost::function LabeledSymbol::TypeLabelTest(unsigned char c, return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } + +/* ************************************************************************* */ +std::ostream &operator<<(std::ostream &os, const LabeledSymbol &symbol) { + os << StreamedKey(symbol); + return os; +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 8c521b0678..5b3ec8766f 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -100,10 +100,15 @@ class GTSAM_EXPORT LabeledSymbol { 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 + // Create a new symbol with a different character. LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); } + + // Create a new symbol with a different label. LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); } + /// Output stream operator that can be used with key_formatter (see Key.h). + friend std::ostream &operator<<(std::ostream &, const LabeledSymbol &); + private: /** Serialization function */ diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index b463f41310..2a56b39c21 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -80,6 +80,29 @@ TEST(LabeledSymbol, ChrTest) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// A custom (nonsensical) formatter. +string myFormatter(Key key) { + return "special"; +} + +TEST(LabeledSymbol, Formatting) { + LabeledSymbol symbol('c', 'A', 3); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(myFormatter) << symbol; + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(MultiRobotKeyFormatter) << symbol; + EXPECT("cA3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */