Added keys() function to LieConfig to return an ordered list of keys

cpp/LieConfig-inl.h

@@ -67,6 +67,14 @@ namespace gtsam {
 	  dim_ += cfg.dim_;
   }
 
+  template<class J, class T>
+  std::list<J> LieConfig<J,T>::keys() const {
+	  std::list<J> ret;
+	  BOOST_FOREACH(const typename Values::value_type& v, values_)
+		  ret.push_back(v.first);
+	  return ret;
+  }
+
   template<class J, class T>
   void LieConfig<J,T>::erase(const J& j) {
     size_t dim; // unused

cpp/LieConfig.h

@@ -10,6 +10,7 @@
 #pragma once
 
 #include <map>
+#include <set>
 
 #include "Vector.h"
 #include "Testable.h"
@@ -93,6 +94,12 @@ namespace gtsam {
      */
     void erase(const J& j, size_t& dim);
 
+    /**
+     * Returns a set of keys in the config
+     * Note: by construction, the list is ordered
+     */
+    std::list<J> keys() const;
+
     /** Replace all keys and variables */
     LieConfig& operator=(const LieConfig& rhs) {
       values_ = rhs.values_;

cpp/testLieConfig.cpp

@@ -8,6 +8,9 @@
 #include <CppUnitLite/TestHarness.h>
 #include <stdexcept>
 
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
+
 #define GTSAM_MAGIC_KEY
 
 #include <Pose2.h>
@@ -147,6 +150,28 @@ TEST(LieConfig, expmap_d)
   CHECK(config0.equals(config0));
 }
 
+/* ************************************************************************* */
+TEST(LieConfig, extract_keys)
+{
+	typedef TypedSymbol<Pose2, 'x'> PoseKey;
+	LieConfig<PoseKey, Pose2> config;
+
+	config.insert(PoseKey(1), Pose2());
+	config.insert(PoseKey(2), Pose2());
+	config.insert(PoseKey(4), Pose2());
+	config.insert(PoseKey(5), Pose2());
+
+	list<PoseKey> expected, actual;
+	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
+	actual = config.keys();
+
+	CHECK(actual.size() == expected.size());
+	list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
+		CHECK(assert_equal(*itExp, *itAct));
+	}
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */