diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h index 6d22dd294..e6719c107 100644 --- a/nonlinear/FusionTupleConfig.h +++ b/nonlinear/FusionTupleConfig.h @@ -7,17 +7,13 @@ #pragma once #include -#include -#include #include #include #include #include #include -#include +#include #include -#include -#include #include "Testable.h" #include "LieConfig.h" @@ -106,6 +102,12 @@ public: return config >().exists(j); } + /** a variant of exists */ + template + boost::optional exists_(const Key& j) const { + return config >().exists_(j); + } + /** retrieve a point */ template const typename Key::Value_t & at(const Key& j) const { @@ -166,10 +168,7 @@ public: bool equals(const FusionTupleConfig& other, double tol=1e-9) const { FusionTupleConfig::equals_helper helper(tol); return boost::fusion::all( - boost::fusion::zip( - boost::fusion::as_vector(base_tuple_), - boost::fusion::as_vector(other.base_tuple_)), - helper); + boost::fusion::zip(base_tuple_,other.base_tuple_), helper); } /** zero: create VectorConfig of appropriate structure */ @@ -335,6 +334,57 @@ inline VectorConfig logmap(const FusionTupleConfig& c0, const FusionTup return c0.logmap(cp); } +/** + * Easy-to-use versions of FusionTupleConfig + * These versions provide a simpler interface more like + * the existing TupleConfig. Each version has a number, and + * takes explicit template arguments for config types. + */ +template +struct FusionTupleConfig1 : public FusionTupleConfig > { + typedef FusionTupleConfig > Base; + typedef FusionTupleConfig1 This; + + typedef C1 Config1; + + FusionTupleConfig1() {} + FusionTupleConfig1(const C1& c1) : Base(boost::fusion::make_set(c1)) {} + + const Config1& first() const { return boost::fusion::at_key(this->base_tuple_); } +}; + +template +struct FusionTupleConfig2 : public FusionTupleConfig > { + typedef FusionTupleConfig > Base; + typedef FusionTupleConfig2 This; + + typedef C1 Config1; + typedef C2 Config2; + + FusionTupleConfig2() {} + FusionTupleConfig2(const C1& c1, const C2& c2) : Base(boost::fusion::make_set(c1, c2)) {} + + const Config1& first() const { return boost::fusion::at_key(this->base_tuple_); } + const Config2& second() const { return boost::fusion::at_key(this->base_tuple_); } +}; + +template +struct FusionTupleConfig3 : public FusionTupleConfig > { + typedef FusionTupleConfig > Base; + + typedef C1 Config1; + typedef C2 Config2; + typedef C3 Config3; + + FusionTupleConfig3() {} + FusionTupleConfig3(const C1& c1, const C2& c2, const C3& c3) + : Base(boost::fusion::make_set(c1, c2, c3)) {} + + const Config1& first() const { return boost::fusion::at_key(this->base_tuple_); } + const Config2& second() const { return boost::fusion::at_key(this->base_tuple_); } + const Config3& third() const { return boost::fusion::at_key(this->base_tuple_); } +}; + } // \namespace gtsam diff --git a/tests/testFusionTupleConfig.cpp b/tests/testFusionTupleConfig.cpp index 83303b7ae..d30d9f570 100644 --- a/tests/testFusionTupleConfig.cpp +++ b/tests/testFusionTupleConfig.cpp @@ -444,6 +444,33 @@ TEST( testFusionTupleConfig, expmap) CHECK(assert_equal(delta, logmap(config1, expected))); } +/* ************************************************************************* */ +TEST( testFusionTupleConfig, configN) +{ + typedef FusionTupleConfig2 ConfigA; + PointConfig expPointConfig; + expPointConfig.insert(l1, l1_val); + expPointConfig.insert(l2, l2_val); + + PoseConfig expPoseConfig; + expPoseConfig.insert(x1, x1_val); + expPoseConfig.insert(x2, x2_val); + + ConfigA cfg1; + EXPECT(cfg1.empty()); + + ConfigA cfg2(expPoseConfig, expPointConfig); + + EXPECT(assert_equal(expPoseConfig, cfg2.config())); + EXPECT(assert_equal(expPointConfig, cfg2.config())); + + EXPECT(assert_equal(expPoseConfig, cfg2.first())); + EXPECT(assert_equal(expPointConfig, cfg2.second())); + + ConfigA cfg3(cfg2); + EXPECT(assert_equal(cfg2, cfg3)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */