From d1267d1ef378500e2259a0457777b71ea8f40a75 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 6 Feb 2010 05:10:25 +0000 Subject: [PATCH] Added more constructors, print functionality and formatting for TupleConfig --- cpp/TupleConfig-inl.h | 134 +++++++++++++++++++++++++++++++++------- cpp/TupleConfig.h | 14 +++-- cpp/testTupleConfig.cpp | 21 +++++-- 3 files changed, 137 insertions(+), 32 deletions(-) diff --git a/cpp/TupleConfig-inl.h b/cpp/TupleConfig-inl.h index aeeb6a68e..ab0cb7e69 100644 --- a/cpp/TupleConfig-inl.h +++ b/cpp/TupleConfig-inl.h @@ -7,8 +7,8 @@ #pragma once +#include #include "LieConfig-inl.h" - #include "TupleConfig.h" #define INSTANTIATE_PAIR_CONFIG(J1,X1,J2,X2) \ @@ -20,6 +20,7 @@ namespace gtsam { +/* ************************************************************************* */ /** PairConfig implementations */ /* ************************************************************************* */ template @@ -40,8 +41,28 @@ void PairConfig::insert(const PairConfig& config) { } } +/* ************************************************************************* */ +/** TupleConfig Implementations */ +/* ************************************************************************* */ + +template +void TupleConfig::print(const std::string& s) const { + std::cout << s << " : " << std::endl; + first_.print(); + second_.print(); +} + +template +void TupleConfigEnd::print(const std::string& s ) const { + first_.print(); +} + +/* ************************************************************************* */ /** TupleConfigN Implementations */ /* ************************************************************************* */ + +/* ************************************************************************* */ +/** TupleConfig 2 */ template TupleConfig2::TupleConfig2(const TupleConfig2& config) : TupleConfig >(config) {} @@ -51,52 +72,117 @@ TupleConfig2::TupleConfig2(const Config1& cfg1, const Config2& TupleConfig >( cfg1, TupleConfigEnd(cfg2)) {} +template +TupleConfig2::TupleConfig2(const TupleConfig >& config) : + TupleConfig >(config) {} + +/* ************************************************************************* */ +/** TupleConfig 3 */ template -TupleConfig3::TupleConfig3(const TupleConfig3& config) : +TupleConfig3::TupleConfig3( + const TupleConfig3& config) : TupleConfig > >(config) {} template -TupleConfig3::TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) : +TupleConfig3::TupleConfig3( + const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) : TupleConfig > >( cfg1, TupleConfig >( cfg2, TupleConfigEnd(cfg3))) {} +template +TupleConfig3::TupleConfig3( + const TupleConfig > >& config) : + TupleConfig > >(config) {} + +/* ************************************************************************* */ +/** TupleConfig 4 */ template -TupleConfig4::TupleConfig4(const TupleConfig4& config) : - TupleConfig > > >(config) {} +TupleConfig4::TupleConfig4( + const TupleConfig4& config) : + TupleConfig > > >(config) {} template -TupleConfig4::TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4) : - TupleConfig > > >( +TupleConfig4::TupleConfig4( + const Config1& cfg1, const Config2& cfg2, + const Config3& cfg3,const Config4& cfg4) : + TupleConfig > > >( cfg1, TupleConfig > >( cfg2, TupleConfig >( cfg3, TupleConfigEnd(cfg4)))) {} +template +TupleConfig4::TupleConfig4( + const TupleConfig > > >& config) : + TupleConfig > > >(config) {} + +/* ************************************************************************* */ +/** TupleConfig 5 */ template -TupleConfig5::TupleConfig5(const TupleConfig5& config) : - TupleConfig > > > >(config) {} +TupleConfig5::TupleConfig5( + const TupleConfig5& config) : + TupleConfig > > > >(config) {} template -TupleConfig5::TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, +TupleConfig5::TupleConfig5( + const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Config4& cfg4, const Config5& cfg5) : - TupleConfig > > > >( - cfg1, TupleConfig > > >( + TupleConfig > > > >( + cfg1, TupleConfig > > >( cfg2, TupleConfig > >( cfg3, TupleConfig >( cfg4, TupleConfigEnd(cfg5))))) {} -template -TupleConfig6::TupleConfig6(const TupleConfig6& config) : - TupleConfig > > > > >(config) {} +template +TupleConfig5::TupleConfig5( + const TupleConfig > > > >& config) : + TupleConfig > > > >(config) {} -template -TupleConfig6::TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, - const Config4& cfg4, const Config5& cfg5, const Config6& cfg6) : - TupleConfig > > > > >( - cfg1, TupleConfig > > > >( - cfg2, TupleConfig > > >( - cfg3, TupleConfig > >( - cfg4, TupleConfig >( - cfg5, TupleConfigEnd(cfg6)))))) {} +/* ************************************************************************* */ +/** TupleConfig 6 */ +template +TupleConfig6::TupleConfig6( + const TupleConfig6& config) : + TupleConfig > > > > >(config) {} + +template +TupleConfig6::TupleConfig6( + const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, + const Config4& cfg4, const Config5& cfg5, const Config6& cfg6) : + TupleConfig > > > > >( + cfg1, TupleConfig > > > >( + cfg2, TupleConfig > > >( + cfg3, TupleConfig > >( + cfg4, TupleConfig >( + cfg5, TupleConfigEnd(cfg6)))))) {} + +template +TupleConfig6::TupleConfig6( + const TupleConfig > > > > >& config) : + TupleConfig > > > > >(config) {} } diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h index d4b232c2f..433d7bb8b 100644 --- a/cpp/TupleConfig.h +++ b/cpp/TupleConfig.h @@ -47,11 +47,11 @@ namespace gtsam { virtual ~TupleConfig() {} /** Print */ - void print(const std::string& s = "") const {} + void print(const std::string& s = "") const; /** Test for equality in keys and values */ bool equals(const TupleConfig& c, double tol=1e-9) const { - return first_.equals(c.first_) && second_.equals(c.second_); + return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); } // insert function that uses the second (recursive) config @@ -131,11 +131,11 @@ namespace gtsam { virtual ~TupleConfigEnd() {} /** Print */ - void print(const std::string& s = "") const {} + void print(const std::string& s = "") const; /** Test for equality in keys and values */ bool equals(const TupleConfigEnd& c, double tol=1e-9) const { - return first_.equals(c.first_); + return first_.equals(c.first_, tol); } void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } @@ -202,6 +202,7 @@ namespace gtsam { TupleConfig2() {} TupleConfig2(const TupleConfig2& config); + TupleConfig2(const TupleConfig >& config); TupleConfig2(const Config1& cfg1, const Config2& cfg2); // access functions @@ -218,8 +219,10 @@ namespace gtsam { typedef Config3 Config3_t; TupleConfig3() {} + TupleConfig3(const TupleConfig > >& config); TupleConfig3(const TupleConfig3& config); TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3); + // access functions inline const Config1_t& first() const { return this->config(); } inline const Config2_t& second() const { return this->rest().config(); } @@ -237,6 +240,7 @@ namespace gtsam { TupleConfig4() {} TupleConfig4(const TupleConfig4& config); + TupleConfig4(const TupleConfig > > >& config); TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4); // access functions @@ -258,6 +262,7 @@ namespace gtsam { TupleConfig5() {} TupleConfig5(const TupleConfig5& config); + TupleConfig5(const TupleConfig > > > >& config); TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Config4& cfg4, const Config5& cfg5); @@ -282,6 +287,7 @@ namespace gtsam { TupleConfig6() {} TupleConfig6(const TupleConfig6& config); + TupleConfig6(const TupleConfig > > > > >& config); TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Config4& cfg4, const Config5& cfg5, const Config6& cfg6); // access functions diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp index ce31b4c1a..ab789bf9c 100644 --- a/cpp/testTupleConfig.cpp +++ b/cpp/testTupleConfig.cpp @@ -324,9 +324,9 @@ TEST(TupleConfig, typedefs) { TupleConfig2 cfg1; TupleConfig3 cfg2; -// TupleConfig4 cfg3; -// TupleConfig5 cfg4; -// TupleConfig6 cfg5; + TupleConfig4 cfg3; + TupleConfig5 cfg4; + TupleConfig6 cfg5; } /* ************************************************************************* */ @@ -350,9 +350,22 @@ TEST( TupleConfig, pairconfig_style ) CHECK(assert_equal(cfg1, config.first())); CHECK(assert_equal(cfg2, config.second())); CHECK(assert_equal(cfg3, config.third())); - } +/* ************************************************************************* */ +#include "NonlinearFactorGraph-inl.h" +TEST( TupleConfig, graphs_and_factors ) +{ + typedef TupleConfig3 ConfigC; + typedef NonlinearFactorGraph GraphC; + typedef NonlinearFactor1 FactorC; + + // test creation + GraphC graph; + ConfigC config; +// FactorC::shared_ptr f1(new FactorC()); + +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */