From 3fd36bca5306fb0a8a51dc84b56a81b0e2cfb5f5 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 12 Aug 2010 12:44:36 +0000
Subject: [PATCH] Added TupleConfig1 wrapper

---
 nonlinear/TupleConfig-inl.h | 17 +++++++++++++
 nonlinear/TupleConfig.h     | 29 ++++++++++++++++++++++
 tests/testTupleConfig.cpp   | 48 +++++++++++++++++++++++++++++++++----
 3 files changed, 89 insertions(+), 5 deletions(-)

diff --git a/nonlinear/TupleConfig-inl.h b/nonlinear/TupleConfig-inl.h
index e34946a2e..e629aab11 100644
--- a/nonlinear/TupleConfig-inl.h
+++ b/nonlinear/TupleConfig-inl.h
@@ -11,6 +11,9 @@
 #include "TupleConfig.h"
 
 // TupleConfig instantiations for N = 1-6
+#define INSTANTIATE_TUPLE_CONFIG1(Config1) \
+		template class TupleConfig1<Config1>;
+
 #define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \
 		template class TupleConfig2<Config1, Config2>;
 
@@ -33,6 +36,20 @@ namespace gtsam {
 /** TupleConfigN Implementations */
 /* ************************************************************************* */
 
+/* ************************************************************************* */
+/** TupleConfig 1 */
+template<class Config1>
+TupleConfig1<Config1>::TupleConfig1(const TupleConfig1<Config1>& config) :
+		  TupleConfigEnd<Config1> (config) {}
+
+template<class Config1>
+TupleConfig1<Config1>::TupleConfig1(const Config1& cfg1) :
+			  TupleConfigEnd<Config1> (cfg1) {}
+
+template<class Config1>
+TupleConfig1<Config1>::TupleConfig1(const TupleConfigEnd<Config1>& config) :
+	TupleConfigEnd<Config1>(config) {}
+
 /* ************************************************************************* */
 /** TupleConfig 2 */
 template<class Config1, class Config2>
diff --git a/nonlinear/TupleConfig.h b/nonlinear/TupleConfig.h
index cf7f6d5a4..b5f2d5eeb 100644
--- a/nonlinear/TupleConfig.h
+++ b/nonlinear/TupleConfig.h
@@ -302,6 +302,35 @@ namespace gtsam {
    *
    * The interface is designed to mimic PairConfig, but for 2-6 config types.
    */
+
+  template<class C1>
+  class TupleConfig1 : public TupleConfigEnd<C1> {
+  public:
+ 	  // typedefs
+ 	  typedef C1 Config1;
+
+ 	  typedef TupleConfigEnd<C1> Base;
+ 	  typedef TupleConfig1<C1> This;
+
+ 	  TupleConfig1() {}
+ 	  TupleConfig1(const This& config);
+ 	  TupleConfig1(const Base& config);
+ 	  TupleConfig1(const Config1& cfg1);
+
+ 	  // access functions
+ 	  inline const Config1& first() const { return this->config(); }
+  };
+
+  template<class C1>
+  TupleConfig1<C1> expmap(const TupleConfig1<C1>& c, const VectorConfig& delta) {
+ 	  return c.expmap(delta);
+  }
+
+  template<class C1>
+  VectorConfig logmap(const TupleConfig1<C1>& c1, const TupleConfig1<C1>& c2) {
+ 	  return c1.logmap(c2);
+  }
+
   template<class C1, class C2>
   class TupleConfig2 : public TupleConfig<C1, TupleConfigEnd<C2> > {
   public:
diff --git a/tests/testTupleConfig.cpp b/tests/testTupleConfig.cpp
index a344258c5..4f3e7176e 100644
--- a/tests/testTupleConfig.cpp
+++ b/tests/testTupleConfig.cpp
@@ -1,8 +1,7 @@
-/*
- * testTupleConfig.cpp
- *
- *  Created on: Jan 13, 2010
- *      Author: richard
+/**
+ * @file testTupleConfig.cpp
+ * @author Richard Roberts
+ * @author Alex Cunningham
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -23,6 +22,8 @@
 using namespace gtsam;
 using namespace std;
 
+static const double tol = 1e-5;
+
 typedef TypedSymbol<Pose2, 'x'> PoseKey;
 typedef TypedSymbol<Point2, 'l'> PointKey;
 typedef LieConfig<PoseKey, Pose2> PoseConfig;
@@ -211,6 +212,43 @@ typedef LieConfig<Point3Key2, Point3> Point3Config2;
 typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA;
 typedef TupleConfig<PoseConfig, TupleConfig<PointConfig, TupleConfigEnd<LamConfig> > > ConfigB;
 
+typedef TupleConfig1<PoseConfig> TuplePoseConfig;
+typedef TupleConfig1<PointConfig> TuplePointConfig;
+typedef TupleConfig2<PoseConfig, PointConfig> SimpleConfig;
+
+/* ************************************************************************* */
+TEST(TupleConfig, slicing) {
+	PointKey l1(1), l2(2);
+	Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0);
+	PoseKey x1(1), x2(2);
+	Pose2 x1_val(1.0, 2.0, 0.3), x2_val(3.0, 4.0, 0.4);
+
+	PoseConfig liePoseConfig;
+	liePoseConfig.insert(x1, x1_val);
+	liePoseConfig.insert(x2, x2_val);
+
+	PointConfig liePointConfig;
+	liePointConfig.insert(l1, l1_val);
+	liePointConfig.insert(l2, l2_val);
+
+	// construct TupleConfig1 from the base config
+	TuplePoseConfig tupPoseConfig1(liePoseConfig);
+	EXPECT(assert_equal(liePoseConfig, tupPoseConfig1.first(), tol));
+
+	TuplePointConfig tupPointConfig1(liePointConfig);
+	EXPECT(assert_equal(liePointConfig, tupPointConfig1.first(), tol));
+
+//	// construct a TupleConfig2 from a TupleConfig1
+//	SimpleConfig pairConfig1(tupPoseConfig1);
+//	EXPECT(assert_equal(liePoseConfig, pairConfig1.first(), tol));
+//	EXPECT(pairConfig1.second().empty());
+//
+//	SimpleConfig pairConfig2(tupPointConfig1);
+//	EXPECT(assert_equal(liePointConfig, pairConfig2.second(), tol));
+//	EXPECT(pairConfig1.first().empty());
+
+}
+
 /* ************************************************************************* */
 TEST(TupleConfig, basic_functions) {
 	// create some tuple configs