Made Key Testable, and moved all but base type away from types.h

gtsam/base/types.cpp

@@ -1,35 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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     types.h
- * @brief    Typedefs for easier changing of types
- * @author   Richard Roberts
- * @date     Aug 21, 2010
- * @addtogroup base
- */
-
-#include <gtsam/base/types.h>
-#include <gtsam/inference/Symbol.h>
-#include <boost/lexical_cast.hpp>
-
-namespace gtsam {
-
-  /* ************************************************************************* */
-  std::string _defaultKeyFormatter(Key key) {
-    const Symbol asSymbol(key);
-    if(asSymbol.chr() > 0)
-      return (std::string)asSymbol;
-    else
-      return boost::lexical_cast<std::string>(key);
-  }
-
-}

gtsam/base/types.h

@@ -21,7 +21,6 @@
 
 #include <gtsam/dllexport.h>
 #include <boost/concept/assert.hpp>
-#include <boost/function.hpp>
 #include <boost/range/concepts.hpp>
 #include <cstddef>
 
@@ -54,18 +53,6 @@ namespace gtsam {
   /// Integer nonlinear key type
   typedef size_t Key;
 
-  /// Typedef for a function to format a key, i.e. to convert it to a string
-  typedef boost::function<std::string(Key)> KeyFormatter;
-
-  // Helper function for DefaultKeyFormatter
-  GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
-
-  /// The default KeyFormatter, which is used if no KeyFormatter is passed to
-  /// a nonlinear 'print' function.  Automatically detects plain integer keys
-  /// and Symbol keys.
-  static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
-
-
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
 

gtsam/inference/Key.cpp

@@ -17,57 +17,72 @@
  * @date Feb 20, 2012
  */
 
-#include <iostream>
-
-#include <boost/lexical_cast.hpp>
-#include <boost/foreach.hpp>
-
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/LabeledSymbol.h>
 
+#include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
+#include <iostream>
+
+using namespace std;
+
 namespace gtsam {
 
 /* ************************************************************************* */
-std::string _multirobotKeyFormatter(Key key) {
+string _defaultKeyFormatter(Key key) {
+  const Symbol asSymbol(key);
+  if (asSymbol.chr() > 0)
+    return (string) asSymbol;
+  else
+    return boost::lexical_cast<string>(key);
+}
+
+/* ************************************************************************* */
+void PrintKey(Key key, const string& s, const KeyFormatter& keyFormatter) {
+  cout << s << keyFormatter(key);
+}
+
+/* ************************************************************************* */
+string _multirobotKeyFormatter(Key key) {
   const LabeledSymbol asLabeledSymbol(key);
   if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0)
-    return (std::string) asLabeledSymbol;
+    return (string) asLabeledSymbol;
 
   const Symbol asSymbol(key);
   if (asLabeledSymbol.chr() > 0)
-    return (std::string) asSymbol;
+    return (string) asSymbol;
   else
-    return boost::lexical_cast<std::string>(key);
+    return boost::lexical_cast<string>(key);
 }
 
 /* ************************************************************************* */
 template<class CONTAINER>
-static void print(const CONTAINER& keys, const std::string& s,
+static void Print(const CONTAINER& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  std::cout << s << " ";
+  cout << s << " ";
   if (keys.empty())
-    std::cout << "(none)" << std::endl;
+    cout << "(none)" << endl;
   else {
     BOOST_FOREACH(const Key& key, keys)
-      std::cout << keyFormatter(key) << " ";
+      cout << keyFormatter(key) << " ";
-    std::cout << std::endl;
+    cout << endl;
   }
 }
 
 /* ************************************************************************* */
-void printKeyList(const KeyList& keys, const std::string& s,
+void PrintKeyList(const KeyList& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  print(keys, s, keyFormatter);
+  Print(keys, s, keyFormatter);
 }
 /* ************************************************************************* */
-void printKeyVector(const KeyVector& keys, const std::string& s,
+void PrintKeyVector(const KeyVector& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  print(keys, s, keyFormatter);
+  Print(keys, s, keyFormatter);
 }
 /* ************************************************************************* */
-void printKeySet(const KeySet& keys, const std::string& s,
+void PrintKeySet(const KeySet& keys, const string& s,
     const KeyFormatter& keyFormatter) {
-  print(keys, s, keyFormatter);
+  Print(keys, s, keyFormatter);
 }
 /* ************************************************************************* */
 

gtsam/inference/Key.h

@@ -17,45 +17,75 @@
  */
 #pragma once
 
-#include <boost/function.hpp>
-#include <string>
-
-#include <gtsam/global_includes.h>
-#include <gtsam/base/FastVector.h>
 #include <gtsam/base/FastList.h>
-#include <gtsam/base/FastSet.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/FastSet.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/types.h>
+#include <gtsam/dllexport.h>
+
+#include <boost/function.hpp>
 
 namespace gtsam {
 
+/// Typedef for a function to format a key, i.e. to convert it to a string
+typedef boost::function<std::string(Key)> KeyFormatter;
 
-  // Helper function for Multi-robot Key Formatter
+// Helper function for DefaultKeyFormatter
-  GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
+GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
 
-  ///
+/// The default KeyFormatter, which is used if no KeyFormatter is passed to
-  /// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
+/// a nonlinear 'print' function.  Automatically detects plain integer keys
-  /// integer keys.  This keyformatter will need to be passed in to override the default
+/// and Symbol keys.
-  /// formatter in print functions.
+static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
-  ///
-  /// 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
+// Helper function for Multi-robot Key Formatter
-  typedef FastList<Key> KeyList;
+GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
-  typedef FastVector<Key> KeyVector;
-  typedef FastSet<Key> KeySet;
-  typedef FastMap<Key,int> KeyGroupMap;
 
-  /// Utility function to print sets of keys with optional prefix
+///
-  GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "",
+/// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+/// 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;
 
-  /// Utility function to print sets of keys with optional prefix
+/// Useful typedef for operations with Values - allows for matlab interface
-  GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "",
+typedef FastVector<Key> KeyVector;
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
-  /// Utility function to print sets of keys with optional prefix
+// TODO(frank): Nothing fast about these :-(
-  GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "",
+typedef FastList<Key> KeyList;
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+typedef FastSet<Key> KeySet;
-}
+typedef FastMap<Key, int> KeyGroupMap;
+
+/// Utility function to print one key with optional prefix
+GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+/// Utility function to print sets of keys with optional prefix
+GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+/// Utility function to print sets of keys with optional prefix
+GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s =
+    "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+/// Utility function to print sets of keys with optional prefix
+GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<Key> {
+  static void Print(const Key& key, const std::string& str = "") {
+    PrintKey(key, str);
+  }
+  static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) {
+    return key1 == key2;
+  }
+};
+
+} // namespace gtsam