Added experimental replacement for TupleConfig using boost.fusion - this should eventually make it possible to make nonlinear factors interchangable regardless of config type.

nonlinear/FusionTupleConfig.h

@@ -0,0 +1,107 @@
+/**
+ * @file FusionTupleConfig.h
+ * @brief Experimental tuple config using boost.Fusion
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <boost/fusion/container/set.hpp>
+#include <boost/fusion/include/make_set.hpp>
+#include <boost/fusion/include/at_key.hpp>
+#include <boost/fusion/algorithm/iteration.hpp>
+#include <boost/fusion/algorithm/query.hpp>
+
+#include <LieConfig.h>
+
+namespace gtsam {
+
+/**
+ * This config uses a real tuple to store types
+ * The template parameter should be a boost fusion structure, like
+ * set<Config1, Config2>
+ */
+template<class Configs>
+class FusionTupleConfig {
+
+public:
+	/** useful types */
+	typedef Configs BaseTuple;
+
+protected:
+	/** the underlying tuple storing everything */
+	Configs base_tuple_;
+
+public:
+	/** create an empty config */
+	FusionTupleConfig() {}
+
+	/** copy constructor */
+	FusionTupleConfig(const FusionTupleConfig<Configs>& other) : base_tuple_(other.base_tuple_) {}
+
+	/** direct initialization of the underlying structure */
+	FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {}
+
+	/** initialization by slicing a larger config */
+	template<class Configs2>
+	FusionTupleConfig(const FusionTupleConfig<Configs2>& other) {
+		// use config subinsert and fusion::foreach
+	}
+
+	virtual ~FusionTupleConfig() {}
+
+	/** insertion  */
+	template <class Key, class Value>
+	void insert(const Key& j, const Value& x) {
+		boost::fusion::at_key<LieConfig<Key, Value> >(base_tuple_).insert(j,x);
+	}
+
+	/** retrieve a point */
+	template<class Key>
+	const typename Key::Value_t & at(const Key& j) const {
+		return boost::fusion::at_key<LieConfig<Key, typename Key::Value_t> >(base_tuple_).at(j);
+	}
+
+	/** retrieve a full config */
+	template<class Config>
+	const Config & config() const {
+		return boost::fusion::at_key<Config>(base_tuple_);
+	}
+
+	/** size of the config - sum all sizes from subconfigs */
+	size_t size() const {
+		return boost::fusion::accumulate(base_tuple_, 0, FusionTupleConfig<Configs>::size_helper());
+	}
+
+	/** returns true if the config is empty */
+	bool empty() const {
+		return boost::fusion::all(base_tuple_, FusionTupleConfig<Configs>::empty_helper());
+	}
+
+private:
+	/** helper structs to make use of fusion algorithms */
+	struct size_helper {
+	    typedef size_t result_type;
+
+	    template<typename T>
+	    size_t operator()(const T& t, const size_t& s) const
+	    {
+	        return s + t.size();
+	    }
+	};
+
+	struct empty_helper
+	{
+	    template<typename T>
+	    bool operator()(T t) const
+	    {
+	        return t.empty();
+	    }
+	};
+
+
+};
+
+
+} // \namespace gtsam
+

nonlinear/Makefile.am

@@ -12,7 +12,8 @@ check_PROGRAMS =
 #----------------------------------------------------------------------------------------------------
 
 # Lie Groups
-headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
+headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h 
+headers += FusionTupleConfig.h
 check_PROGRAMS += tests/testLieConfig 
 
 # Nonlinear nonlinear 

tests/Makefile.am

@@ -13,6 +13,9 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
 check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint 
 check_PROGRAMS += testTransformConstraint testLinearApproxFactor
 
+# experimental
+check_PROGRAMS += testFusionTupleConfig
+
 if USE_LDL
 check_PROGRAMS += testConstraintOptimizer 
 endif

tests/testFusionTupleConfig.cpp

@@ -0,0 +1,119 @@
+/**
+ * @file testFusionTupleConfig.cpp
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <Pose2.h>
+#include <Point2.h>
+#include <Pose3.h>
+#include <Point3.h>
+#include "Key.h"
+#include <LieConfig-inl.h>
+
+#include <FusionTupleConfig.h>
+
+using namespace boost;
+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;
+typedef LieConfig<PointKey, Point2> PointConfig;
+
+// some generic poses, points and keys
+PoseKey x1(1), x2(2);
+Pose2 x1_val(1.0, 2.0, 0.3), x2_val(2.0, 3.0, 0.4);
+PointKey l1(1), l2(2);
+Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0);
+
+typedef FusionTupleConfig<fusion::set<PointConfig> > TupPointConfig;
+typedef FusionTupleConfig<fusion::set<PoseConfig> > TupPoseConfig;
+typedef FusionTupleConfig<fusion::set<PoseConfig, PointConfig> > TupPairConfig;
+
+/* ************************************************************************* */
+TEST( testFusionTupleConfig, basic_config ) {
+
+	// some initialized configs to start with
+	PoseConfig poseConfig1;
+	poseConfig1.insert(x1, x1_val);
+	PointConfig pointConfig1;
+	pointConfig1.insert(l1, l1_val);
+
+	// basic initialization
+	TupPointConfig cfg1A, cfg2A(pointConfig1);
+	EXPECT(cfg1A.size() == 0);
+	EXPECT(cfg2A.size() == 1);
+	EXPECT(cfg1A.empty());
+	EXPECT(!cfg2A.empty());
+
+	TupPoseConfig cfg1B, cfg2B(poseConfig1);
+	EXPECT(cfg1B.size() == 0);
+	EXPECT(cfg2B.size() == 1);
+	EXPECT(cfg1B.empty());
+	EXPECT(!cfg2B.empty());
+
+	TupPairConfig cfg1, cfg2(fusion::make_set(poseConfig1, pointConfig1));
+	EXPECT(cfg1.size() == 0);
+	EXPECT(cfg1.empty());
+	EXPECT(cfg2.size() == 2);
+	EXPECT(!cfg2.empty());
+
+	// insertion
+	cfg1A.insert(l1, l1_val);
+	EXPECT(cfg1A.size() == 1);
+	cfg1A.insert(l2, l2_val);
+	EXPECT(cfg1A.size() == 2);
+	cfg2A.insert(l2, l2_val);
+	EXPECT(cfg2A.size() == 2);
+
+	cfg1B.insert(x1, x1_val);
+	EXPECT(cfg1B.size() == 1);
+	cfg1B.insert(x2, x2_val);
+	EXPECT(cfg1B.size() == 2);
+	cfg2B.insert(x2, x2_val);
+	EXPECT(cfg2B.size() == 2);
+
+	cfg1.insert(x1, x1_val);
+	cfg1.insert(x2, x2_val);
+	cfg1.insert(l1, l1_val);
+	cfg1.insert(l2, l2_val);
+	EXPECT(cfg1.size() == 4);
+
+	// retrieval of elements
+	EXPECT(assert_equal(l1_val, cfg1A.at(l1)));
+	EXPECT(assert_equal(l2_val, cfg1A.at(l2)));
+
+	EXPECT(assert_equal(x1_val, cfg1B.at(x1)));
+	EXPECT(assert_equal(x2_val, cfg1B.at(x2)));
+
+	EXPECT(assert_equal(l1_val, cfg1.at(l1)));
+	EXPECT(assert_equal(l2_val, cfg1.at(l2)));
+	EXPECT(assert_equal(x1_val, cfg1.at(x1)));
+	EXPECT(assert_equal(x2_val, cfg1.at(x2)));
+
+	// config extraction
+	PointConfig expPointConfig;
+	expPointConfig.insert(l1, l1_val);
+	expPointConfig.insert(l2, l2_val);
+
+	PoseConfig expPoseConfig;
+	expPoseConfig.insert(x1, x1_val);
+	expPoseConfig.insert(x2, x2_val);
+
+	EXPECT(assert_equal(expPointConfig, cfg1A.config<PointConfig>()));
+	EXPECT(assert_equal(expPoseConfig, cfg1B.config<PoseConfig>()));
+
+	EXPECT(assert_equal(expPointConfig, cfg1.config<PointConfig>()));
+	EXPECT(assert_equal(expPoseConfig, cfg1.config<PoseConfig>()));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
+
+