More of FusionTupleConfig works

nonlinear/FusionTupleConfig.h

@@ -7,12 +7,20 @@
 #pragma once
 
 #include <boost/fusion/container/set.hpp>
+#include <boost/fusion/container/vector.hpp>
+#include <boost/fusion/sequence/intrinsic.hpp>
 #include <boost/fusion/include/make_set.hpp>
+#include <boost/fusion/include/as_vector.hpp>
 #include <boost/fusion/include/at_key.hpp>
+#include <boost/fusion/include/has_key.hpp>
+#include <boost/fusion/include/zip.hpp>
 #include <boost/fusion/algorithm/iteration.hpp>
 #include <boost/fusion/algorithm/query.hpp>
+#include <boost/fusion/functional/adapter/fused_function_object.hpp>
 
-#include <LieConfig.h>
+
+#include "Testable.h"
+#include "LieConfig.h"
 
 namespace gtsam {
 
@@ -22,7 +30,7 @@ namespace gtsam {
  * set<Config1, Config2>
  */
 template<class Configs>
-class FusionTupleConfig {
+class FusionTupleConfig : public Testable<FusionTupleConfig<Configs> >{
 
 public:
 	/** useful types */
@@ -42,10 +50,13 @@ public:
 	/** direct initialization of the underlying structure */
 	FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {}
 
-	/** initialization by slicing a larger config */
+	/** initialization by slicing a larger config - BROKEN */
 	template<class Configs2>
-	FusionTupleConfig(const FusionTupleConfig<Configs2>& other) {
+	FusionTupleConfig(const FusionTupleConfig<Configs2>& other)
-		// use config subinsert and fusion::foreach
+		//: base_tuple_(other.base_tuple()) // fails
+	{
+//		this->base_tuple_ = other.base_tuple(); // fails
+//		Configs val = other.base_tuple(); // also fails
 	}
 
 	virtual ~FusionTupleConfig() {}
@@ -73,14 +84,39 @@ public:
 		return boost::fusion::accumulate(base_tuple_, 0, FusionTupleConfig<Configs>::size_helper());
 	}
 
+	/** number of configs in the config */
+	size_t nrConfigs() const {
+		return boost::fusion::size(base_tuple_);
+	}
+
 	/** returns true if the config is empty */
 	bool empty() const {
 		return boost::fusion::all(base_tuple_, FusionTupleConfig<Configs>::empty_helper());
 	}
 
+	/** print */
+	void print(const std::string& s="") const {
+		std::cout << "FusionTupleConfig " << s << ":" << std::endl;
+		boost::fusion::for_each(base_tuple_, FusionTupleConfig<Configs>::print_helper());
+	}
+
+	/** equals */
+	bool equals(const FusionTupleConfig<Configs>& other, double tol=1e-9) const {
+		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));
+	}
+
+	/** direct access to the underlying fusion set - don't use this */
+	const Configs & base_tuple() const { return base_tuple_; }
+
 private:
+
 	/** helper structs to make use of fusion algorithms */
-	struct size_helper {
+	struct size_helper
+	{
 	    typedef size_t result_type;
 
 	    template<typename T>
@@ -99,6 +135,25 @@ private:
 	    }
 	};
 
+	struct print_helper
+	{
+	    template<typename T>
+	    void operator()(T t) const
+	    {
+	        t.print();
+	    }
+	};
+
+	struct equals_helper
+	{
+		double tol;
+		equals_helper(double t) : tol(t) {}
+	    template<typename T>
+	    bool operator()(T t) const
+	    {
+	        return boost::fusion::at_c<0>(t).equals(boost::fusion::at_c<1>(t), tol);
+	    }
+	};
 
 };
 

tests/testFusionTupleConfig.cpp

@@ -110,10 +110,86 @@ TEST( testFusionTupleConfig, basic_config ) {
 
 	EXPECT(assert_equal(expPointConfig, cfg1.config<PointConfig>()));
 	EXPECT(assert_equal(expPoseConfig, cfg1.config<PoseConfig>()));
+
+	// getting sizes of configs
+	EXPECT(cfg1A.nrConfigs() == 1);
+	EXPECT(cfg1B.nrConfigs() == 1);
+	EXPECT(cfg1.nrConfigs() == 2);
+}
+
+/* ************************************************************************* */
+TEST( testFusionTupleConfig, slicing ) {
+	TupPairConfig cfg_pair;
+	cfg_pair.insert(x1, x1_val);
+	cfg_pair.insert(x2, x2_val);
+	cfg_pair.insert(l1, l1_val);
+	cfg_pair.insert(l2, l2_val);
+
+	// initialize configs by slicing a larger config
+	TupPointConfig cfg_point(cfg_pair);
+	TupPoseConfig cfg_pose(cfg_pair);
+
+	PointConfig expPointConfig;
+	expPointConfig.insert(l1, l1_val);
+	expPointConfig.insert(l2, l2_val);
+
+	PoseConfig expPoseConfig;
+	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>()));
+}
+
+/* ************************************************************************* */
+TEST( testFusionTupleConfig, equals ) {
+	TupPairConfig c1, c2, c3, c4, c5, c6;
+	c1.insert(l1, l1_val);
+	c1.insert(l2, l2_val);
+	c1.insert(x1, x1_val);
+	c1.insert(x2, x2_val);
+	c4 = c1;
+	c2.insert(x1, x1_val);
+	c2.insert(x2, x2_val);
+
+	EXPECT(assert_equal(c1, c1, tol));
+	EXPECT(assert_equal(c4, c1, tol));
+	EXPECT(assert_equal(c4, c4, tol));
+	EXPECT(!c1.equals(c2, tol));
+	EXPECT(!c2.equals(c1, tol));
+	EXPECT(!c1.equals(c3, tol));
+	EXPECT(!c2.equals(c3, tol));
+	EXPECT(assert_equal(c3, c3));
+
+	c5.insert(l1, l1_val);
+	c6.insert(l1, l1_val + Point2(1e-6, 1e-6));
+	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); }
 /* ************************************************************************* */
-
-