Added dummy initialization of LieConfigs from arbitrary other LieConfigs, FusionTupleConfigs can now be created from arbitrary other FusionConfigs.

nonlinear/FusionTupleConfig.h

@@ -14,11 +14,11 @@
 #include <boost/fusion/include/at_key.hpp>
 #include <boost/fusion/include/has_key.hpp>
 #include <boost/fusion/include/zip.hpp>
+#include <boost/fusion/algorithm/transformation.hpp>
 #include <boost/fusion/algorithm/iteration.hpp>
 #include <boost/fusion/algorithm/query.hpp>
 #include <boost/fusion/functional/adapter/fused_function_object.hpp>
 
-
 #include "Testable.h"
 #include "LieConfig.h"
 
@@ -50,13 +50,11 @@ public:
 	/** direct initialization of the underlying structure */
 	FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {}
 
-	/** initialization by slicing a larger config - BROKEN */
+	/** initialization from arbitrary other configs */
 	template<class Configs2>
 	FusionTupleConfig(const FusionTupleConfig<Configs2>& other)
-		//: base_tuple_(other.base_tuple()) // fails
+		: base_tuple_(boost::fusion::fold(other.base_tuple(), Configs(), assign_outer<Configs>()))
 	{
-//		this->base_tuple_ = other.base_tuple(); // fails
-//		Configs val = other.base_tuple(); // also fails
 	}
 
 	virtual ~FusionTupleConfig() {}
@@ -102,15 +100,16 @@ public:
 
 	/** equals */
 	bool equals(const FusionTupleConfig<Configs>& other, double tol=1e-9) const {
+		FusionTupleConfig<Configs>::equals_helper helper(tol);
 		return boost::fusion::all(
 				boost::fusion::zip(
 						boost::fusion::as_vector(base_tuple_),
 						boost::fusion::as_vector(other.base_tuple_)),
-				FusionTupleConfig<Configs>::equals_helper(tol));
+						helper);
 	}
 
 	/** direct access to the underlying fusion set - don't use this */
-	const Configs & base_tuple() const { return base_tuple_; }
+	const BaseTuple & base_tuple() const { return base_tuple_; }
 
 private:
 
@@ -155,6 +154,33 @@ private:
 	    }
 	};
 
+	/** two separate function objects for arbitrary copy construction */
+	template<typename Ret, typename Config>
+	struct assign_inner {
+		typedef Ret result_type;
+
+		Config config;
+		assign_inner(const Config& cfg) : config(cfg) {}
+
+		template<typename T>
+		result_type operator()(const T& t, const result_type& s) const {
+			result_type new_s(s);
+			T new_cfg(config);
+			if (!new_cfg.empty()) boost::fusion::at_key<T>(new_s) = new_cfg;
+			return new_s;
+		}
+	};
+
+	template<typename Ret>
+	struct assign_outer {
+		typedef Ret result_type;
+
+		template<typename T> // T is the config from the "other" config
+		Ret operator()(const T& t, const Ret& s) const {
+			assign_inner<Ret, T> helper(t);
+			return boost::fusion::fold(s, s, helper); // loop over the "self" config
+		}
+	};
 };
 
 

nonlinear/LieConfig.h

@@ -58,6 +58,8 @@ namespace gtsam {
     LieConfig() {}
     LieConfig(const LieConfig& config) :
       values_(config.values_) {}
+    template<class J_alt, class T_alt>
+    LieConfig(const LieConfig<J_alt,T_alt>& other) {} // do nothing when initializing with wrong type
     virtual ~LieConfig() {}
 
     /** print */

nonlinear/tests/testLieConfig.cpp

@@ -230,6 +230,29 @@ TEST(LieConfig, update)
 	expected.insert("v2", Vector_(1, -2.));
 	CHECK(assert_equal(expected,config0));
 }
+
+/* ************************************************************************* */
+TEST(LieConfig, dummy_initialization)
+{
+	typedef TypedSymbol<Vector, 'z'> Key;
+	typedef LieConfig<Key,Vector> Config1;
+	typedef LieConfig<string,Vector> Config2;
+
+	Config1 init1;
+	init1.insert(Key(1), Vector_(2, 1.0, 2.0));
+	init1.insert(Key(2), Vector_(2, 4.0, 3.0));
+
+	Config2 init2;
+	init2.insert("v1", Vector_(2, 1.0, 2.0));
+	init2.insert("v2", Vector_(2, 4.0, 3.0));
+
+	Config1 actual1(init2);
+	Config2 actual2(init1);
+
+	EXPECT(actual1.empty());
+	EXPECT(actual2.empty());
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

tests/testFusionTupleConfig.cpp

@@ -3,13 +3,15 @@
  * @author Alex Cunningham
  */
 
+#include <iostream>
+
 #include <CppUnitLite/TestHarness.h>
 
 #include <Pose2.h>
 #include <Point2.h>
 #include <Pose3.h>
 #include <Point3.h>
-#include "Key.h"
+#include <Key.h>
 #include <LieConfig-inl.h>
 
 #include <FusionTupleConfig.h>
@@ -126,8 +128,8 @@ TEST( testFusionTupleConfig, slicing ) {
 	cfg_pair.insert(l2, l2_val);
 
 	// initialize configs by slicing a larger config
-	TupPointConfig cfg_point(cfg_pair);
 	TupPoseConfig cfg_pose(cfg_pair);
+	TupPointConfig cfg_point(cfg_pair);
 
 	PointConfig expPointConfig;
 	expPointConfig.insert(l1, l1_val);
@@ -137,9 +139,19 @@ TEST( testFusionTupleConfig, slicing ) {
 	expPoseConfig.insert(x1, x1_val);
 	expPoseConfig.insert(x2, x2_val);
 
-	// verify - slicing at initialization isn't implemented
-//	EXPECT(assert_equal(expPointConfig, cfg_point.config<PointConfig>()));
-//	EXPECT(assert_equal(expPoseConfig, cfg_pose.config<PoseConfig>()));
+	// verify
+	EXPECT(assert_equal(expPointConfig, cfg_point.config<PointConfig>()));
+	EXPECT(assert_equal(expPoseConfig, cfg_pose.config<PoseConfig>()));
+}
+
+/* ************************************************************************* */
+TEST( testFusionTupleConfig, upgrading ) {
+	TupPoseConfig small;
+	small.insert(x1, x1_val);
+
+	TupPairConfig actual(small);
+	EXPECT(actual.size() == 1);
+	EXPECT(assert_equal(x1_val, actual.at(x1), tol));
 }
 
 /* ************************************************************************* */
@@ -167,29 +179,6 @@ TEST( testFusionTupleConfig, equals ) {
 	EXPECT(assert_equal(c5, c6, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( testFusionTupleConfig, fusion_slicing ) {
-	typedef fusion::set<int, double, bool> Set1;
-	typedef fusion::set<double> Set2;
-	Set1 s1(1, 2.0, true);
-
-	// assignment
-	EXPECT(fusion::at_key<int>(s1) == 1);
-	fusion::at_key<int>(s1) = 2;
-	EXPECT(fusion::at_key<int>(s1) == 2);
-
-	// slicing: big set sliced to a small set
-	Set2 s2 = s1; // by assignment
-	EXPECT_DOUBLES_EQUAL(2.0, fusion::at_key<double>(s2), tol);
-
-	Set2 s3(s1); // by initialization
-	EXPECT_DOUBLES_EQUAL(2.0, fusion::at_key<double>(s3), tol);
-
-	// upgrading: small set initializing a big set
-//	Set2 s3(5);
-//	Set1 s4 = s3; // apparently can't do this
-}
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */