diff --git a/examples/PlanarSLAMSelfContained.cpp b/examples/PlanarSLAMSelfContained.cpp index 9543f0a71..8e827128e 100644 --- a/examples/PlanarSLAMSelfContained.cpp +++ b/examples/PlanarSLAMSelfContained.cpp @@ -24,16 +24,16 @@ #include // implementations for structures - needed if self-contained, and these should be included last -#include +#include #include #include // Main typedefs typedef gtsam::TypedSymbol PoseKey; // Key for poses, with type included typedef gtsam::TypedSymbol PointKey; // Key for points, with type included -typedef gtsam::LieConfig PoseConfig; // config type for poses -typedef gtsam::LieConfig PointConfig; // config type for points -typedef gtsam::TupleConfig2 Config; // main config with two variable classes +typedef gtsam::LieValues PoseConfig; // config type for poses +typedef gtsam::LieValues PointConfig; // config type for points +typedef gtsam::TupleValues2 Config; // main config with two variable classes typedef gtsam::NonlinearFactorGraph Graph; // graph structure typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 16611f21f..7595bfc39 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include @@ -31,7 +31,7 @@ using namespace std; using namespace gtsam; typedef TypedSymbol Key; -typedef LieConfig Config; +typedef LieValues Config; typedef NonlinearFactorGraph Graph; typedef Factorization Solver; typedef NonlinearOptimizer Optimizer; diff --git a/nonlinear/Key.h b/nonlinear/Key.h index 1a403d8a6..88e75fa0a 100644 --- a/nonlinear/Key.h +++ b/nonlinear/Key.h @@ -1,5 +1,5 @@ -/* - * Key.h +/** + * @file Key.h * * Created on: Jan 12, 2010 * @author: Frank Dellaert @@ -11,7 +11,6 @@ #include #include #include -//#include #include #ifdef GTSAM_MAGIC_KEY #include diff --git a/nonlinear/LieConfig-inl.h b/nonlinear/LieValues-inl.h similarity index 78% rename from nonlinear/LieConfig-inl.h rename to nonlinear/LieValues-inl.h index 5523e2e73..0d54a0d55 100644 --- a/nonlinear/LieConfig-inl.h +++ b/nonlinear/LieValues-inl.h @@ -1,5 +1,5 @@ /** - * @file LieConfig.cpp + * @file LieValues.cpp * @author Richard Roberts */ @@ -16,14 +16,14 @@ #include #include -#include +#include #define INSTANTIATE_LIE_CONFIG(J) \ /*INSTANTIATE_LIE(T);*/ \ - /*template LieConfig expmap(const LieConfig&, const VectorConfig&);*/ \ - /*template LieConfig expmap(const LieConfig&, const Vector&);*/ \ - /*template VectorConfig logmap(const LieConfig&, const LieConfig&);*/ \ - template class LieConfig; + /*template LieValues expmap(const LieValues&, const VectorConfig&);*/ \ + /*template LieValues expmap(const LieValues&, const Vector&);*/ \ + /*template VectorConfig logmap(const LieValues&, const LieValues&);*/ \ + template class LieValues; using namespace std; @@ -31,8 +31,8 @@ namespace gtsam { /* ************************************************************************* */ template - void LieConfig::print(const string &s) const { - cout << "LieConfig " << s << ", size " << values_.size() << "\n"; + void LieValues::print(const string &s) const { + cout << "LieValues " << s << ", size " << values_.size() << "\n"; BOOST_FOREACH(const KeyValuePair& v, values_) { gtsam::print(v.second, (string)(v.first)); } @@ -40,7 +40,7 @@ namespace gtsam { /* ************************************************************************* */ template - bool LieConfig::equals(const LieConfig& expected, double tol) const { + bool LieValues::equals(const LieValues& expected, double tol) const { if (values_.size() != expected.values_.size()) return false; BOOST_FOREACH(const KeyValuePair& v, values_) { if (!expected.exists(v.first)) return false; @@ -52,7 +52,7 @@ namespace gtsam { /* ************************************************************************* */ template - const typename J::Value_t& LieConfig::at(const J& j) const { + const typename J::Value_t& LieValues::at(const J& j) const { const_iterator it = values_.find(j); if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); else return it->second; @@ -60,7 +60,7 @@ namespace gtsam { /* ************************************************************************* */ template - size_t LieConfig::dim() const { + size_t LieValues::dim() const { size_t n = 0; BOOST_FOREACH(const KeyValuePair& value, values_) n += value.second.dim(); @@ -69,7 +69,7 @@ namespace gtsam { /* ************************************************************************* */ template - VectorConfig LieConfig::zero(const Ordering& ordering) const { + VectorConfig LieValues::zero(const Ordering& ordering) const { VectorConfig z(this->dims(ordering)); z.makeZero(); return z; @@ -77,20 +77,20 @@ namespace gtsam { /* ************************************************************************* */ template - void LieConfig::insert(const J& name, const typename J::Value_t& val) { + void LieValues::insert(const J& name, const typename J::Value_t& val) { values_.insert(make_pair(name, val)); } /* ************************************************************************* */ template - void LieConfig::insert(const LieConfig& cfg) { + void LieValues::insert(const LieValues& cfg) { BOOST_FOREACH(const KeyValuePair& v, cfg.values_) insert(v.first, v.second); } /* ************************************************************************* */ template - void LieConfig::update(const LieConfig& cfg) { + void LieValues::update(const LieValues& cfg) { BOOST_FOREACH(const KeyValuePair& v, values_) { boost::optional t = cfg.exists_(v.first); if (t) values_[v.first] = *t; @@ -99,13 +99,13 @@ namespace gtsam { /* ************************************************************************* */ template - void LieConfig::update(const J& j, const typename J::Value_t& val) { + void LieValues::update(const J& j, const typename J::Value_t& val) { values_[j] = val; } /* ************************************************************************* */ template - std::list LieConfig::keys() const { + std::list LieValues::keys() const { std::list ret; BOOST_FOREACH(const KeyValuePair& v, values_) ret.push_back(v.first); @@ -114,14 +114,14 @@ namespace gtsam { /* ************************************************************************* */ template - void LieConfig::erase(const J& j) { + void LieValues::erase(const J& j) { size_t dim; // unused erase(j, dim); } /* ************************************************************************* */ template - void LieConfig::erase(const J& j, size_t& dim) { + void LieValues::erase(const J& j, size_t& dim) { iterator it = values_.find(j); if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); dim = it->second.dim(); @@ -131,8 +131,8 @@ namespace gtsam { /* ************************************************************************* */ // todo: insert for every element is inefficient template - LieConfig LieConfig::expmap(const VectorConfig& delta, const Ordering& ordering) const { - LieConfig newConfig; + LieValues LieValues::expmap(const VectorConfig& delta, const Ordering& ordering) const { + LieValues newConfig; typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, this->values_) { const J& j = value.first; @@ -145,7 +145,7 @@ namespace gtsam { /* ************************************************************************* */ template - std::vector LieConfig::dims(const Ordering& ordering) const { + std::vector LieValues::dims(const Ordering& ordering) const { _ConfigDimensionCollector dimCollector(ordering); this->apply(dimCollector); return dimCollector.dimensions; @@ -153,7 +153,7 @@ namespace gtsam { /* ************************************************************************* */ template - Ordering::shared_ptr LieConfig::orderingArbitrary(varid_t firstVar) const { + Ordering::shared_ptr LieValues::orderingArbitrary(varid_t firstVar) const { // Generate an initial key ordering in iterator order _ConfigKeyOrderer keyOrderer(firstVar); this->apply(keyOrderer); @@ -163,12 +163,12 @@ namespace gtsam { // /* ************************************************************************* */ // // todo: insert for every element is inefficient // template -// LieConfig LieConfig::expmap(const Vector& delta) const { +// LieValues LieValues::expmap(const Vector& delta) const { // if(delta.size() != dim()) { -// cout << "LieConfig::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; +// cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // throw invalid_argument("Delta vector length does not match config dimensionality."); // } -// LieConfig newConfig; +// LieValues newConfig; // int delta_offset = 0; // typedef pair KeyValue; // BOOST_FOREACH(const KeyValue& value, this->values_) { @@ -185,7 +185,7 @@ namespace gtsam { // todo: insert for every element is inefficient // todo: currently only logmaps elements in both configs template - inline VectorConfig LieConfig::logmap(const LieConfig& cp, const Ordering& ordering) const { + inline VectorConfig LieValues::logmap(const LieValues& cp, const Ordering& ordering) const { VectorConfig delta(this->dims(ordering)); logmap(cp, ordering, delta); return delta; @@ -193,7 +193,7 @@ namespace gtsam { /* ************************************************************************* */ template - void LieConfig::logmap(const LieConfig& cp, const Ordering& ordering, VectorConfig& delta) const { + void LieValues::logmap(const LieValues& cp, const Ordering& ordering, VectorConfig& delta) const { typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, cp) { assert(this->exists(value.first)); diff --git a/nonlinear/LieConfig.h b/nonlinear/LieValues.h similarity index 89% rename from nonlinear/LieConfig.h rename to nonlinear/LieValues.h index 7e0b57c75..48ca2452a 100644 --- a/nonlinear/LieConfig.h +++ b/nonlinear/LieValues.h @@ -1,12 +1,12 @@ /** - * @file LieConfig.h + * @file LieValues.h * @Author: Richard Roberts * * @brief A templated config for Lie-group elements * * Detailed story: * A configuration is a map from keys to values. It is used to specify the value of a bunch - * of variables in a factor graph. A LieConfig is a configuration which can hold variables that + * of variables in a factor graph. A LieValues is a configuration which can hold variables that * are elements of Lie groups, not just vectors. It then, as a whole, implements a aggregate type * which is also a Lie group, and hence supports operations dim, expmap, and logmap. */ @@ -18,8 +18,6 @@ #include -//#include - #include #include #include @@ -40,7 +38,7 @@ namespace gtsam { * labels (example: Pose2, Point2, etc) */ template - class LieConfig : public Testable > { + class LieValues : public Testable > { public: @@ -60,18 +58,18 @@ namespace gtsam { public: - LieConfig() {} - LieConfig(const LieConfig& config) : + LieValues() {} + LieValues(const LieValues& config) : values_(config.values_) {} template - LieConfig(const LieConfig& other) {} // do nothing when initializing with wrong type - virtual ~LieConfig() {} + LieValues(const LieValues& other) {} // do nothing when initializing with wrong type + virtual ~LieValues() {} /** print */ void print(const std::string &s="") const; /** Test whether configs are identical in keys and values */ - bool equals(const LieConfig& expected, double tol=1e-9) const; + bool equals(const LieValues& expected, double tol=1e-9) const; /** Retrieve a variable by j, throws std::invalid_argument if not found */ const Value& at(const J& j) const; @@ -108,16 +106,16 @@ namespace gtsam { // Lie operations /** Add a delta config to current config and returns a new config */ - LieConfig expmap(const VectorConfig& delta, const Ordering& ordering) const; + LieValues expmap(const VectorConfig& delta, const Ordering& ordering) const; // /** Add a delta vector to current config and returns a new config, uses iterator order */ -// LieConfig expmap(const Vector& delta) const; +// LieValues expmap(const Vector& delta) const; /** Get a delta config about a linearization point c0 (*this) */ - VectorConfig logmap(const LieConfig& cp, const Ordering& ordering) const; + VectorConfig logmap(const LieValues& cp, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - void logmap(const LieConfig& cp, const Ordering& ordering, VectorConfig& delta) const; + void logmap(const LieValues& cp, const Ordering& ordering, VectorConfig& delta) const; // imperative methods: @@ -125,10 +123,10 @@ namespace gtsam { void insert(const J& j, const Value& val); /** Add a set of variables - does note replace existing values */ - void insert(const LieConfig& cfg); + void insert(const LieValues& cfg); /** update the current available values without adding new ones */ - void update(const LieConfig& cfg); + void update(const LieValues& cfg); /** single element change of existing element */ void update(const J& j, const Value& val); @@ -148,7 +146,7 @@ namespace gtsam { std::list keys() const; /** Replace all keys and variables */ - LieConfig& operator=(const LieConfig& rhs) { + LieValues& operator=(const LieValues& rhs) { values_ = rhs.values_; return (*this); } diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index 0af14f879..c36a97a4e 100644 --- a/nonlinear/Makefile.am +++ b/nonlinear/Makefile.am @@ -16,8 +16,8 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # Lie Groups -headers += Key.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h -check_PROGRAMS += tests/testLieConfig +headers += Key.h LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h +check_PROGRAMS += tests/testLieValues # Nonlinear nonlinear headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h diff --git a/nonlinear/TupleConfig-inl.h b/nonlinear/TupleConfig-inl.h deleted file mode 100644 index d48ffa7f1..000000000 --- a/nonlinear/TupleConfig-inl.h +++ /dev/null @@ -1,177 +0,0 @@ -/** - * @file TupleConfig-inl.h - * @author Richard Roberts - * @author Manohar Paluri - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -// TupleConfig instantiations for N = 1-6 -#define INSTANTIATE_TUPLE_CONFIG1(Config1) \ - template class TupleConfig1; - -#define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \ - template class TupleConfig2; - -#define INSTANTIATE_TUPLE_CONFIG3(Config1, Config2, Config3) \ - template class TupleConfig3; - -#define INSTANTIATE_TUPLE_CONFIG4(Config1, Config2, Config3, Config4) \ - template class TupleConfig4; - -#define INSTANTIATE_TUPLE_CONFIG5(Config1, Config2, Config3, Config4, Config5) \ - template class TupleConfig5; - -#define INSTANTIATE_TUPLE_CONFIG6(Config1, Config2, Config3, Config4, Config5, Config6) \ - template class TupleConfig6; - - -namespace gtsam { - -/* ************************************************************************* */ -/** TupleConfigN Implementations */ -/* ************************************************************************* */ - -/* ************************************************************************* */ -/** TupleConfig 1 */ -template -TupleConfig1::TupleConfig1(const TupleConfig1& config) : - TupleConfigEnd (config) {} - -template -TupleConfig1::TupleConfig1(const Config1& cfg1) : - TupleConfigEnd (cfg1) {} - -template -TupleConfig1::TupleConfig1(const TupleConfigEnd& config) : - TupleConfigEnd(config) {} - -/* ************************************************************************* */ -/** TupleConfig 2 */ -template -TupleConfig2::TupleConfig2(const TupleConfig2& config) : - TupleConfig >(config) {} - -template -TupleConfig2::TupleConfig2(const Config1& cfg1, const Config2& cfg2) : - TupleConfig >( - cfg1, TupleConfigEnd(cfg2)) {} - -template -TupleConfig2::TupleConfig2(const TupleConfig >& config) : - TupleConfig >(config) {} - -/* ************************************************************************* */ -/** TupleConfig 3 */ -template -TupleConfig3::TupleConfig3( - const TupleConfig3& config) : - TupleConfig > >(config) {} - -template -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) {} - -template -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) {} - -template -TupleConfig5::TupleConfig5( - const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, - const Config4& cfg4, const Config5& cfg5) : - TupleConfig > > > >( - cfg1, TupleConfig > > >( - cfg2, TupleConfig > >( - cfg3, TupleConfig >( - cfg4, TupleConfigEnd(cfg5))))) {} - -template -TupleConfig5::TupleConfig5( - const TupleConfig > > > >& config) : - TupleConfig > > > >(config) {} - -/* ************************************************************************* */ -/** 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/nonlinear/TupleValues-inl.h b/nonlinear/TupleValues-inl.h new file mode 100644 index 000000000..8eea3d2e7 --- /dev/null +++ b/nonlinear/TupleValues-inl.h @@ -0,0 +1,177 @@ +/** + * @file TupleValues-inl.h + * @author Richard Roberts + * @author Manohar Paluri + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +// TupleValues instantiations for N = 1-6 +#define INSTANTIATE_TUPLE_CONFIG1(Config1) \ + template class TupleValues1; + +#define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \ + template class TupleValues2; + +#define INSTANTIATE_TUPLE_CONFIG3(Config1, Config2, Config3) \ + template class TupleValues3; + +#define INSTANTIATE_TUPLE_CONFIG4(Config1, Config2, Config3, Config4) \ + template class TupleValues4; + +#define INSTANTIATE_TUPLE_CONFIG5(Config1, Config2, Config3, Config4, Config5) \ + template class TupleValues5; + +#define INSTANTIATE_TUPLE_CONFIG6(Config1, Config2, Config3, Config4, Config5, Config6) \ + template class TupleValues6; + + +namespace gtsam { + +/* ************************************************************************* */ +/** TupleValuesN Implementations */ +/* ************************************************************************* */ + +/* ************************************************************************* */ +/** TupleValues 1 */ +template +TupleValues1::TupleValues1(const TupleValues1& config) : + TupleValuesEnd (config) {} + +template +TupleValues1::TupleValues1(const Config1& cfg1) : + TupleValuesEnd (cfg1) {} + +template +TupleValues1::TupleValues1(const TupleValuesEnd& config) : + TupleValuesEnd(config) {} + +/* ************************************************************************* */ +/** TupleValues 2 */ +template +TupleValues2::TupleValues2(const TupleValues2& config) : + TupleValues >(config) {} + +template +TupleValues2::TupleValues2(const Config1& cfg1, const Config2& cfg2) : + TupleValues >( + cfg1, TupleValuesEnd(cfg2)) {} + +template +TupleValues2::TupleValues2(const TupleValues >& config) : + TupleValues >(config) {} + +/* ************************************************************************* */ +/** TupleValues 3 */ +template +TupleValues3::TupleValues3( + const TupleValues3& config) : + TupleValues > >(config) {} + +template +TupleValues3::TupleValues3( + const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) : + TupleValues > >( + cfg1, TupleValues >( + cfg2, TupleValuesEnd(cfg3))) {} + +template +TupleValues3::TupleValues3( + const TupleValues > >& config) : + TupleValues > >(config) {} + +/* ************************************************************************* */ +/** TupleValues 4 */ +template +TupleValues4::TupleValues4( + const TupleValues4& config) : + TupleValues > > >(config) {} + +template +TupleValues4::TupleValues4( + const Config1& cfg1, const Config2& cfg2, + const Config3& cfg3,const Config4& cfg4) : + TupleValues > > >( + cfg1, TupleValues > >( + cfg2, TupleValues >( + cfg3, TupleValuesEnd(cfg4)))) {} + +template +TupleValues4::TupleValues4( + const TupleValues > > >& config) : + TupleValues > > >(config) {} + +/* ************************************************************************* */ +/** TupleValues 5 */ +template +TupleValues5::TupleValues5( + const TupleValues5& config) : + TupleValues > > > >(config) {} + +template +TupleValues5::TupleValues5( + const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, + const Config4& cfg4, const Config5& cfg5) : + TupleValues > > > >( + cfg1, TupleValues > > >( + cfg2, TupleValues > >( + cfg3, TupleValues >( + cfg4, TupleValuesEnd(cfg5))))) {} + +template +TupleValues5::TupleValues5( + const TupleValues > > > >& config) : + TupleValues > > > >(config) {} + +/* ************************************************************************* */ +/** TupleValues 6 */ +template +TupleValues6::TupleValues6( + const TupleValues6& config) : + TupleValues > > > > >(config) {} + +template +TupleValues6::TupleValues6( + const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, + const Config4& cfg4, const Config5& cfg5, const Config6& cfg6) : + TupleValues > > > > >( + cfg1, TupleValues > > > >( + cfg2, TupleValues > > >( + cfg3, TupleValues > >( + cfg4, TupleValues >( + cfg5, TupleValuesEnd(cfg6)))))) {} + +template +TupleValues6::TupleValues6( + const TupleValues > > > > >& config) : + TupleValues > > > > >(config) {} + +} diff --git a/nonlinear/TupleConfig.h b/nonlinear/TupleValues.h similarity index 75% rename from nonlinear/TupleConfig.h rename to nonlinear/TupleValues.h index cddb4423e..158f00faf 100644 --- a/nonlinear/TupleConfig.h +++ b/nonlinear/TupleValues.h @@ -1,11 +1,11 @@ /** - * @file TupleConfig.h + * @file TupleValues.h * @author Richard Roberts * @author Manohar Paluri * @author Alex Cunningham */ -#include +#include #include #pragma once @@ -13,35 +13,35 @@ namespace gtsam { /** - * TupleConfigs are a structure to manage heterogenous LieConfigs, so as to + * TupleValuess are a structure to manage heterogenous LieValuess, so as to * enable different types of variables/keys to be used simultaneously. We * do this with recursive templates (instead of blind pointer casting) to * reduce run-time overhead and keep static type checking. The interface - * mimics that of a single LieConfig. + * mimics that of a single LieValues. * * This uses a recursive structure of config pairs to form a lisp-like - * list, with a special case (TupleConfigEnd) that contains only one config + * list, with a special case (TupleValuesEnd) that contains only one config * at the end. Because this recursion is done at compile time, there is no * runtime performance hit to using this structure. * - * For an easy interface, there are TupleConfigN classes, which wrap - * the recursive TupleConfigs together as a single class, so you can have - * mixed-type classes from 2-6 types. Note that a TupleConfig2 is equivalent + * For an easy interface, there are TupleValuesN classes, which wrap + * the recursive TupleValuess together as a single class, so you can have + * mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent * to the previously used PairConfig. * * Design and extension note: * To implement a recursively templated data structure, note that most operations * have two versions: one with templates and one without. The templated one allows * for the arguments to be passed to the next config, while the specialized one - * operates on the "first" config. TupleConfigEnd contains only the specialized version. + * operates on the "first" config. TupleValuesEnd contains only the specialized version. */ template - class TupleConfig : public Testable > { + class TupleValues : public Testable > { protected: // Data for internal configs Config1 first_; /// Arbitrary config - Config2 second_; /// A TupleConfig or TupleConfigEnd, which wraps an arbitrary config + Config2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary config public: // typedefs for config subtypes @@ -49,14 +49,14 @@ namespace gtsam { typedef class Config1::Value Value1; /** default constructor */ - TupleConfig() {} + TupleValues() {} /** Copy constructor */ - TupleConfig(const TupleConfig& config) : + TupleValues(const TupleValues& config) : first_(config.first_), second_(config.second_) {} /** Construct from configs */ - TupleConfig(const Config1& cfg1, const Config2& cfg2) : + TupleValues(const Config1& cfg1, const Config2& cfg2) : first_(cfg1), second_(cfg2) {} /** Print */ @@ -66,7 +66,7 @@ namespace gtsam { } /** Equality with tolerance for keys and values */ - bool equals(const TupleConfig& c, double tol=1e-9) const { + bool equals(const TupleValues& c, double tol=1e-9) const { return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); } @@ -89,8 +89,8 @@ namespace gtsam { * @param config is a full config to add */ template - void insert(const TupleConfig& config) { second_.insert(config); } - void insert(const TupleConfig& config) { + void insert(const TupleValues& config) { second_.insert(config); } + void insert(const TupleValues& config) { first_.insert(config.first_); second_.insert(config.second_); } @@ -100,8 +100,8 @@ namespace gtsam { * @param config is a config to add */ template - void update(const TupleConfig& config) { second_.update(config); } - void update(const TupleConfig& config) { + void update(const TupleValues& config) { second_.update(config); } + void update(const TupleValues& config) { first_.update(config.first_); second_.update(config.second_); } @@ -192,19 +192,19 @@ namespace gtsam { } /** Expmap */ - TupleConfig expmap(const VectorConfig& delta, const Ordering& ordering) const { - return TupleConfig(first_.expmap(delta, ordering), second_.expmap(delta, ordering)); + TupleValues expmap(const VectorConfig& delta, const Ordering& ordering) const { + return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering)); } /** logmap each element */ - VectorConfig logmap(const TupleConfig& cp, const Ordering& ordering) const { + VectorConfig logmap(const TupleValues& cp, const Ordering& ordering) const { VectorConfig delta(this->dims(ordering)); logmap(cp, ordering, delta); return delta; } /** logmap each element */ - void logmap(const TupleConfig& cp, const Ordering& ordering, VectorConfig& delta) const { + void logmap(const TupleValues& cp, const Ordering& ordering, VectorConfig& delta) const { first_.logmap(cp.first_, ordering, delta); second_.logmap(cp.second_, ordering, delta); } @@ -237,13 +237,13 @@ namespace gtsam { }; /** - * End of a recursive TupleConfig - contains only one config + * End of a recursive TupleValues - contains only one config * * Do not use this class directly - it should only be used as a part * of a recursive structure */ template - class TupleConfigEnd : public Testable > { + class TupleValuesEnd : public Testable > { protected: // Data for internal configs @@ -254,28 +254,28 @@ namespace gtsam { typedef class Config::Key Key1; typedef class Config::Value Value1; - TupleConfigEnd() {} + TupleValuesEnd() {} - TupleConfigEnd(const TupleConfigEnd& config) : + TupleValuesEnd(const TupleValuesEnd& config) : first_(config.first_) {} - TupleConfigEnd(const Config& cfg) : + TupleValuesEnd(const Config& cfg) : first_(cfg) {} void print(const std::string& s = "") const { first_.print(); } - bool equals(const TupleConfigEnd& c, double tol=1e-9) const { + bool equals(const TupleValuesEnd& c, double tol=1e-9) const { return first_.equals(c.first_, tol); } void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} - void insert(const TupleConfigEnd& config) {first_.insert(config.first_); } + void insert(const TupleValuesEnd& config) {first_.insert(config.first_); } - void update(const TupleConfigEnd& config) {first_.update(config.first_); } + void update(const TupleValuesEnd& config) {first_.update(config.first_); } void update(const Key1& key, const Value1& value) { first_.update(key, value); } @@ -307,17 +307,17 @@ namespace gtsam { size_t dim() const { return first_.dim(); } - TupleConfigEnd expmap(const VectorConfig& delta, const Ordering& ordering) const { - return TupleConfigEnd(first_.expmap(delta, ordering)); + TupleValuesEnd expmap(const VectorConfig& delta, const Ordering& ordering) const { + return TupleValuesEnd(first_.expmap(delta, ordering)); } - VectorConfig logmap(const TupleConfigEnd& cp, const Ordering& ordering) const { + VectorConfig logmap(const TupleValuesEnd& cp, const Ordering& ordering) const { VectorConfig delta(this->dims(ordering)); logmap(cp, ordering, delta); return delta; } - void logmap(const TupleConfigEnd& cp, const Ordering& ordering, VectorConfig& delta) const { + void logmap(const TupleValuesEnd& cp, const Ordering& ordering, VectorConfig& delta) const { first_.logmap(cp.first_, ordering, delta); } @@ -345,44 +345,44 @@ namespace gtsam { /** * Wrapper classes to act as containers for configs. Note that these can be cascaded - * recursively, as they are TupleConfigs, and are primarily a short form of the config - * structure to make use of the TupleConfigs easier. + * recursively, as they are TupleValuess, and are primarily a short form of the config + * structure to make use of the TupleValuess easier. * * The interface is designed to mimic PairConfig, but for 2-6 config types. */ template - class TupleConfig1 : public TupleConfigEnd { + class TupleValues1 : public TupleValuesEnd { public: // typedefs typedef C1 Config1; - typedef TupleConfigEnd Base; - typedef TupleConfig1 This; + typedef TupleValuesEnd Base; + typedef TupleValues1 This; - TupleConfig1() {} - TupleConfig1(const This& config); - TupleConfig1(const Base& config); - TupleConfig1(const Config1& cfg1); + TupleValues1() {} + TupleValues1(const This& config); + TupleValues1(const Base& config); + TupleValues1(const Config1& cfg1); // access functions inline const Config1& first() const { return this->config(); } }; template - class TupleConfig2 : public TupleConfig > { + class TupleValues2 : public TupleValues > { public: // typedefs typedef C1 Config1; typedef C2 Config2; - typedef TupleConfig > Base; - typedef TupleConfig2 This; + typedef TupleValues > Base; + typedef TupleValues2 This; - TupleConfig2() {} - TupleConfig2(const This& config); - TupleConfig2(const Base& config); - TupleConfig2(const Config1& cfg1, const Config2& cfg2); + TupleValues2() {} + TupleValues2(const This& config); + TupleValues2(const Base& config); + TupleValues2(const Config1& cfg1, const Config2& cfg2); // access functions inline const Config1& first() const { return this->config(); } @@ -390,17 +390,17 @@ namespace gtsam { }; template - class TupleConfig3 : public TupleConfig > > { + class TupleValues3 : public TupleValues > > { public: // typedefs typedef C1 Config1; typedef C2 Config2; typedef C3 Config3; - TupleConfig3() {} - TupleConfig3(const TupleConfig > >& config); - TupleConfig3(const TupleConfig3& config); - TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3); + TupleValues3() {} + TupleValues3(const TupleValues > >& config); + TupleValues3(const TupleValues3& config); + TupleValues3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3); // access functions inline const Config1& first() const { return this->config(); } @@ -409,7 +409,7 @@ namespace gtsam { }; template - class TupleConfig4 : public TupleConfig > > > { + class TupleValues4 : public TupleValues > > > { public: // typedefs typedef C1 Config1; @@ -417,13 +417,13 @@ namespace gtsam { typedef C3 Config3; typedef C4 Config4; - typedef TupleConfig > > > Base; - typedef TupleConfig4 This; + typedef TupleValues > > > Base; + typedef TupleValues4 This; - TupleConfig4() {} - TupleConfig4(const This& config); - TupleConfig4(const Base& config); - TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4); + TupleValues4() {} + TupleValues4(const This& config); + TupleValues4(const Base& config); + TupleValues4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4); // access functions inline const Config1& first() const { return this->config(); } @@ -433,7 +433,7 @@ namespace gtsam { }; template - class TupleConfig5 : public TupleConfig > > > > { + class TupleValues5 : public TupleValues > > > > { public: // typedefs typedef C1 Config1; @@ -442,10 +442,10 @@ namespace gtsam { typedef C4 Config4; typedef C5 Config5; - TupleConfig5() {} - TupleConfig5(const TupleConfig5& config); - TupleConfig5(const TupleConfig > > > >& config); - TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, + TupleValues5() {} + TupleValues5(const TupleValues5& config); + TupleValues5(const TupleValues > > > >& config); + TupleValues5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Config4& cfg4, const Config5& cfg5); // access functions @@ -457,7 +457,7 @@ namespace gtsam { }; template - class TupleConfig6 : public TupleConfig > > > > > { + class TupleValues6 : public TupleValues > > > > > { public: // typedefs typedef C1 Config1; @@ -467,10 +467,10 @@ namespace gtsam { typedef C5 Config5; typedef C6 Config6; - TupleConfig6() {} - TupleConfig6(const TupleConfig6& config); - TupleConfig6(const TupleConfig > > > > >& config); - TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, + TupleValues6() {} + TupleValues6(const TupleValues6& config); + TupleValues6(const TupleValues > > > > >& config); + TupleValues6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Config4& cfg4, const Config5& cfg5, const Config6& cfg6); // access functions inline const Config1& first() const { return this->config(); } diff --git a/nonlinear/tests/testLieConfig.cpp b/nonlinear/tests/testLieValues.cpp similarity index 91% rename from nonlinear/tests/testLieConfig.cpp rename to nonlinear/tests/testLieValues.cpp index 84a29bce0..aa68c8813 100644 --- a/nonlinear/tests/testLieConfig.cpp +++ b/nonlinear/tests/testLieValues.cpp @@ -1,5 +1,5 @@ /** - * @file testLieConfig.cpp + * @file testLieValues.cpp * @author Richard Roberts */ @@ -9,7 +9,7 @@ #include // for operator += using namespace boost::assign; -#include +#include #include using namespace gtsam; @@ -17,12 +17,12 @@ using namespace std; static double inf = std::numeric_limits::infinity(); typedef TypedSymbol VecKey; -typedef LieConfig Config; +typedef LieValues Config; VecKey key1(1), key2(2), key3(3), key4(4); /* ************************************************************************* */ -TEST( LieConfig, equals1 ) +TEST( LieValues, equals1 ) { Config expected; Vector v = Vector_(3, 5.0, 6.0, 7.0); @@ -33,7 +33,7 @@ TEST( LieConfig, equals1 ) } /* ************************************************************************* */ -TEST( LieConfig, equals2 ) +TEST( LieValues, equals2 ) { Config cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); @@ -45,7 +45,7 @@ TEST( LieConfig, equals2 ) } /* ************************************************************************* */ -TEST( LieConfig, equals_nan ) +TEST( LieValues, equals_nan ) { Config cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); @@ -57,7 +57,7 @@ TEST( LieConfig, equals_nan ) } /* ************************************************************************* */ -TEST( LieConfig, insert_config ) +TEST( LieValues, insert_config ) { Config cfg1, cfg2, expected; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); @@ -80,7 +80,7 @@ TEST( LieConfig, insert_config ) } /* ************************************************************************* */ -TEST( LieConfig, update_element ) +TEST( LieValues, update_element ) { Config cfg; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); @@ -96,7 +96,7 @@ TEST( LieConfig, update_element ) } ///* ************************************************************************* */ -//TEST(LieConfig, dim_zero) +//TEST(LieValues, dim_zero) //{ // Config config0; // config0.insert(key1, Vector_(2, 2.0, 3.0)); @@ -110,7 +110,7 @@ TEST( LieConfig, update_element ) //} /* ************************************************************************* */ -TEST(LieConfig, expmap_a) +TEST(LieValues, expmap_a) { Config config0; config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); @@ -129,7 +129,7 @@ TEST(LieConfig, expmap_a) } ///* ************************************************************************* */ -//TEST(LieConfig, expmap_b) +//TEST(LieValues, expmap_b) //{ // Config config0; // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); @@ -147,7 +147,7 @@ TEST(LieConfig, expmap_a) //} ///* ************************************************************************* */ -//TEST(LieConfig, expmap_c) +//TEST(LieValues, expmap_c) //{ // Config config0; // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); @@ -165,7 +165,7 @@ TEST(LieConfig, expmap_a) //} /* ************************************************************************* */ -/*TEST(LieConfig, expmap_d) +/*TEST(LieValues, expmap_d) { Config config0; config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); @@ -174,7 +174,7 @@ TEST(LieConfig, expmap_a) CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); - LieConfig poseconfig; + LieValues poseconfig; poseconfig.insert("p1", Pose2(1,2,3)); poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); //poseconfig.print("poseconfig"); @@ -183,10 +183,10 @@ TEST(LieConfig, expmap_a) }*/ /* ************************************************************************* */ -/*TEST(LieConfig, extract_keys) +/*TEST(LieValues, extract_keys) { typedef TypedSymbol PoseKey; - LieConfig config; + LieValues config; config.insert(PoseKey(1), Pose2()); config.insert(PoseKey(2), Pose2()); @@ -205,7 +205,7 @@ TEST(LieConfig, expmap_a) }*/ /* ************************************************************************* */ -TEST(LieConfig, exists_) +TEST(LieValues, exists_) { Config config0; config0.insert(key1, Vector_(1, 1.)); @@ -216,7 +216,7 @@ TEST(LieConfig, exists_) } /* ************************************************************************* */ -TEST(LieConfig, update) +TEST(LieValues, update) { Config config0; config0.insert(key1, Vector_(1, 1.)); @@ -235,10 +235,10 @@ TEST(LieConfig, update) } /* ************************************************************************* */ -TEST(LieConfig, dummy_initialization) +TEST(LieValues, dummy_initialization) { typedef TypedSymbol Key; - typedef LieConfig Config1; + typedef LieValues Config1; Config1 init1; init1.insert(Key(1), Vector_(2, 1.0, 2.0)); diff --git a/slam/PriorFactor.h b/slam/PriorFactor.h index 22e393a59..d6014d5de 100644 --- a/slam/PriorFactor.h +++ b/slam/PriorFactor.h @@ -16,7 +16,7 @@ namespace gtsam { * It takes three template parameters: * T is the Lie group type for which the prior is define * Key (typically TypedSymbol) is used to look up T's in a Config - * Config where the T's are stored, typically LieConfig or a TupleConfig<...> + * Config where the T's are stored, typically LieValues or a TupleValues<...> * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * a simple type like int will not work */ diff --git a/slam/Simulated3D.cpp b/slam/Simulated3D.cpp index 8db46593d..5be9a49ff 100644 --- a/slam/Simulated3D.cpp +++ b/slam/Simulated3D.cpp @@ -5,8 +5,8 @@ **/ #include -#include -#include +#include +#include namespace gtsam { diff --git a/slam/Simulated3D.h b/slam/Simulated3D.h index 42b0f8d72..9162d8803 100644 --- a/slam/Simulated3D.h +++ b/slam/Simulated3D.h @@ -13,7 +13,7 @@ #include #include #include -#include +#include // \namespace @@ -23,9 +23,9 @@ namespace simulated3D { typedef gtsam::TypedSymbol PoseKey; typedef gtsam::TypedSymbol PointKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; - typedef TupleConfig2 Config; + typedef LieValues PoseConfig; + typedef LieValues PointConfig; + typedef TupleValues2 Config; /** * Prior on a single pose diff --git a/slam/planarSLAM.cpp b/slam/planarSLAM.cpp index 0c090132a..4eb6a702c 100644 --- a/slam/planarSLAM.cpp +++ b/slam/planarSLAM.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include // Use planarSLAM namespace for specific SLAM instance namespace gtsam { diff --git a/slam/planarSLAM.h b/slam/planarSLAM.h index 02921039f..cdf02c94a 100644 --- a/slam/planarSLAM.h +++ b/slam/planarSLAM.h @@ -7,7 +7,7 @@ #pragma once #include -#include +#include #include #include #include @@ -23,9 +23,9 @@ namespace gtsam { // Keys and Config typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; - typedef TupleConfig2 Config; + typedef LieValues PoseConfig; + typedef LieValues PointConfig; + typedef TupleValues2 Config; // Factors typedef NonlinearEquality Constraint; diff --git a/slam/pose2SLAM.cpp b/slam/pose2SLAM.cpp index 3b1d8abfc..b5c3a047a 100644 --- a/slam/pose2SLAM.cpp +++ b/slam/pose2SLAM.cpp @@ -5,7 +5,7 @@ **/ #include -#include +#include #include #include diff --git a/slam/pose2SLAM.h b/slam/pose2SLAM.h index 8588fdfd7..288805e06 100644 --- a/slam/pose2SLAM.h +++ b/slam/pose2SLAM.h @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include #include @@ -23,7 +23,7 @@ namespace gtsam { // Keys and Config typedef TypedSymbol Key; - typedef LieConfig Config; + typedef LieValues Config; /** * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) diff --git a/slam/pose3SLAM.cpp b/slam/pose3SLAM.cpp index e9ac4c626..da2590543 100644 --- a/slam/pose3SLAM.cpp +++ b/slam/pose3SLAM.cpp @@ -5,7 +5,7 @@ **/ #include -#include +#include #include #include diff --git a/slam/pose3SLAM.h b/slam/pose3SLAM.h index 35f5edf46..d3cf818d1 100644 --- a/slam/pose3SLAM.h +++ b/slam/pose3SLAM.h @@ -7,7 +7,7 @@ #pragma once #include -#include +#include #include #include #include @@ -22,7 +22,7 @@ namespace gtsam { // Keys and Config typedef TypedSymbol Key; - typedef LieConfig Config; + typedef LieValues Config; /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) diff --git a/slam/saveGraph.cpp b/slam/saveGraph.cpp index 7d57846d7..6ce97c3b4 100644 --- a/slam/saveGraph.cpp +++ b/slam/saveGraph.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include using namespace std; diff --git a/slam/saveGraph.h b/slam/saveGraph.h index e488eac7e..f6b12479e 100644 --- a/slam/saveGraph.h +++ b/slam/saveGraph.h @@ -12,12 +12,12 @@ #include #include #include -#include +#include namespace gtsam { class Point2; - typedef LieConfig SymbolicConfig; + typedef LieValues SymbolicConfig; // save graph to the graphviz format void saveGraph(const SymbolicFactorGraph& fg, const SymbolicConfig& config, const std::string& s); diff --git a/slam/simulated2D.cpp b/slam/simulated2D.cpp index cb1a4df54..85702cf67 100644 --- a/slam/simulated2D.cpp +++ b/slam/simulated2D.cpp @@ -5,8 +5,8 @@ */ #include -#include -#include +#include +#include namespace gtsam { diff --git a/slam/simulated2D.h b/slam/simulated2D.h index 39d30470b..77ad1344e 100644 --- a/slam/simulated2D.h +++ b/slam/simulated2D.h @@ -9,7 +9,7 @@ #pragma once #include -#include +#include #include // \namespace @@ -21,9 +21,9 @@ namespace gtsam { // Simulated2D robots have no orientation, just a position typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; - typedef TupleConfig2 Config; + typedef LieValues PoseConfig; + typedef LieValues PointConfig; + typedef TupleValues2 Config; /** * Prior on a single pose, and optional derivative version diff --git a/slam/simulated2DOriented.cpp b/slam/simulated2DOriented.cpp index 73fe29a3c..80d1382ce 100644 --- a/slam/simulated2DOriented.cpp +++ b/slam/simulated2DOriented.cpp @@ -5,7 +5,7 @@ */ #include -#include +#include namespace gtsam { diff --git a/slam/simulated2DOriented.h b/slam/simulated2DOriented.h index 3e63e5912..1801b5228 100644 --- a/slam/simulated2DOriented.h +++ b/slam/simulated2DOriented.h @@ -9,7 +9,7 @@ #pragma once #include -#include +#include #include // \namespace @@ -21,9 +21,9 @@ namespace gtsam { // The types that take an oriented pose2 rather than point2 typedef TypedSymbol PointKey; typedef TypedSymbol PoseKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; - typedef TupleConfig2 Config; + typedef LieValues PoseConfig; + typedef LieValues PointConfig; + typedef TupleValues2 Config; //TODO:: point prior is not implemented right now diff --git a/slam/smallExample.cpp b/slam/smallExample.cpp index 2703fd25b..a74a6e242 100644 --- a/slam/smallExample.cpp +++ b/slam/smallExample.cpp @@ -22,7 +22,7 @@ using namespace std; // template definitions #include #include -#include +#include #include namespace gtsam { diff --git a/slam/visualSLAM.cpp b/slam/visualSLAM.cpp index 70cdb5376..14004ca5f 100644 --- a/slam/visualSLAM.cpp +++ b/slam/visualSLAM.cpp @@ -6,7 +6,7 @@ */ #include -#include +#include #include #include diff --git a/slam/visualSLAM.h b/slam/visualSLAM.h index 5befdc0ef..fdbd170da 100644 --- a/slam/visualSLAM.h +++ b/slam/visualSLAM.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include namespace gtsam { namespace visualSLAM { @@ -24,9 +24,9 @@ namespace gtsam { namespace visualSLAM { */ typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; - typedef TupleConfig2 Config; + typedef LieValues PoseConfig; + typedef LieValues PointConfig; + typedef TupleValues2 Config; typedef boost::shared_ptr shared_config; typedef NonlinearEquality PoseConstraint; diff --git a/tests/Makefile.am b/tests/Makefile.am index 0b6ec283f..e7b19922a 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -18,7 +18,7 @@ check_PROGRAMS += testGaussianJunctionTree check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph check_PROGRAMS += testNonlinearOptimizer check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph -check_PROGRAMS += testTupleConfig +check_PROGRAMS += testTupleValues #check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint #check_PROGRAMS += testTransformConstraint testLinearApproxFactor diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index c99fff1e3..501a7e901 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -17,7 +17,7 @@ using namespace boost::assign; #define GTSAM_MAGIC_GAUSSIAN 3 #include -#include +#include #include #include diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 5cda14cbc..e64ca8468 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -11,13 +11,13 @@ #include #include -#include +#include using namespace std; using namespace gtsam; typedef TypedSymbol PoseKey; -typedef LieConfig PoseConfig; +typedef LieValues PoseConfig; typedef PriorFactor PosePrior; typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; diff --git a/tests/testTransformConstraint.cpp b/tests/testTransformConstraint.cpp index 4b4d20346..180d42582 100644 --- a/tests/testTransformConstraint.cpp +++ b/tests/testTransformConstraint.cpp @@ -18,8 +18,8 @@ #include // implementations -#include -#include +#include +#include #include #include @@ -31,11 +31,11 @@ typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; typedef TypedSymbol TransformKey; -typedef LieConfig PoseConfig; -typedef LieConfig PointConfig; -typedef LieConfig TransformConfig; +typedef LieValues PoseConfig; +typedef LieValues PointConfig; +typedef LieValues TransformConfig; -typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig; +typedef TupleValues3< PoseConfig, PointConfig, TransformConfig > DDFConfig; typedef NonlinearFactorGraph DDFGraph; typedef NonlinearOptimizer Optimizer; diff --git a/tests/testTupleConfig.cpp b/tests/testTupleValues.cpp similarity index 86% rename from tests/testTupleConfig.cpp rename to tests/testTupleValues.cpp index cbbd07e71..f19395f36 100644 --- a/tests/testTupleConfig.cpp +++ b/tests/testTupleValues.cpp @@ -1,5 +1,5 @@ /** - * @file testTupleConfig.cpp + * @file testTupleValues.cpp * @author Richard Roberts * @author Alex Cunningham */ @@ -18,7 +18,7 @@ #include #include -#include +#include using namespace gtsam; using namespace std; @@ -27,12 +27,12 @@ static const double tol = 1e-5; typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; -typedef LieConfig PoseConfig; -typedef LieConfig PointConfig; -typedef TupleConfig2 Config; +typedef LieValues PoseConfig; +typedef LieValues PointConfig; +typedef TupleValues2 Config; /* ************************************************************************* */ -TEST( TupleConfig, constructors ) +TEST( TupleValues, constructors ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -54,7 +54,7 @@ TEST( TupleConfig, constructors ) } /* ************************************************************************* */ -TEST( TupleConfig, insert_equals1 ) +TEST( TupleValues, insert_equals1 ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -75,7 +75,7 @@ TEST( TupleConfig, insert_equals1 ) } /* ************************************************************************* */ -TEST( TupleConfig, insert_equals2 ) +TEST( TupleValues, insert_equals2 ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -99,7 +99,7 @@ TEST( TupleConfig, insert_equals2 ) } /* ************************************************************************* */ -TEST( TupleConfig, insert_duplicate ) +TEST( TupleValues, insert_duplicate ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -117,7 +117,7 @@ TEST( TupleConfig, insert_duplicate ) } /* ************************************************************************* */ -TEST( TupleConfig, size_dim ) +TEST( TupleValues, size_dim ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -133,7 +133,7 @@ TEST( TupleConfig, size_dim ) } /* ************************************************************************* */ -TEST(TupleConfig, at) +TEST(TupleValues, at) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -154,7 +154,7 @@ TEST(TupleConfig, at) } /* ************************************************************************* */ -TEST(TupleConfig, zero_expmap_logmap) +TEST(TupleValues, zero_expmap_logmap) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -204,23 +204,23 @@ typedef TypedSymbol Point3Key; typedef TypedSymbol Point3Key2; // some config types -typedef LieConfig PoseConfig; -typedef LieConfig PointConfig; -typedef LieConfig LamConfig; -typedef LieConfig Pose3Config; -typedef LieConfig Point3Config; -typedef LieConfig Point3Config2; +typedef LieValues PoseConfig; +typedef LieValues PointConfig; +typedef LieValues LamConfig; +typedef LieValues Pose3Config; +typedef LieValues Point3Config; +typedef LieValues Point3Config2; -// some TupleConfig types -typedef TupleConfig > ConfigA; -typedef TupleConfig > > ConfigB; +// some TupleValues types +typedef TupleValues > ConfigA; +typedef TupleValues > > ConfigB; -typedef TupleConfig1 TuplePoseConfig; -typedef TupleConfig1 TuplePointConfig; -typedef TupleConfig2 SimpleConfig; +typedef TupleValues1 TuplePoseConfig; +typedef TupleValues1 TuplePointConfig; +typedef TupleValues2 SimpleConfig; /* ************************************************************************* */ -TEST(TupleConfig, slicing) { +TEST(TupleValues, slicing) { PointKey l1(1), l2(2); Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0); PoseKey x1(1), x2(2); @@ -234,14 +234,14 @@ TEST(TupleConfig, slicing) { liePointConfig.insert(l1, l1_val); liePointConfig.insert(l2, l2_val); - // construct TupleConfig1 from the base config + // construct TupleValues1 from the base config TuplePoseConfig tupPoseConfig1(liePoseConfig); EXPECT(assert_equal(liePoseConfig, tupPoseConfig1.first(), tol)); TuplePointConfig tupPointConfig1(liePointConfig); EXPECT(assert_equal(liePointConfig, tupPointConfig1.first(), tol)); -// // construct a TupleConfig2 from a TupleConfig1 +// // construct a TupleValues2 from a TupleValues1 // SimpleConfig pairConfig1(tupPoseConfig1); // EXPECT(assert_equal(liePoseConfig, pairConfig1.first(), tol)); // EXPECT(pairConfig1.second().empty()); @@ -253,7 +253,7 @@ TEST(TupleConfig, slicing) { } /* ************************************************************************* */ -TEST(TupleConfig, basic_functions) { +TEST(TupleValues, basic_functions) { // create some tuple configs ConfigA configA; ConfigB configB; @@ -325,7 +325,7 @@ TEST(TupleConfig, basic_functions) { } /* ************************************************************************* */ -TEST(TupleConfig, insert_config) { +TEST(TupleValues, insert_config) { ConfigB config1, config2, expected; PoseKey x1(1), x2(2); @@ -356,9 +356,9 @@ TEST(TupleConfig, insert_config) { } /* ************************************************************************* */ -TEST( TupleConfig, update_element ) +TEST( TupleValues, update_element ) { - TupleConfig2 cfg; + TupleValues2 cfg; Pose2 x1(2.0, 1.0, 2.0), x2(3.0, 4.0, 5.0); Point2 l1(1.0, 2.0), l2(3.0, 4.0); PoseKey xk(1); @@ -382,7 +382,7 @@ TEST( TupleConfig, update_element ) } /* ************************************************************************* */ -TEST( TupleConfig, equals ) +TEST( TupleValues, equals ) { Pose2 x1(1,2,3), x2(6,7,8), x2_alt(5,6,7); PoseKey x1k(1), x2k(2); @@ -420,7 +420,7 @@ TEST( TupleConfig, equals ) } /* ************************************************************************* */ -TEST(TupleConfig, expmap) +TEST(TupleValues, expmap) { Pose2 x1(1,2,3), x2(6,7,8); PoseKey x1k(1), x2k(2); @@ -452,7 +452,7 @@ TEST(TupleConfig, expmap) } /* ************************************************************************* */ -TEST(TupleConfig, expmap_typedefs) +TEST(TupleValues, expmap_typedefs) { Pose2 x1(1,2,3), x2(6,7,8); PoseKey x1k(1), x2k(2); @@ -461,7 +461,7 @@ TEST(TupleConfig, expmap_typedefs) Ordering o; o += "x1", "x2", "l1", "l2"; - TupleConfig2 config1, expected, actual; + TupleValues2 config1, expected, actual; config1.insert(x1k, x1); config1.insert(x2k, x2); config1.insert(l1k, l1); @@ -478,22 +478,22 @@ TEST(TupleConfig, expmap_typedefs) expected.insert(l1k, Point2(5.0, 6.1)); expected.insert(l2k, Point2(10.3, 11.4)); - CHECK(assert_equal(expected, TupleConfig2(config1.expmap(delta, o)))); + CHECK(assert_equal(expected, TupleValues2(config1.expmap(delta, o)))); //CHECK(assert_equal(delta, config1.logmap(expected))); } /* ************************************************************************* */ -TEST(TupleConfig, typedefs) +TEST(TupleValues, typedefs) { - TupleConfig2 config1; - TupleConfig3 config2; - TupleConfig4 config3; - TupleConfig5 config4; - TupleConfig6 config5; + TupleValues2 config1; + TupleValues3 config2; + TupleValues4 config3; + TupleValues5 config4; + TupleValues6 config5; } /* ************************************************************************* */ -TEST( TupleConfig, pairconfig_style ) +TEST( TupleValues, pairconfig_style ) { PoseKey x1(1); PointKey l1(1); @@ -507,7 +507,7 @@ TEST( TupleConfig, pairconfig_style ) LamConfig config3; config3.insert(L1, lam1); // Constructor - TupleConfig3 config(config1, config2, config3); + TupleValues3 config(config1, config2, config3); // access CHECK(assert_equal(config1, config.first())); @@ -516,9 +516,9 @@ TEST( TupleConfig, pairconfig_style ) } /* ************************************************************************* */ -TEST(TupleConfig, insert_config_typedef) { +TEST(TupleValues, insert_config_typedef) { - TupleConfig4 config1, config2, expected; + TupleValues4 config1, config2, expected; PoseKey x1(1), x2(2); PointKey l1(1), l2(2); @@ -548,8 +548,8 @@ TEST(TupleConfig, insert_config_typedef) { } /* ************************************************************************* */ -TEST(TupleConfig, partial_insert) { - TupleConfig3 init, expected; +TEST(TupleValues, partial_insert) { + TupleValues3 init, expected; PoseKey x1(1), x2(2); PointKey l1(1), l2(2); @@ -576,8 +576,8 @@ TEST(TupleConfig, partial_insert) { } /* ************************************************************************* */ -TEST(TupleConfig, update) { - TupleConfig3 init, superset, expected; +TEST(TupleValues, update) { + TupleValues3 init, superset, expected; PoseKey x1(1), x2(2); PointKey l1(1), l2(2);