Moved utility functions and formatter from LabeledSymbol to Key. Added namespace functions

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 <gtsam/nonlinear/Ordering.h>
 class Ordering {
   // Standard Constructors and Named Constructors

gtsam/nonlinear/Key.cpp

@@ -13,16 +13,21 @@
  * @file Key.h
  * @brief
  * @author Richard Roberts
+ * @author Alex Cunningham
  * @date Feb 20, 2012
  */
 
-#include <gtsam/nonlinear/Key.h>
+#include <iostream>
 
 #include <boost/lexical_cast.hpp>
-#include <gtsam/nonlinear/Symbol.h>
+#include <boost/foreach.hpp>
+
+#include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/LabeledSymbol.h>
 
 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<std::string>(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<std::string>(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

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<Key> KeyList;
   typedef FastVector<Key> KeyVector;
   typedef FastSet<Key> 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);
 }
 

gtsam/nonlinear/LabeledSymbol.cpp

@@ -129,63 +129,5 @@ boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c,
       bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor<LabeledSymbol>(), 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<std::string>(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
 

gtsam/nonlinear/LabeledSymbol.h

@@ -20,14 +20,18 @@
 #pragma once
 
 #include <gtsam/nonlinear/Symbol.h>
-#include <gtsam/nonlinear/Ordering.h>
 
 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
+/** Create a symbol key from a character, label and index, i.e. xA5. */
-std::string _multirobotKeyFormatter(gtsam::Key key);
+inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) {
+  return (Key)LabeledSymbol(c,label,j);
+}
 
-/**
+/** Return the character portion of a symbol key. */
- * A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
+inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); }
- * 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
+/** Return the label portion of a symbol key. */
-struct MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter {
+inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); }
-  MultiRobotLinearFormatter(const gtsam::Ordering& ordering)
-  : gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {}
-};
 
-/// Utility function to print sets of keys with optional prefix
+/** Return the index portion of a symbol key. */
-void printKeySet(const KeySet& keys, const std::string& s = "",
+inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); }
-    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
 

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