diff --git a/cpp/TupleConfig-inl.h b/cpp/TupleConfig-inl.h index 7532bcf9e..06094e2b0 100644 --- a/cpp/TupleConfig-inl.h +++ b/cpp/TupleConfig-inl.h @@ -5,6 +5,8 @@ * Author: richard */ +#pragma once + #include "LieConfig-inl.h" #include "TupleConfig.h" @@ -21,16 +23,16 @@ namespace gtsam { template void PairConfig::print(const std::string& s) const { std::cout << "TupleConfig " << s << ", size " << size_ << "\n"; - first.print(s + "Config1: "); - second.print(s + "Config2: "); + first().print(s + "Config1: "); + second().print(s + "Config2: "); } template void PairConfig::insert(const PairConfig& config) { - for (typename Config1::const_iterator it = config.first.begin(); it!=config.first.end(); it++) { + for (typename Config1::const_iterator it = config.first().begin(); it!=config.first().end(); it++) { insert(it->first, it->second); } - for (typename Config2::const_iterator it = config.second.begin(); it!=config.second.end(); it++) { + for (typename Config2::const_iterator it = config.second().begin(); it!=config.second().end(); it++) { insert(it->first, it->second); } } diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h index 3893108dd..eb39a85b4 100644 --- a/cpp/TupleConfig.h +++ b/cpp/TupleConfig.h @@ -12,136 +12,7 @@ namespace gtsam { - /** - * PairConfig: a config that holds two data types. - */ - template - class PairConfig : public Testable > { - - public: - - // publicly available types - typedef LieConfig Config1; - typedef LieConfig Config2; - - // Two configs in the pair as in std:pair - LieConfig first; - LieConfig second; - - private: - - size_t size_; - size_t dim_; - - PairConfig(const LieConfig& config1, const LieConfig& config2) : - first(config1), second(config2), - size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {} - - public: - - /** - * Default constructor creates an empty config. - */ - PairConfig(): size_(0), dim_(0) {} - - /** - * Copy constructor - */ - PairConfig(const PairConfig& c): - first(c.first), second(c.second), size_(c.size_), dim_(c.dim_) {} - - /** - * Print - */ - void print(const std::string& s = "") const; - - /** - * Test for equality in keys and values - */ - bool equals(const PairConfig& c, double tol=1e-9) const { - return first.equals(c.first, tol) && second.equals(c.second, tol); } - - /** - * operator[] syntax to get a value by j, throws invalid_argument if - * value with specified j is not present. Will generate compile-time - * errors if j type does not match that on which the Config is templated. - */ - const X1& operator[](const J1& j) const { return first[j]; } - const X2& operator[](const J2& j) const { return second[j]; } - - /** - * size is the total number of variables in this config. - */ - size_t size() const { return size_; } - - /** - * dim is the dimensionality of the tangent space - */ - size_t dim() const { return dim_; } - - private: - template - void insert_helper(Config& config, const Key& j, const Value& value) { - config.insert(j, value); - size_ ++; - dim_ += gtsam::dim(value); - } - - template - void erase_helper(Config& config, const Key& j) { - size_t dim; - config.erase(j, dim); - dim_ -= dim; - size_ --; - } - - public: - /** - * expmap each element - */ - PairConfig expmap(const VectorConfig& delta) const { - return PairConfig(gtsam::expmap(first, delta), gtsam::expmap(second, delta)); } - - /** - * logmap each element - */ - VectorConfig logmap(const PairConfig& cp) const { - VectorConfig ret(gtsam::logmap(first, cp.first)); - ret.insert(gtsam::logmap(second, cp.second)); - return ret; - } - - /** - * Insert a variable with the given j - */ - void insert(const J1& j, const X1& value) { insert_helper(first, j, value); } - void insert(const J2& j, const X2& value) { insert_helper(second, j, value); } - - void insert(const PairConfig& config); - - /** - * Remove the variable with the given j. Throws invalid_argument if the - * j is not present in the config. - */ - void erase(const J1& j) { erase_helper(first, j); } - void erase(const J2& j) { erase_helper(second, j); } - - /** - * Check if a variable exists - */ - bool exists(const J1& j) const { return first.exists(j); } - bool exists(const J2& j) const { return second.exists(j); } - - - }; - - template - inline PairConfig expmap(const PairConfig c, const VectorConfig& delta) { return c.expmap(delta); } - - template - inline VectorConfig logmap(const PairConfig c0, const PairConfig& cp) { return c0.logmap(cp); } - - /** +/** * Tuple configs to handle subconfigs of LieConfigs * * This uses a recursive structure of config pairs to form a lisp-like @@ -319,6 +190,8 @@ namespace gtsam { TupleConfig2() {} TupleConfig2(const TupleConfig2& config) : TupleConfig >(config) {} + TupleConfig2(const Config1& cfg1, const Config2& cfg2) : + TupleConfig >(cfg1, TupleConfigEnd(cfg2)) {} }; template @@ -326,6 +199,8 @@ namespace gtsam { TupleConfig3() {} TupleConfig3(const TupleConfig3& config) : TupleConfig > >(config) {} + TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) : + TupleConfig > >(cfg1, TupleConfig >(cfg2, TupleConfigEnd(cfg3))) {} }; template @@ -348,4 +223,153 @@ namespace gtsam { TupleConfig6(const TupleConfig6& config) : TupleConfig > > > > >(config) {} }; + + /** + * PairConfig: an alias for a pair of configs using TupleConfig2 + * STILL IN TESTING - will soon replace PairConfig + */ +// template +// struct PairConfig : TupleConfig2, LieConfig > { +// PairConfig() {} +// PairConfig(const PairConfig& config) : +// TupleConfig2, LieConfig >(config) {} +// PairConfig(const LieConfig& cfg1,const LieConfig& cfg2) : +// TupleConfig2, LieConfig >(cfg1, cfg2) {} +// }; + + /** + * PairConfig: a config that holds two data types. + */ + template + class PairConfig : public Testable > { + + public: + + // publicly available types + typedef LieConfig Config1; + typedef LieConfig Config2; + + protected: + + // Two configs in the pair as in std:pair + LieConfig first_; + LieConfig second_; + + private: + + size_t size_; + size_t dim_; + + PairConfig(const LieConfig& config1, const LieConfig& config2) : + first_(config1), second_(config2), + size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {} + + public: + + /** + * Default constructor creates an empty config. + */ + PairConfig(): size_(0), dim_(0) {} + + /** + * Copy constructor + */ + PairConfig(const PairConfig& c): + first_(c.first_), second_(c.second_), size_(c.size_), dim_(c.dim_) {} + + /** + * Print + */ + void print(const std::string& s = "") const; + + /** + * Test for equality in keys and values + */ + bool equals(const PairConfig& c, double tol=1e-9) const { + return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); } + + /** Direct access functions */ + inline const Config1& first() const { return first_; } + inline const Config2& second() const { return second_; } + + /** + * operator[] syntax to get a value by j, throws invalid_argument if + * value with specified j is not present. Will generate compile-time + * errors if j type does not match that on which the Config is templated. + */ + const X1& operator[](const J1& j) const { return first_[j]; } + const X2& operator[](const J2& j) const { return second_[j]; } + + /** + * size is the total number of variables in this config. + */ + size_t size() const { return size_; } + + /** + * dim is the dimensionality of the tangent space + */ + size_t dim() const { return dim_; } + + private: + template + void insert_helper(Config& config, const Key& j, const Value& value) { + config.insert(j, value); + size_ ++; + dim_ += gtsam::dim(value); + } + + template + void erase_helper(Config& config, const Key& j) { + size_t dim; + config.erase(j, dim); + dim_ -= dim; + size_ --; + } + + public: + /** + * expmap each element + */ + PairConfig expmap(const VectorConfig& delta) const { + return PairConfig(gtsam::expmap(first_, delta), gtsam::expmap(second_, delta)); } + + /** + * logmap each element + */ + VectorConfig logmap(const PairConfig& cp) const { + VectorConfig ret(gtsam::logmap(first_, cp.first_)); + ret.insert(gtsam::logmap(second_, cp.second_)); + return ret; + } + + /** + * Insert a variable with the given j + */ + void insert(const J1& j, const X1& value) { insert_helper(first_, j, value); } + void insert(const J2& j, const X2& value) { insert_helper(second_, j, value); } + + void insert(const PairConfig& config); + + /** + * Remove the variable with the given j. Throws invalid_argument if the + * j is not present in the config. + */ + void erase(const J1& j) { erase_helper(first_, j); } + void erase(const J2& j) { erase_helper(second_, j); } + + /** + * Check if a variable exists + */ + bool exists(const J1& j) const { return first_.exists(j); } + bool exists(const J2& j) const { return second_.exists(j); } + + + }; + + template + inline PairConfig expmap(const PairConfig c, const VectorConfig& delta) { return c.expmap(delta); } + + template + inline VectorConfig logmap(const PairConfig c0, const PairConfig& cp) { return c0.logmap(cp); } + } diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp index 82fd76354..d033d56bf 100644 --- a/cpp/testTupleConfig.cpp +++ b/cpp/testTupleConfig.cpp @@ -328,6 +328,14 @@ TEST(TupleConfig, typedefs) TupleConfig6 cfg5; } +/* ************************************************************************* */ +TEST( TupleConfig, constructor_insert ) +{ + PoseConfig cfg1; + PointConfig cfg2; + LamConfig cfg3; + TupleConfig3 config(cfg1, cfg2, cfg3); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }