From a2fecf51c9f182d1ab07538f7a601a716523cde7 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 14 Aug 2010 21:15:45 +0000 Subject: [PATCH] FusionTupleConfig is now a full config --- nonlinear/FusionTupleConfig.h | 200 +++++++++++++++++++++--- tests/testFusionTupleConfig.cpp | 269 +++++++++++++++++++++++++++++++- tests/testTupleConfig.cpp | 1 + 3 files changed, 444 insertions(+), 26 deletions(-) diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h index e96162200..6d22dd294 100644 --- a/nonlinear/FusionTupleConfig.h +++ b/nonlinear/FusionTupleConfig.h @@ -62,16 +62,61 @@ public: /** insertion */ template void insert(const Key& j, const Value& x) { - boost::fusion::at_key >(base_tuple_).insert(j,x); + config_ >().insert(j,x); + } + + /** insert a full config at a time */ + template + void insertSub(const Config& config) { + config_().insert(config); + } + + /** + * Update function for whole configs - this will change existing values + * @param config is a config to add + */ + void update(const FusionTupleConfig& config) { + base_tuple_ = boost::fusion::accumulate( + config.base_tuple_, base_tuple_, + FusionTupleConfig::update_helper()); + } + + /** + * Update function for whole subconfigs - this will change existing values + * @param config is a config to add + */ + template + void update(const Config& config) { + config_().update(config); + } + + /** + * Update function for single key/value pairs - will change existing values + * @param key is the variable identifier + * @param value is the variable value to update + */ + template + void update(const Key& key, const Value& value) { + config_ >().update(key,value); + } + + /** check if a given element exists */ + template + bool exists(const Key& j) const { + return config >().exists(j); } /** retrieve a point */ template const typename Key::Value_t & at(const Key& j) const { - return boost::fusion::at_key >(base_tuple_).at(j); + return config >().at(j); } - /** retrieve a full config */ + /** access operator */ + template + const typename Key::Value_t & operator[](const Key& j) const { return at(j); } + + /** retrieve a reference to a full subconfig */ template const Config & config() const { return boost::fusion::at_key(base_tuple_); @@ -79,7 +124,14 @@ public: /** size of the config - sum all sizes from subconfigs */ size_t size() const { - return boost::fusion::accumulate(base_tuple_, 0, FusionTupleConfig::size_helper()); + return boost::fusion::accumulate(base_tuple_, 0, + FusionTupleConfig::size_helper()); + } + + /** combined dimension of the subconfigs */ + size_t dim() const { + return boost::fusion::accumulate(base_tuple_, 0, + FusionTupleConfig::dim_helper()); } /** number of configs in the config */ @@ -87,9 +139,21 @@ public: return boost::fusion::size(base_tuple_); } + /** erases a specific key */ + template + void erase(const Key& j) { + config_ >().erase(j); + } + + /** clears the config */ + void clear() { + base_tuple_ = Configs(); + } + /** returns true if the config is empty */ bool empty() const { - return boost::fusion::all(base_tuple_, FusionTupleConfig::empty_helper()); + return boost::fusion::all(base_tuple_, + FusionTupleConfig::empty_helper()); } /** print */ @@ -108,48 +172,124 @@ public: helper); } - /** direct access to the underlying fusion set - don't use this */ + /** zero: create VectorConfig of appropriate structure */ + VectorConfig zero() const { + return boost::fusion::accumulate(base_tuple_, VectorConfig(), + FusionTupleConfig::zero_helper()); + } + + FusionTupleConfig expmap(const VectorConfig& delta) const { + return boost::fusion::accumulate(base_tuple_, base_tuple_, + FusionTupleConfig::expmap_helper(delta)); + } + + VectorConfig logmap(const FusionTupleConfig& cp) const { + return boost::fusion::accumulate(boost::fusion::zip(base_tuple_, cp.base_tuple_), + VectorConfig(), FusionTupleConfig::logmap_helper()); + } + + /** + * direct access to the underlying fusion set - don't use this normally + * TODO: make this a friend function or similar + */ const BaseTuple & base_tuple() const { return base_tuple_; } private: - /** helper structs to make use of fusion algorithms */ - struct size_helper - { - typedef size_t result_type; + /** retrieve a non-const reference to a full subconfig */ + template + Config & config_() { + return boost::fusion::at_key(base_tuple_); + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(base_tuple_); + } + +private: + + /** helper structs to make use of fusion algorithms */ + struct size_helper { + typedef size_t result_type; template - size_t operator()(const T& t, const size_t& s) const - { + size_t operator()(const T& t, const size_t& s) const { return s + t.size(); } }; - struct empty_helper - { + struct dim_helper { + typedef size_t result_type; template - bool operator()(T t) const - { + size_t operator()(const T& t, const size_t& s) const { + return s + t.dim(); + } + }; + + struct zero_helper { + typedef VectorConfig result_type; + template + result_type operator()(const T& t, const result_type& s) const { + result_type new_s(s); + new_s.insert(t.zero()); + return new_s; + } + }; + + struct update_helper { + typedef Configs result_type; + template + result_type operator()(const T& t, const result_type& s) const { + result_type new_s(s); + boost::fusion::at_key(new_s).update(t); + return new_s; + } + }; + + struct expmap_helper { + typedef FusionTupleConfig result_type; + VectorConfig delta; + expmap_helper(const VectorConfig& d) : delta(d) {} + template + result_type operator()(const T& t, const result_type& s) const { + result_type new_s(s); + boost::fusion::at_key(new_s.base_tuple_) = T(gtsam::expmap(t, delta)); + return new_s; + } + }; + + struct logmap_helper { + typedef VectorConfig result_type; + template + result_type operator()(const T& t, const result_type& s) const { + result_type new_s(s); + new_s.insert(gtsam::logmap(boost::fusion::at_c<0>(t), boost::fusion::at_c<1>(t))); + return new_s; + } + }; + + struct empty_helper { + template + bool operator()(T t) const { return t.empty(); } }; - struct print_helper - { + struct print_helper { template - void operator()(T t) const - { + void operator()(T t) const { t.print(); } }; - struct equals_helper - { + struct equals_helper { double tol; equals_helper(double t) : tol(t) {} template - bool operator()(T t) const - { + bool operator()(T t) const { return boost::fusion::at_c<0>(t).equals(boost::fusion::at_c<1>(t), tol); } }; @@ -183,6 +323,18 @@ private: }; }; +/** Exmap static functions */ +template +inline FusionTupleConfig expmap(const FusionTupleConfig& c, const VectorConfig& delta) { + return c.expmap(delta); +} + +/** logmap static functions */ +template +inline VectorConfig logmap(const FusionTupleConfig& c0, const FusionTupleConfig& cp) { + return c0.logmap(cp); +} + } // \namespace gtsam diff --git a/tests/testFusionTupleConfig.cpp b/tests/testFusionTupleConfig.cpp index 789c25ecb..83303b7ae 100644 --- a/tests/testFusionTupleConfig.cpp +++ b/tests/testFusionTupleConfig.cpp @@ -7,13 +7,18 @@ #include +#define GTSAM_MAGIC_KEY + #include #include #include #include #include -#include +#include +#include +#include +#include #include using namespace boost; @@ -38,7 +43,7 @@ typedef FusionTupleConfig > TupPoseConfig; typedef FusionTupleConfig > TupPairConfig; /* ************************************************************************* */ -TEST( testFusionTupleConfig, basic_config ) { +TEST( testFusionTupleConfig, basic_config_operations ) { // some initialized configs to start with PoseConfig poseConfig1; @@ -86,6 +91,12 @@ TEST( testFusionTupleConfig, basic_config ) { cfg1.insert(l2, l2_val); EXPECT(cfg1.size() == 4); + // exists + EXPECT(cfg1.exists(x1)); + EXPECT(cfg1.exists(x2)); + EXPECT(cfg1.exists(l1)); + EXPECT(cfg1.exists(l2)); + // retrieval of elements EXPECT(assert_equal(l1_val, cfg1A.at(l1))); EXPECT(assert_equal(l2_val, cfg1A.at(l2))); @@ -117,6 +128,34 @@ TEST( testFusionTupleConfig, basic_config ) { EXPECT(cfg1A.nrConfigs() == 1); EXPECT(cfg1B.nrConfigs() == 1); EXPECT(cfg1.nrConfigs() == 2); + + // erase + cfg1.erase(x1); + EXPECT(cfg1.size() == 3); + EXPECT(!cfg1.exists(x1)); + cfg1.erase(l1); + EXPECT(cfg1.size() == 2); + EXPECT(!cfg1.exists(l1)); + + // clear + cfg1.clear(); + cfg1A.clear(); cfg1B.clear(); + EXPECT(cfg1.size() == 0); + EXPECT(cfg1.empty()); + EXPECT(cfg1A.size() == 0); + EXPECT(cfg1A.empty()); + EXPECT(cfg1B.size() == 0); + EXPECT(cfg1B.empty()); + + cfg2.clear(); + cfg2A.clear(); cfg2B.clear(); + EXPECT(cfg2.size() == 0); + EXPECT(cfg2.empty()); + EXPECT(cfg2A.size() == 0); + EXPECT(cfg2A.empty()); + EXPECT(cfg2B.size() == 0); + EXPECT(cfg2B.empty()); + } /* ************************************************************************* */ @@ -179,6 +218,232 @@ TEST( testFusionTupleConfig, equals ) { EXPECT(assert_equal(c5, c6, 1e-5)); } +/* ************************************************************************* */ +TEST( testFusionTupleConfig, insert_equals1 ) +{ + TupPairConfig expected; + expected.insert(PoseKey(1), x1_val); + expected.insert(PoseKey(2), x2_val); + expected.insert(PointKey(1), l1_val); + expected.insert(PointKey(2), l2_val); + + TupPairConfig actual; + actual.insert(PoseKey(1), x1_val); + actual.insert(PoseKey(2), x2_val); + actual.insert(PointKey(1), l1_val); + actual.insert(PointKey(2), l2_val); + + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, insert_equals2 ) +{ + TupPairConfig config1; + config1.insert(PoseKey(1), x1_val); + config1.insert(PoseKey(2), x2_val); + config1.insert(PointKey(1), l1_val); + config1.insert(PointKey(2), l2_val); + + TupPairConfig config2; + config2.insert(PoseKey(1), x1_val); + config2.insert(PoseKey(2), x2_val); + config2.insert(PointKey(1), l1_val); + + EXPECT(!config1.equals(config2)); + + config2.insert(PointKey(2), Point2(9,11)); + + EXPECT(!config1.equals(config2)); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, insert_duplicate ) +{ + TupPairConfig config1; + config1.insert(x1, x1_val); // 3 + config1.insert(x2, x2_val); // 6 + config1.insert(l1, l1_val); // 8 + config1.insert(l2, l2_val); // 10 + config1.insert(l2, l1_val); // still 10 !!!! + + EXPECT(assert_equal(l2_val, config1[PointKey(2)])); + LONGS_EQUAL(4,config1.size()); + LONGS_EQUAL(10,config1.dim()); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, size_dim ) +{ + TupPairConfig config1; + config1.insert(PoseKey(1), x1_val); + config1.insert(PoseKey(2), x2_val); + config1.insert(PointKey(1), l1_val); + config1.insert(PointKey(2), l2_val); + + EXPECT(config1.size() == 4); + EXPECT(config1.dim() == 10); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, at) +{ + TupPairConfig config1; + config1.insert(PoseKey(1), x1_val); + config1.insert(PoseKey(2), x2_val); + config1.insert(PointKey(1), l1_val); + config1.insert(PointKey(2), l2_val); + + EXPECT(assert_equal(x1_val, config1[PoseKey(1)])); + EXPECT(assert_equal(x2_val, config1[PoseKey(2)])); + EXPECT(assert_equal(l1_val, config1[PointKey(1)])); + EXPECT(assert_equal(l2_val, config1[PointKey(2)])); + + CHECK_EXCEPTION(config1[PoseKey(3)], std::invalid_argument); + CHECK_EXCEPTION(config1[PointKey(3)], std::invalid_argument); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, zero_expmap_logmap) +{ + Pose2 xA1(1,2,3), xA2(6,7,8); + Point2 lA1(4,5), lA2(9,10); + + TupPairConfig config1; + config1.insert(PoseKey(1), xA1); + config1.insert(PoseKey(2), xA2); + config1.insert(PointKey(1), lA1); + config1.insert(PointKey(2), lA2); + + VectorConfig expected_zero; + expected_zero.insert("x1", zero(3)); + expected_zero.insert("x2", zero(3)); + expected_zero.insert("l1", zero(2)); + expected_zero.insert("l2", zero(2)); + + EXPECT(assert_equal(expected_zero, config1.zero())); + + VectorConfig delta; + delta.insert("x1", Vector_(3, 1.0, 1.1, 1.2)); + delta.insert("x2", Vector_(3, 1.3, 1.4, 1.5)); + delta.insert("l1", Vector_(2, 1.0, 1.1)); + delta.insert("l2", Vector_(2, 1.3, 1.4)); + + TupPairConfig expected; + expected.insert(PoseKey(1), expmap(xA1, Vector_(3, 1.0, 1.1, 1.2))); + expected.insert(PoseKey(2), expmap(xA2, Vector_(3, 1.3, 1.4, 1.5))); + expected.insert(PointKey(1), Point2(5.0, 6.1)); + expected.insert(PointKey(2), Point2(10.3, 11.4)); + + TupPairConfig actual = expmap(config1, delta); + EXPECT(assert_equal(expected, actual)); + + // Check log + VectorConfig expected_log = delta; + VectorConfig actual_log = logmap(config1,actual); + EXPECT(assert_equal(expected_log, actual_log)); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, partial_insert) { + TupPairConfig init, expected; + + Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); + Point2 point1(2.0, 3.0), point2(5.0, 6.0); + + init.insert(x1, pose1); + init.insert(l1, point1); + + PoseConfig cfg1; + cfg1.insert(x2, pose2); + + init.insertSub(cfg1); + + expected.insert(x1, pose1); + expected.insert(l1, point1); + expected.insert(x2, pose2); + + CHECK(assert_equal(expected, init)); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, update) { + TupPairConfig init, superset, expected; + + Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); + Point2 point1(2.0, 3.0), point2(5.0, 6.0); + + init.insert(x1, pose1); + init.insert(l1, point1); + + Pose2 pose1_(1.0, 2.0, 0.4); + Point2 point1_(2.0, 4.0); + superset.insert(x1, pose1_); + superset.insert(l1, point1_); + superset.insert(x2, pose2); + superset.insert(l2, point2); + init.update(superset); + + expected.insert(x1, pose1_); + expected.insert(l1, point1_); + + CHECK(assert_equal(expected, init)); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, update_element ) +{ + TupPairConfig cfg; + PoseKey xk(1); + PointKey lk(1); + + cfg.insert(xk, x1_val); + EXPECT(cfg.size() == 1); + EXPECT(assert_equal(x1_val, cfg.at(xk))); + + cfg.update(xk, x2_val); + EXPECT(cfg.size() == 1); + EXPECT(assert_equal(x2_val, cfg.at(xk))); + + cfg.insert(lk, l1_val); + EXPECT(cfg.size() == 2); + EXPECT(assert_equal(l1_val, cfg.at(lk))); + + cfg.update(lk, l2_val); + EXPECT(cfg.size() == 2); + EXPECT(assert_equal(l2_val, cfg.at(lk))); +} + +/* ************************************************************************* */ +TEST( testFusionTupleConfig, expmap) +{ + Pose2 xA1(1,2,3), xA2(6,7,8); + PoseKey x1k(1), x2k(2); + Point2 lA1(4,5), lA2(9,10); + PointKey l1k(1), l2k(2); + + TupPairConfig config1; + config1.insert(x1k, xA1); + config1.insert(x2k, xA2); + config1.insert(l1k, lA1); + config1.insert(l2k, lA2); + + VectorConfig delta; + delta.insert("x1", Vector_(3, 1.0, 1.1, 1.2)); + delta.insert("x2", Vector_(3, 1.3, 1.4, 1.5)); + delta.insert("l1", Vector_(2, 1.0, 1.1)); + delta.insert("l2", Vector_(2, 1.3, 1.4)); + + TupPairConfig expected; + expected.insert(x1k, expmap(xA1, Vector_(3, 1.0, 1.1, 1.2))); + expected.insert(x2k, expmap(xA2, Vector_(3, 1.3, 1.4, 1.5))); + expected.insert(l1k, Point2(5.0, 6.1)); + expected.insert(l2k, Point2(10.3, 11.4)); + + CHECK(assert_equal(expected, expmap(config1, delta))); + CHECK(assert_equal(delta, logmap(config1, expected))); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testTupleConfig.cpp b/tests/testTupleConfig.cpp index 4f3e7176e..eb77a6599 100644 --- a/tests/testTupleConfig.cpp +++ b/tests/testTupleConfig.cpp @@ -73,6 +73,7 @@ TEST( TupleConfig, insert_equals1 ) CHECK(assert_equal(expected,actual)); } +/* ************************************************************************* */ TEST( TupleConfig, insert_equals2 ) { Pose2 x1(1,2,3), x2(6,7,8);