diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h index fdc3a6371..b9298b29c 100644 --- a/nonlinear/FusionTupleConfig.h +++ b/nonlinear/FusionTupleConfig.h @@ -7,12 +7,20 @@ #pragma once #include +#include +#include #include +#include #include +#include +#include #include #include +#include -#include + +#include "Testable.h" +#include "LieConfig.h" namespace gtsam { @@ -22,7 +30,7 @@ namespace gtsam { * set */ template -class FusionTupleConfig { +class FusionTupleConfig : public Testable >{ 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 - FusionTupleConfig(const FusionTupleConfig& other) { - // use config subinsert and fusion::foreach + FusionTupleConfig(const FusionTupleConfig& other) + //: 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::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::empty_helper()); } + /** print */ + void print(const std::string& s="") const { + std::cout << "FusionTupleConfig " << s << ":" << std::endl; + boost::fusion::for_each(base_tuple_, FusionTupleConfig::print_helper()); + } + + /** equals */ + bool equals(const FusionTupleConfig& 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::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 @@ -99,6 +135,25 @@ private: } }; + struct print_helper + { + template + void operator()(T t) const + { + t.print(); + } + }; + + struct equals_helper + { + double tol; + equals_helper(double t) : tol(t) {} + template + bool operator()(T t) const + { + return boost::fusion::at_c<0>(t).equals(boost::fusion::at_c<1>(t), tol); + } + }; }; diff --git a/tests/testFusionTupleConfig.cpp b/tests/testFusionTupleConfig.cpp index 087549e1d..07a348532 100644 --- a/tests/testFusionTupleConfig.cpp +++ b/tests/testFusionTupleConfig.cpp @@ -110,10 +110,86 @@ TEST( testFusionTupleConfig, basic_config ) { EXPECT(assert_equal(expPointConfig, cfg1.config())); EXPECT(assert_equal(expPoseConfig, cfg1.config())); + + // 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())); +// EXPECT(assert_equal(expPoseConfig, cfg_pose.config())); +} + +/* ************************************************************************* */ +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 Set1; + typedef fusion::set Set2; + Set1 s1(1, 2.0, true); + + // assignment + EXPECT(fusion::at_key(s1) == 1); + fusion::at_key(s1) = 2; + EXPECT(fusion::at_key(s1) == 2); + + // slicing: big set sliced to a small set + Set2 s2 = s1; // by assignment + EXPECT_DOUBLES_EQUAL(2.0, fusion::at_key(s2), tol); + + Set2 s3(s1); // by initialization + EXPECT_DOUBLES_EQUAL(2.0, fusion::at_key(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); } /* ************************************************************************* */ - -