diff --git a/gtsam.h b/gtsam.h index d3909fcba..4f64bf155 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1476,6 +1476,11 @@ class LabeledSymbol { void print(string s) const; }; +size_t mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(size_t key); +unsigned char mrsymbolLabel(size_t key); +size_t mrsymbolIndex(size_t key); + #include class Ordering { // Standard Constructors and Named Constructors diff --git a/gtsam/nonlinear/Key.cpp b/gtsam/nonlinear/Key.cpp index 821285558..fe9f8c480 100644 --- a/gtsam/nonlinear/Key.cpp +++ b/gtsam/nonlinear/Key.cpp @@ -13,16 +13,21 @@ * @file Key.h * @brief * @author Richard Roberts + * @author Alex Cunningham * @date Feb 20, 2012 */ -#include +#include #include -#include +#include + +#include +#include namespace gtsam { +/* ************************************************************************* */ std::string _defaultKeyFormatter(Key key) { const Symbol asSymbol(key); if(asSymbol.chr() > 0) @@ -31,4 +36,63 @@ std::string _defaultKeyFormatter(Key key) { return boost::lexical_cast(key); } +/* ************************************************************************* */ +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) { + std::cout << s << " "; + if (keys.empty()) + std::cout << "(none)" << std::endl; + else { + BOOST_FOREACH(const gtsam::Key& key, keys) + std::cout << keyFormatter(key) << " "; + std::cout << std::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/Key.h b/gtsam/nonlinear/Key.h index 477febce9..d7e7ed224 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/nonlinear/Key.h @@ -41,10 +41,33 @@ namespace gtsam { /// and Symbol keys. static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; + // Helper function for Multi-robot Key Formatter + GTSAM_EXPORT 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; + /// Useful typedefs for operations with Values - allow for matlab interfaces typedef FastList KeyList; typedef FastVector KeyVector; typedef FastSet KeySet; + /// 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); } diff --git a/gtsam/nonlinear/LabeledSymbol.cpp b/gtsam/nonlinear/LabeledSymbol.cpp index 4a792edb9..97272e873 100644 --- a/gtsam/nonlinear/LabeledSymbol.cpp +++ b/gtsam/nonlinear/LabeledSymbol.cpp @@ -129,63 +129,5 @@ boost::function LabeledSymbol::TypeLabelTest(unsigned char 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 index e30b66f79..eaa7f373f 100644 --- a/gtsam/nonlinear/LabeledSymbol.h +++ b/gtsam/nonlinear/LabeledSymbol.h @@ -20,14 +20,18 @@ #pragma once #include -#include namespace gtsam { /** - * Customized version of gtsam::Symbol for multi-robot use + * Customized version of gtsam::Symbol for multi-robot use. + * + * This variation of Symbol stores two char values in addition to + * an integer key, which is useful for encoding a group for a + * variable. This was originally designed for multi-robot systems, + * which allows expressing "Pose 7 from robot B" as "xB7". */ -class LabeledSymbol { +class GTSAM_EXPORT LabeledSymbol { protected: unsigned char c_, label_; size_t j_; @@ -112,36 +116,19 @@ private: } }; // \class LabeledSymbol -// Helper function for Multi-robot Key Formatter -std::string _multirobotKeyFormatter(gtsam::Key key); +/** Create a symbol key from a character, label and index, i.e. xA5. */ +inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) { + return (Key)LabeledSymbol(c,label,j); +} -/** - * 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; +/** Return the character portion of a symbol key. */ +inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); } -/// Version of orderingIndexFormatter using multi-robot formatter -struct MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter { - MultiRobotLinearFormatter(const gtsam::Ordering& ordering) - : gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {} -}; +/** Return the label portion of a symbol key. */ +inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); } -/// 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); +/** Return the index portion of a symbol key. */ +inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); } } // \namespace gtsam diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index ba9beea8c..bc69ff1bd 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -267,5 +267,11 @@ public: return keyFormatter_(ordering_.key(index)); } }; +/// Version of orderingIndexFormatter using multi-robot formatter +struct GTSAM_EXPORT MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter { + MultiRobotLinearFormatter(const gtsam::Ordering& ordering) + : gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {} +}; + } // \namespace gtsam