Now Pose3Config == LieConfig

cpp/Pose3.h

@@ -68,6 +68,9 @@ namespace gtsam {
     }
   }; // Pose3 class
 
+  // global print
+  inline void print(const Pose3& p, const std::string& s = "") { p.print(s);}
+
   // Dimensionality of the tangent space
   inline size_t dim(const Pose3&) { return 6; }
 

cpp/Pose3Factor.h

@@ -6,25 +6,16 @@
 #pragma once
 
 #include <map>
-#include "BetweenFactor.h"
 #include "Pose3.h"
+#include "LieConfig.h"
+#include "BetweenFactor.h"
 
 namespace gtsam {
 
 	/**
 	 * A config specifically for 3D poses
 	 */
-	class Pose3Config: public std::map<std::string, Pose3> {
+	typedef LieConfig<Pose3> Pose3Config;
-
-	public:
-
-		const Pose3& get(std::string key) const {
-			const_iterator it = find(key);
-			if (it == end()) throw std::invalid_argument("invalid key");
-			return it->second;
-		}
-
-	};
 
 	/**
 	 * A Factor for 3D pose measurements

cpp/testPose3Factor.cpp

@@ -9,6 +9,7 @@
 using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 #include "Pose3Factor.h"
+#include "LieConfig-inl.h"
 
 using namespace std;
 using namespace gtsam;
@@ -27,8 +28,8 @@ TEST( Pose3Factor, error )
 
 	// Create config
 	Pose3Config x;
-	x.insert(make_pair("t1",t1));
+	x.insert("t1",t1);
-	x.insert(make_pair("t2",t2));
+	x.insert("t2",t2);
 
 	// Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z)
 	Vector actual = factor.error_vector(x);
@@ -36,6 +37,13 @@ TEST( Pose3Factor, error )
 	CHECK(assert_equal(expected,actual));
 }
 
+/* ************************************************************************* */
+TEST( Pose3Factor, simple_circle )
+{
+	Pose3Config expected, actual;
+	CHECK(assert_equal(expected,actual));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;