From 77eda5ab8c2275542c25f21de25a1ad196476ef4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 23 Aug 2010 19:44:17 +0000 Subject: [PATCH] In all nonlinear factors and configs, it is only necessary to specify a key with a typedef Value_t for the associated value. This has been removed from factor template definitions, as it is too easy to use the wrong value. Also, no more string keys or linear symbol keys in nonlinear systems. Updated MastSLAM to work, and ISAM2 works without change --- base/Lie-inl.h | 5 - nonlinear/FusionTupleConfig.h | 12 +- nonlinear/LieConfig-inl.h | 86 ++++++------ nonlinear/LieConfig.h | 46 ++++--- nonlinear/NonlinearConstraint.h | 83 ++++++++---- nonlinear/NonlinearEquality.h | 14 +- nonlinear/NonlinearFactor.h | 32 ++++- nonlinear/tests/testLieConfig.cpp | 154 +++++++++++----------- slam/BearingFactor.h | 5 +- slam/BearingRangeFactor.h | 5 +- slam/BetweenConstraint.h | 11 +- slam/BetweenFactor.h | 15 ++- slam/BoundingConstraint.h | 20 +-- slam/PriorFactor.h | 13 +- slam/RangeFactor.h | 5 +- slam/Simulated3D.h | 13 +- slam/TransformConstraint.h | 11 +- slam/planarSLAM.cpp | 2 +- slam/planarSLAM.h | 10 +- slam/pose2SLAM.cpp | 2 +- slam/pose2SLAM.h | 10 +- slam/pose3SLAM.cpp | 2 +- slam/pose3SLAM.h | 8 +- slam/simulated2D.cpp | 4 +- slam/simulated2D.h | 18 ++- slam/simulated2DConstraints.h | 22 ++-- slam/simulated2DOriented.cpp | 2 +- slam/simulated2DOriented.h | 13 +- slam/smallExample.cpp | 5 +- slam/visualSLAM.h | 12 +- tests/testFusionTupleConfig.cpp | 8 +- tests/testNonlinearEquality.cpp | 108 +++++---------- tests/testNonlinearEqualityConstraint.cpp | 2 +- tests/testTransformConstraint.cpp | 14 +- tests/testTupleConfig.cpp | 16 +-- 35 files changed, 406 insertions(+), 382 deletions(-) diff --git a/base/Lie-inl.h b/base/Lie-inl.h index 5e4c39d08..002d81740 100644 --- a/base/Lie-inl.h +++ b/base/Lie-inl.h @@ -18,11 +18,6 @@ template class Lie; namespace gtsam { -// template -// size_t Lie::dim() const { -// return gtsam::dim(*((T*)this)); -// } - /** * Returns Exponential mapy * This is for matlab wrapper diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h index 543e45cbc..d07b61f59 100644 --- a/nonlinear/FusionTupleConfig.h +++ b/nonlinear/FusionTupleConfig.h @@ -169,7 +169,7 @@ public: /** insertion */ template void insert(const Key& j, const Value& x) { - config_ >().insert(j,x); + config_ >().insert(j,x); } /** insert a full config at a time */ @@ -204,25 +204,25 @@ public: */ template void update(const Key& key, const Value& value) { - config_ >().update(key,value); + config_ >().update(key,value); } /** check if a given element exists */ template bool exists(const Key& j) const { - return config >().exists(j); + return config >().exists(j); } /** a variant of exists */ template boost::optional exists_(const Key& j) const { - return config >().exists_(j); + return config >().exists_(j); } /** retrieve a point */ template const typename Key::Value_t & at(const Key& j) const { - return config >().at(j); + return config >().at(j); } /** access operator */ @@ -255,7 +255,7 @@ public: /** erases a specific key */ template void erase(const Key& j) { - config_ >().erase(j); + config_ >().erase(j); } /** clears the config */ diff --git a/nonlinear/LieConfig-inl.h b/nonlinear/LieConfig-inl.h index 640f2eb43..31c96dcda 100644 --- a/nonlinear/LieConfig-inl.h +++ b/nonlinear/LieConfig-inl.h @@ -19,27 +19,27 @@ #include -#define INSTANTIATE_LIE_CONFIG(J,T) \ +#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 LieConfig expmap(const LieConfig&, const VectorConfig&); \ + template LieConfig expmap(const LieConfig&, const Vector&); \ + template VectorConfig logmap(const LieConfig&, const LieConfig&); \ + template class LieConfig; using namespace std; namespace gtsam { - template - void LieConfig::print(const string &s) const { + template + void LieConfig::print(const string &s) const { cout << "LieConfig " << s << ", size " << values_.size() << "\n"; BOOST_FOREACH(const typename Values::value_type& v, values_) { gtsam::print(v.second, (string)(v.first)); } } - template - bool LieConfig::equals(const LieConfig& expected, double tol) const { + template + bool LieConfig::equals(const LieConfig& expected, double tol) const { if (values_.size() != expected.values_.size()) return false; BOOST_FOREACH(const typename Values::value_type& v, values_) { if (!expected.exists(v.first)) return false; @@ -49,69 +49,69 @@ namespace gtsam { return true; } - template - const T& LieConfig::at(const J& j) const { + template + const typename J::Value_t& LieConfig::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; } - template - size_t LieConfig::dim() const { + template + size_t LieConfig::dim() const { size_t n = 0; BOOST_FOREACH(const typename Values::value_type& value, values_) n += gtsam::dim(value.second); return n; } - template - VectorConfig LieConfig::zero() const { + template + VectorConfig LieConfig::zero() const { VectorConfig z; BOOST_FOREACH(const typename Values::value_type& value, values_) z.insert(value.first,gtsam::zero(gtsam::dim(value.second))); return z; } - template - void LieConfig::insert(const J& name, const T& val) { + template + void LieConfig::insert(const J& name, const typename J::Value_t& val) { values_.insert(make_pair(name, val)); } - template - void LieConfig::insert(const LieConfig& cfg) { + template + void LieConfig::insert(const LieConfig& cfg) { BOOST_FOREACH(const typename Values::value_type& v, cfg.values_) insert(v.first, v.second); } - template - void LieConfig::update(const LieConfig& cfg) { + template + void LieConfig::update(const LieConfig& cfg) { BOOST_FOREACH(const typename Values::value_type& v, values_) { - boost::optional t = cfg.exists_(v.first); + boost::optional t = cfg.exists_(v.first); if (t) values_[v.first] = *t; } } - template - void LieConfig::update(const J& j, const T& val) { + template + void LieConfig::update(const J& j, const typename J::Value_t& val) { values_[j] = val; } - template - std::list LieConfig::keys() const { + template + std::list LieConfig::keys() const { std::list ret; BOOST_FOREACH(const typename Values::value_type& v, values_) ret.push_back(v.first); return ret; } - template - void LieConfig::erase(const J& j) { + template + void LieConfig::erase(const J& j) { size_t dim; // unused erase(j, dim); } - template - void LieConfig::erase(const J& j, size_t& dim) { + template + void LieConfig::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 = gtsam::dim(it->second); @@ -119,13 +119,13 @@ namespace gtsam { } // todo: insert for every element is inefficient - template - LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { - LieConfig newConfig; - typedef pair KeyValue; + template + LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { + LieConfig newConfig; + typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, c) { const J& j = value.first; - const T& pj = value.second; + const typename J::Value_t& pj = value.second; Symbol jkey = (Symbol)j; if (delta.contains(jkey)) { const Vector& dj = delta[jkey]; @@ -137,18 +137,18 @@ namespace gtsam { } // todo: insert for every element is inefficient - template - LieConfig expmap(const LieConfig& c, const Vector& delta) { + template + LieConfig expmap(const LieConfig& c, const Vector& delta) { if(delta.size() != dim(c)) { cout << "LieConfig::dim (" << dim(c) << ") <> delta.size (" << delta.size() << ")" << endl; throw invalid_argument("Delta vector length does not match config dimensionality."); } - LieConfig newConfig; + LieConfig newConfig; int delta_offset = 0; - typedef pair KeyValue; + typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, c) { const J& j = value.first; - const T& pj = value.second; + const typename J::Value_t& pj = value.second; int cur_dim = dim(pj); newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim))); delta_offset += cur_dim; @@ -158,10 +158,10 @@ namespace gtsam { // todo: insert for every element is inefficient // todo: currently only logmaps elements in both configs - template - VectorConfig logmap(const LieConfig& c0, const LieConfig& cp) { + template + VectorConfig logmap(const LieConfig& c0, const LieConfig& cp) { VectorConfig delta; - typedef pair KeyValue; + typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, cp) { if(c0.exists(value.first)) delta.insert(value.first, diff --git a/nonlinear/LieConfig.h b/nonlinear/LieConfig.h index 3d738c497..d428c0ddb 100644 --- a/nonlinear/LieConfig.h +++ b/nonlinear/LieConfig.h @@ -32,11 +32,15 @@ namespace gtsam { /** * Lie type configuration * Takes two template types - * J: a type to look up values in the configuration (need to be sortable) - * T: the type of values being stored in the configuration + * J: a key type to look up values in the configuration (need to be sortable) + * + * Key concept: + * The key will be assumed to be sortable, and must have a + * typedef called "Value_t" with the type of the value the key + * labels (example: Pose2, Point2, etc) */ - template - class LieConfig : public Testable > { + template + class LieConfig : public Testable > { public: @@ -44,8 +48,8 @@ namespace gtsam { * Typedefs */ typedef J Key; - typedef T Value; - typedef std::map Values; + typedef typename J::Value_t Value; + typedef std::map Values; typedef typename Values::iterator iterator; typedef typename Values::const_iterator const_iterator; @@ -58,8 +62,8 @@ namespace gtsam { LieConfig() {} LieConfig(const LieConfig& config) : values_(config.values_) {} - template - LieConfig(const LieConfig& other) {} // do nothing when initializing with wrong type + template + LieConfig(const LieConfig& other) {} // do nothing when initializing with wrong type virtual ~LieConfig() {} /** print */ @@ -69,16 +73,16 @@ namespace gtsam { bool equals(const LieConfig& expected, double tol=1e-9) const; /** Retrieve a variable by j, throws std::invalid_argument if not found */ - const T& at(const J& j) const; + const Value& at(const J& j) const; /** operator[] syntax for get */ - const T& operator[](const J& j) const { return at(j); } + const Value& operator[](const J& j) const { return at(j); } /** Check if a variable exists */ bool exists(const J& i) const { return values_.find(i)!=values_.end(); } /** Check if a variable exists and return it if so */ - boost::optional exists_(const J& i) const { + boost::optional exists_(const J& i) const { const_iterator it = values_.find(i); if (it==values_.end()) return boost::none; else return it->second; } @@ -103,7 +107,7 @@ namespace gtsam { // imperative methods: /** Add a variable with the given j - does not replace existing values */ - void insert(const J& j, const T& val); + void insert(const J& j, const Value& val); /** Add a set of variables - does note replace existing values */ void insert(const LieConfig& cfg); @@ -112,7 +116,7 @@ namespace gtsam { void update(const LieConfig& cfg); /** single element change of existing element */ - void update(const J& j, const T& val); + void update(const J& j, const Value& val); /** Remove a variable from the config */ void erase(const J& j); @@ -150,20 +154,20 @@ namespace gtsam { }; /** Dimensionality of the tangent space */ - template - inline size_t dim(const LieConfig& c) { return c.dim(); } + template + inline size_t dim(const LieConfig& c) { return c.dim(); } /** Add a delta config */ - template - LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + template + LieConfig expmap(const LieConfig& c, const VectorConfig& delta); /** Add a delta vector, uses iterator order */ - template - LieConfig expmap(const LieConfig& c, const Vector& delta); + template + LieConfig expmap(const LieConfig& c, const Vector& delta); /** Get a delta config about a linearization point c0 */ - template - VectorConfig logmap(const LieConfig& c0, const LieConfig& cp); + template + VectorConfig logmap(const LieConfig& c0, const LieConfig& cp); } diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index d7ea4cb9c..242699153 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -95,11 +95,14 @@ public: /** * A unary constraint that defaults to an equality constraint */ -template +template class NonlinearConstraint1 : public NonlinearConstraint { +public: + typedef typename Key::Value_t X; + protected: - typedef NonlinearConstraint1 This; + typedef NonlinearConstraint1 This; typedef NonlinearConstraint Base; /** key for the constrained variable */ @@ -165,12 +168,15 @@ public: /** * Unary Equality constraint - simply forces the value of active() to true */ -template -class NonlinearEqualityConstraint1 : public NonlinearConstraint1 { +template +class NonlinearEqualityConstraint1 : public NonlinearConstraint1 { + +public: + typedef typename Key::Value_t X; protected: - typedef NonlinearEqualityConstraint1 This; - typedef NonlinearConstraint1 Base; + typedef NonlinearEqualityConstraint1 This; + typedef NonlinearConstraint1 Base; public: NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0) @@ -184,11 +190,15 @@ public: /** * A binary constraint with arbitrary cost and jacobian functions */ -template +template class NonlinearConstraint2 : public NonlinearConstraint { +public: + typedef typename Key1::Value_t X1; + typedef typename Key2::Value_t X2; + protected: - typedef NonlinearConstraint2 This; + typedef NonlinearConstraint2 This; typedef NonlinearConstraint Base; /** keys for the constrained variables */ @@ -261,12 +271,16 @@ public: /** * Binary Equality constraint - simply forces the value of active() to true */ -template -class NonlinearEqualityConstraint2 : public NonlinearConstraint2 { +template +class NonlinearEqualityConstraint2 : public NonlinearConstraint2 { + +public: + typedef typename Key1::Value_t X1; + typedef typename Key2::Value_t X2; protected: - typedef NonlinearEqualityConstraint2 This; - typedef NonlinearConstraint2 Base; + typedef NonlinearEqualityConstraint2 This; + typedef NonlinearConstraint2 Base; public: NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) @@ -281,11 +295,16 @@ public: /** * A ternary constraint */ -template +template class NonlinearConstraint3 : public NonlinearConstraint { +public: + typedef typename Key1::Value_t X1; + typedef typename Key2::Value_t X2; + typedef typename Key3::Value_t X3; + protected: - typedef NonlinearConstraint3 This; + typedef NonlinearConstraint3 This; typedef NonlinearConstraint Base; /** keys for the constrained variables */ @@ -366,12 +385,17 @@ public: /** * Ternary Equality constraint - simply forces the value of active() to true */ -template -class NonlinearEqualityConstraint3 : public NonlinearConstraint3 { +template +class NonlinearEqualityConstraint3 : public NonlinearConstraint3 { + +public: + typedef typename Key1::Value_t X1; + typedef typename Key2::Value_t X2; + typedef typename Key3::Value_t X3; protected: - typedef NonlinearEqualityConstraint3 This; - typedef NonlinearConstraint3 Base; + typedef NonlinearEqualityConstraint3 This; + typedef NonlinearConstraint3 Base; public: NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3, @@ -387,16 +411,20 @@ public: /** * Simple unary equality constraint - fixes a value for a variable */ -template -class NonlinearEquality1 : public NonlinearEqualityConstraint1 { +template +class NonlinearEquality1 : public NonlinearEqualityConstraint1 { + +public: + typedef typename Key::Value_t X; + protected: - typedef NonlinearEqualityConstraint1 Base; + typedef NonlinearEqualityConstraint1 Base; X value_; /// fixed value for variable public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) : Base(key1, X::Dim(), mu), value_(value) {} @@ -415,14 +443,17 @@ public: * Simple binary equality constraint - this constraint forces two factors to * be the same. This constraint requires the underlying type to a Lie type */ -template -class NonlinearEquality2 : public NonlinearEqualityConstraint2 { +template +class NonlinearEquality2 : public NonlinearEqualityConstraint2 { +public: + typedef typename Key::Value_t X; + protected: - typedef NonlinearEqualityConstraint2 Base; + typedef NonlinearEqualityConstraint2 Base; public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) : Base(key1, key2, X::Dim(), mu) {} diff --git a/nonlinear/NonlinearEquality.h b/nonlinear/NonlinearEquality.h index 950e78cef..fac5385f9 100644 --- a/nonlinear/NonlinearEquality.h +++ b/nonlinear/NonlinearEquality.h @@ -30,8 +30,12 @@ namespace gtsam { * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error */ - template - class NonlinearEquality: public NonlinearFactor1 { + template + class NonlinearEquality: public NonlinearFactor1 { + + public: + typedef typename Key::Value_t T; + private: // feasible value @@ -50,7 +54,7 @@ namespace gtsam { */ bool (*compare_)(const T& a, const T& b); - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; /** * Constructor - forces exact evaluation @@ -78,8 +82,8 @@ namespace gtsam { /** Check if two factors are equal */ bool equals(const Factor& f, double tol = 1e-9) const { - const NonlinearEquality* p = - dynamic_cast*> (&f); + const NonlinearEquality* p = + dynamic_cast*> (&f); if (p == NULL) return false; if (!Base::equals(*p)) return false; return compare_(feasible_, p->feasible_); diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index 2d7b20402..c7fbf967a 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -139,16 +139,21 @@ namespace gtsam { * the derived class implements error_vector(c) = h(x)-z \approx Ax-b * This allows a graph to have factors with measurements of mixed type. */ - template + template class NonlinearFactor1: public NonlinearFactor { + public: + + // typedefs for value types pulled from keys + typedef typename Key::Value_t X; + protected: // The value of the key. Not const to allow serialization Key key_; typedef NonlinearFactor Base; - typedef NonlinearFactor1 This; + typedef NonlinearFactor1 This; public: @@ -237,9 +242,15 @@ namespace gtsam { /** * A Gaussian nonlinear factor that takes 2 parameters */ - template + template class NonlinearFactor2: public NonlinearFactor { + public: + + // typedefs for value types pulled from keys + typedef typename Key1::Value_t X1; + typedef typename Key2::Value_t X2; + protected: // The values of the keys. Not const to allow serialization @@ -247,7 +258,7 @@ namespace gtsam { Key2 key2_; typedef NonlinearFactor Base; - typedef NonlinearFactor2 This; + typedef NonlinearFactor2 This; public: @@ -352,18 +363,25 @@ namespace gtsam { /** * A Gaussian nonlinear factor that takes 3 parameters */ - template + template class NonlinearFactor3: public NonlinearFactor { + public: + + // typedefs for value types pulled from keys + typedef typename Key1::Value_t X1; + typedef typename Key2::Value_t X2; + typedef typename Key3::Value_t X3; + protected: - // The values of the keys. Not const to allow serialization + // The values of the keys. Not const to allow serialization Key1 key1_; Key2 key2_; Key3 key3_; typedef NonlinearFactor Base; - typedef NonlinearFactor3 This; + typedef NonlinearFactor3 This; public: diff --git a/nonlinear/tests/testLieConfig.cpp b/nonlinear/tests/testLieConfig.cpp index 262d9339c..3b17c1f46 100644 --- a/nonlinear/tests/testLieConfig.cpp +++ b/nonlinear/tests/testLieConfig.cpp @@ -11,34 +11,37 @@ #include // for operator += using namespace boost::assign; -#define GTSAM_MAGIC_KEY - #include -#include +#include using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); +typedef TypedSymbol VecKey; +typedef LieConfig Config; + +VecKey key1(1), key2(2), key3(3), key4(4); + /* ************************************************************************* */ TEST( LieConfig, equals1 ) { - LieConfig expected; + Config expected; Vector v = Vector_(3, 5.0, 6.0, 7.0); - expected.insert("a",v); - LieConfig actual; - actual.insert("a",v); + expected.insert(key1,v); + Config actual; + actual.insert(key1,v); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ TEST( LieConfig, equals2 ) { - LieConfig cfg1, cfg2; + Config cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 5.0, 6.0, 8.0); - cfg1.insert("x", v1); - cfg2.insert("x", v2); + cfg1.insert(key1, v1); + cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); CHECK(!cfg2.equals(cfg1)); } @@ -46,11 +49,11 @@ TEST( LieConfig, equals2 ) /* ************************************************************************* */ TEST( LieConfig, equals_nan ) { - LieConfig cfg1, cfg2; + Config cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, inf, inf, inf); - cfg1.insert("x", v1); - cfg2.insert("x", v2); + cfg1.insert(key1, v1); + cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); CHECK(!cfg2.equals(cfg1)); } @@ -58,22 +61,22 @@ TEST( LieConfig, equals_nan ) /* ************************************************************************* */ TEST( LieConfig, insert_config ) { - LieConfig cfg1, cfg2, expected; + Config cfg1, cfg2, expected; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v3 = Vector_(3, 2.0, 4.0, 3.0); Vector v4 = Vector_(3, 8.0, 3.0, 7.0); - cfg1.insert("x1", v1); - cfg1.insert("x2", v2); - cfg2.insert("x2", v3); - cfg2.insert("x3", v4); + cfg1.insert(key1, v1); + cfg1.insert(key2, v2); + cfg2.insert(key2, v3); + cfg2.insert(key3, v4); cfg1.insert(cfg2); - expected.insert("x1", v1); - expected.insert("x2", v2); - expected.insert("x2", v3); - expected.insert("x3", v4); + expected.insert(key1, v1); + expected.insert(key2, v2); + expected.insert(key2, v3); + expected.insert(key3, v4); CHECK(assert_equal(cfg1, expected)); } @@ -81,47 +84,47 @@ TEST( LieConfig, insert_config ) /* ************************************************************************* */ TEST( LieConfig, update_element ) { - LieConfig cfg; + Config cfg; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0); - cfg.insert("x1", v1); + cfg.insert(key1, v1); CHECK(cfg.size() == 1); - CHECK(assert_equal(v1, cfg.at("x1"))); + CHECK(assert_equal(v1, cfg.at(key1))); - cfg.update("x1", v2); + cfg.update(key1, v2); CHECK(cfg.size() == 1); - CHECK(assert_equal(v2, cfg.at("x1"))); + CHECK(assert_equal(v2, cfg.at(key1))); } /* ************************************************************************* */ TEST(LieConfig, dim_zero) { - LieConfig config0; - config0.insert("v1", Vector_(2, 2.0, 3.0)); - config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + Config config0; + config0.insert(key1, Vector_(2, 2.0, 3.0)); + config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); LONGS_EQUAL(5,config0.dim()); VectorConfig expected; - expected.insert("v1", zero(2)); - expected.insert("v2", zero(3)); + expected.insert(key1, zero(2)); + expected.insert(key2, zero(3)); CHECK(assert_equal(expected, config0.zero())); } /* ************************************************************************* */ TEST(LieConfig, expmap_a) { - LieConfig config0; - config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); - config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + Config config0; + config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); + config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); VectorConfig increment; - increment.insert("v1", Vector_(3, 1.0, 1.1, 1.2)); - increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5)); + increment.insert(key1, Vector_(3, 1.0, 1.1, 1.2)); + increment.insert(key2, Vector_(3, 1.3, 1.4, 1.5)); - LieConfig expected; - expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); - expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); + Config expected; + expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); + expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); CHECK(assert_equal(expected, expmap(config0, increment))); } @@ -129,16 +132,16 @@ TEST(LieConfig, expmap_a) /* ************************************************************************* */ TEST(LieConfig, expmap_b) { - LieConfig config0; - config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); - config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + Config config0; + config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); + config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); VectorConfig increment; - increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5)); + increment.insert(key2, Vector_(3, 1.3, 1.4, 1.5)); - LieConfig expected; - expected.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); - expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); + Config expected; + expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); + expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); CHECK(assert_equal(expected, expmap(config0, increment))); } @@ -146,17 +149,17 @@ TEST(LieConfig, expmap_b) /* ************************************************************************* */ TEST(LieConfig, expmap_c) { - LieConfig config0; - config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); - config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + Config config0; + config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); + config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); Vector increment = Vector_(6, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5); - LieConfig expected; - expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); - expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); + Config expected; + expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); + expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); CHECK(assert_equal(expected, expmap(config0, increment))); } @@ -164,9 +167,9 @@ TEST(LieConfig, expmap_c) /* ************************************************************************* */ /*TEST(LieConfig, expmap_d) { - LieConfig config0; - config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); - config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); + Config config0; + config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); + config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); @@ -204,30 +207,30 @@ TEST(LieConfig, expmap_c) /* ************************************************************************* */ TEST(LieConfig, exists_) { - LieConfig config0; - config0.insert("v1", Vector_(1, 1.)); - config0.insert("v2", Vector_(1, 2.)); + Config config0; + config0.insert(key1, Vector_(1, 1.)); + config0.insert(key2, Vector_(1, 2.)); - boost::optional v = config0.exists_("v1"); + boost::optional v = config0.exists_(key1); CHECK(assert_equal(Vector_(1, 1.),*v)); } /* ************************************************************************* */ TEST(LieConfig, update) { - LieConfig config0; - config0.insert("v1", Vector_(1, 1.)); - config0.insert("v2", Vector_(1, 2.)); + Config config0; + config0.insert(key1, Vector_(1, 1.)); + config0.insert(key2, Vector_(1, 2.)); - LieConfig superset; - superset.insert("v1", Vector_(1, -1.)); - superset.insert("v2", Vector_(1, -2.)); - superset.insert("v3", Vector_(1, -3.)); + Config superset; + superset.insert(key1, Vector_(1, -1.)); + superset.insert(key2, Vector_(1, -2.)); + superset.insert(key3, Vector_(1, -3.)); config0.update(superset); - LieConfig expected; - expected.insert("v1", Vector_(1, -1.)); - expected.insert("v2", Vector_(1, -2.)); + Config expected; + expected.insert(key1, Vector_(1, -1.)); + expected.insert(key2, Vector_(1, -2.)); CHECK(assert_equal(expected,config0)); } @@ -235,19 +238,18 @@ TEST(LieConfig, update) TEST(LieConfig, dummy_initialization) { typedef TypedSymbol Key; - typedef LieConfig Config1; - typedef LieConfig Config2; + typedef LieConfig Config1; Config1 init1; init1.insert(Key(1), Vector_(2, 1.0, 2.0)); init1.insert(Key(2), Vector_(2, 4.0, 3.0)); - Config2 init2; - init2.insert("v1", Vector_(2, 1.0, 2.0)); - init2.insert("v2", Vector_(2, 4.0, 3.0)); + Config init2; + init2.insert(key1, Vector_(2, 1.0, 2.0)); + init2.insert(key2, Vector_(2, 4.0, 3.0)); Config1 actual1(init2); - Config2 actual2(init1); + Config actual2(init1); EXPECT(actual1.empty()); EXPECT(actual2.empty()); diff --git a/slam/BearingFactor.h b/slam/BearingFactor.h index 6ab89014c..539fbf676 100644 --- a/slam/BearingFactor.h +++ b/slam/BearingFactor.h @@ -16,13 +16,12 @@ namespace gtsam { * Binary factor for a bearing measurement */ template - class BearingFactor: public NonlinearFactor2 { + class BearingFactor: public NonlinearFactor2 { private: Rot2 z_; /** measurement */ - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; public: diff --git a/slam/BearingRangeFactor.h b/slam/BearingRangeFactor.h index 889ae4de9..aa60be5d6 100644 --- a/slam/BearingRangeFactor.h +++ b/slam/BearingRangeFactor.h @@ -17,15 +17,14 @@ namespace gtsam { * Binary factor for a bearing measurement */ template - class BearingRangeFactor: public NonlinearFactor2 { + class BearingRangeFactor: public NonlinearFactor2 { private: // the measurement Rot2 bearing_; double range_; - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; public: diff --git a/slam/BetweenConstraint.h b/slam/BetweenConstraint.h index 4b02bf99a..9beb91a85 100644 --- a/slam/BetweenConstraint.h +++ b/slam/BetweenConstraint.h @@ -14,16 +14,19 @@ namespace gtsam { * Binary between constraint - forces between to a given value * This constraint requires the underlying type to a Lie type */ - template - class BetweenConstraint : public NonlinearEqualityConstraint2 { + template + class BetweenConstraint : public NonlinearEqualityConstraint2 { + public: + typedef typename Key::Value_t X; + protected: - typedef NonlinearEqualityConstraint2 Base; + typedef NonlinearEqualityConstraint2 Base; X measured_; /// fixed between value public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) : Base(key1, key2, X::Dim(), mu), measured_(measured) {} diff --git a/slam/BetweenFactor.h b/slam/BetweenFactor.h index e45228720..5c7e66ca4 100644 --- a/slam/BetweenFactor.h +++ b/slam/BetweenFactor.h @@ -16,13 +16,18 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" * T is the Lie group type, Config where the T's are gotten from + * + * FIXME: This should only need one key type, as we can't have different types */ - template - class BetweenFactor: public NonlinearFactor2 { + template + class BetweenFactor: public NonlinearFactor2 { + + public: + typedef typename Key1::Value_t T; private: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; T measured_; /** The measurement */ @@ -47,8 +52,8 @@ namespace gtsam { /** equals */ bool equals(const NonlinearFactor& expected, double tol) const { - const BetweenFactor *e = - dynamic_cast*> (&expected); + const BetweenFactor *e = + dynamic_cast*> (&expected); return e != NULL && Base::equals(expected) && this->measured_.equals( e->measured_, tol); } diff --git a/slam/BoundingConstraint.h b/slam/BoundingConstraint.h index 0fee49d30..14dd6f576 100644 --- a/slam/BoundingConstraint.h +++ b/slam/BoundingConstraint.h @@ -17,10 +17,11 @@ namespace gtsam { * will need to have its value function implemented to return * a scalar for comparison. */ - template - struct BoundingConstraint1: public NonlinearConstraint1 { - typedef NonlinearConstraint1 Base; - typedef boost::shared_ptr > shared_ptr; + template + struct BoundingConstraint1: public NonlinearConstraint1 { + typedef typename Key::Value_t X; + typedef NonlinearConstraint1 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than @@ -61,10 +62,13 @@ namespace gtsam { * Binary scalar inequality constraint, with a similar value() function * to implement for specific systems */ - template - struct BoundingConstraint2: public NonlinearConstraint2 { - typedef NonlinearConstraint2 Base; - typedef boost::shared_ptr > shared_ptr; + template + struct BoundingConstraint2: public NonlinearConstraint2 { + typedef typename Key1::Value_t X1; + typedef typename Key2::Value_t X2; + + typedef NonlinearConstraint2 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than diff --git a/slam/PriorFactor.h b/slam/PriorFactor.h index a05349240..7238055ea 100644 --- a/slam/PriorFactor.h +++ b/slam/PriorFactor.h @@ -20,12 +20,15 @@ namespace gtsam { * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * a simple type like int will not work */ - template - class PriorFactor: public NonlinearFactor1 { + template + class PriorFactor: public NonlinearFactor1 { + + public: + typedef typename Key::Value_t T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; T prior_; /** The measurement */ @@ -50,8 +53,8 @@ namespace gtsam { /** equals */ bool equals(const NonlinearFactor& expected, double tol) const { - const PriorFactor *e = dynamic_cast*> (&expected); + const PriorFactor *e = dynamic_cast*> (&expected); return e != NULL && Base::equals(expected) && this->prior_.equals( e->prior_, tol); } diff --git a/slam/RangeFactor.h b/slam/RangeFactor.h index 7c0e38f15..57cb5372b 100644 --- a/slam/RangeFactor.h +++ b/slam/RangeFactor.h @@ -15,13 +15,12 @@ namespace gtsam { * Binary factor for a range measurement */ template - class RangeFactor: public NonlinearFactor2 { + class RangeFactor: public NonlinearFactor2 { private: double z_; /** measurement */ - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; public: diff --git a/slam/Simulated3D.h b/slam/Simulated3D.h index 397470468..c5f504252 100644 --- a/slam/Simulated3D.h +++ b/slam/Simulated3D.h @@ -21,8 +21,8 @@ namespace simulated3D { typedef VectorConfig VectorConfig; - typedef gtsam::Symbol PoseKey; - typedef gtsam::Symbol PointKey; + typedef gtsam::TypedSymbol PoseKey; + typedef gtsam::TypedSymbol PointKey; /** * Prior on a single pose @@ -44,14 +44,13 @@ namespace simulated3D { Matrix Dmea1(const Vector& x, const Vector& l); Matrix Dmea2(const Vector& x, const Vector& l); - struct Point2Prior3D: public NonlinearFactor1 { + struct Point2Prior3D: public NonlinearFactor1 { Vector z_; Point2Prior3D(const Vector& z, const SharedGaussian& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { + NonlinearFactor1 (model, j), z_(z) { } Vector evaluateError(const Vector& x, boost::optional H = @@ -62,13 +61,13 @@ namespace simulated3D { }; struct Simulated3DMeasurement: public NonlinearFactor2 { + PoseKey, PointKey> { Vector z_; Simulated3DMeasurement(const Vector& z, const SharedGaussian& model, PoseKey& j1, PointKey j2) : - NonlinearFactor2 ( + NonlinearFactor2 ( model, j1, j2), z_(z) { } diff --git a/slam/TransformConstraint.h b/slam/TransformConstraint.h index 5e0861985..975714698 100644 --- a/slam/TransformConstraint.h +++ b/slam/TransformConstraint.h @@ -25,12 +25,15 @@ namespace gtsam { * This base class should be specialized to implement the cost function for * specific classes of landmarks */ -template -class TransformConstraint : public NonlinearEqualityConstraint3 { +template +class TransformConstraint : public NonlinearEqualityConstraint3 { public: - typedef NonlinearEqualityConstraint3 Base; - typedef TransformConstraint This; + typedef typename PKey::Value_t Point; + typedef typename TKey::Value_t Transform; + + typedef NonlinearEqualityConstraint3 Base; + typedef TransformConstraint This; /** * General constructor with all of the keys to variables in the map diff --git a/slam/planarSLAM.cpp b/slam/planarSLAM.cpp index 6e74e78a7..73a155c1b 100644 --- a/slam/planarSLAM.cpp +++ b/slam/planarSLAM.cpp @@ -13,7 +13,7 @@ namespace gtsam { using namespace planarSLAM; - INSTANTIATE_LIE_CONFIG(PointKey, Point2) + INSTANTIATE_LIE_CONFIG(PointKey) INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/planarSLAM.h b/slam/planarSLAM.h index 0fcf34ab6..e70eca85c 100644 --- a/slam/planarSLAM.h +++ b/slam/planarSLAM.h @@ -23,14 +23,14 @@ namespace gtsam { // Keys and Config typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; + typedef LieConfig PoseConfig; + typedef LieConfig PointConfig; typedef TupleConfig2 Config; // Factors - typedef NonlinearEquality Constraint; - typedef PriorFactor Prior; - typedef BetweenFactor Odometry; + typedef NonlinearEquality Constraint; + typedef PriorFactor Prior; + typedef BetweenFactor Odometry; typedef BearingFactor Bearing; typedef RangeFactor Range; typedef BearingRangeFactor BearingRange; diff --git a/slam/pose2SLAM.cpp b/slam/pose2SLAM.cpp index 312556dbe..3b1d8abfc 100644 --- a/slam/pose2SLAM.cpp +++ b/slam/pose2SLAM.cpp @@ -13,7 +13,7 @@ namespace gtsam { using namespace pose2SLAM; - INSTANTIATE_LIE_CONFIG(Key, Pose2) + INSTANTIATE_LIE_CONFIG(Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/pose2SLAM.h b/slam/pose2SLAM.h index ba04c7778..28148b8cc 100644 --- a/slam/pose2SLAM.h +++ b/slam/pose2SLAM.h @@ -23,7 +23,7 @@ namespace gtsam { // Keys and Config typedef TypedSymbol Key; - typedef LieConfig Config; + typedef LieConfig Config; /** * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) @@ -35,13 +35,13 @@ namespace gtsam { Config circle(size_t n, double R); // Factors - typedef PriorFactor Prior; - typedef BetweenFactor Constraint; - typedef NonlinearEquality HardConstraint; + typedef PriorFactor Prior; + typedef BetweenFactor Constraint; + typedef NonlinearEquality HardConstraint; // Graph struct Graph: public NonlinearFactorGraph { - typedef BetweenFactor Constraint; + typedef BetweenFactor Constraint; typedef Pose2 Pose; void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model); void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model); diff --git a/slam/pose3SLAM.cpp b/slam/pose3SLAM.cpp index 46a95eed6..e9ac4c626 100644 --- a/slam/pose3SLAM.cpp +++ b/slam/pose3SLAM.cpp @@ -13,7 +13,7 @@ namespace gtsam { using namespace pose3SLAM; - INSTANTIATE_LIE_CONFIG(Key, Pose3) + INSTANTIATE_LIE_CONFIG(Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/pose3SLAM.h b/slam/pose3SLAM.h index c2660dac7..43a7928a8 100644 --- a/slam/pose3SLAM.h +++ b/slam/pose3SLAM.h @@ -22,7 +22,7 @@ namespace gtsam { // Keys and Config typedef TypedSymbol Key; - typedef LieConfig Config; + typedef LieConfig Config; /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) @@ -34,9 +34,9 @@ namespace gtsam { Config circle(size_t n, double R); // Factors - typedef PriorFactor Prior; - typedef BetweenFactor Constraint; - typedef NonlinearEquality HardConstraint; + typedef PriorFactor Prior; + typedef BetweenFactor Constraint; + typedef NonlinearEquality HardConstraint; // Graph struct Graph: public NonlinearFactorGraph { diff --git a/slam/simulated2D.cpp b/slam/simulated2D.cpp index 03bdc4857..cb1a4df54 100644 --- a/slam/simulated2D.cpp +++ b/slam/simulated2D.cpp @@ -11,8 +11,8 @@ namespace gtsam { using namespace simulated2D; -// INSTANTIATE_LIE_CONFIG(PointKey, Point2) - INSTANTIATE_LIE_CONFIG(PoseKey, Point2) +// INSTANTIATE_LIE_CONFIG(PointKey) + INSTANTIATE_LIE_CONFIG(PoseKey) INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) // INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) // INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/simulated2D.h b/slam/simulated2D.h index 93eab3564..e288f87f9 100644 --- a/slam/simulated2D.h +++ b/slam/simulated2D.h @@ -21,8 +21,8 @@ namespace gtsam { // Simulated2D robots have no orientation, just a position typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; + typedef LieConfig PoseConfig; + typedef LieConfig PointConfig; typedef TupleConfig2 Config; /** @@ -55,13 +55,13 @@ namespace gtsam { * Unary factor encoding a soft prior on a vector */ template - struct GenericPrior: public NonlinearFactor1 { + struct GenericPrior: public NonlinearFactor1 { typedef boost::shared_ptr > shared_ptr; Point2 z_; GenericPrior(const Point2& z, const SharedGaussian& model, const Key& key) : - NonlinearFactor1 (model, key), z_(z) { + NonlinearFactor1 (model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional H = @@ -75,14 +75,13 @@ namespace gtsam { * Binary factor simulating "odometry" between two Vectors */ template - struct GenericOdometry: public NonlinearFactor2 { + struct GenericOdometry: public NonlinearFactor2 { typedef boost::shared_ptr > shared_ptr; Point2 z_; GenericOdometry(const Point2& z, const SharedGaussian& model, const Key& i1, const Key& i2) : - NonlinearFactor2 (model, i1, i2), z_(z) { + NonlinearFactor2 (model, i1, i2), z_(z) { } Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< @@ -96,15 +95,14 @@ namespace gtsam { * Binary factor simulating "measurement" between two Vectors */ template - class GenericMeasurement: public NonlinearFactor2 { + class GenericMeasurement: public NonlinearFactor2 { public: typedef boost::shared_ptr > shared_ptr; Point2 z_; GenericMeasurement(const Point2& z, const SharedGaussian& model, const XKey& i, const LKey& j) : - NonlinearFactor2 (model, i, j), z_(z) { + NonlinearFactor2 (model, i, j), z_(z) { } Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< diff --git a/slam/simulated2DConstraints.h b/slam/simulated2DConstraints.h index 689a812f0..d3995dc5c 100644 --- a/slam/simulated2DConstraints.h +++ b/slam/simulated2DConstraints.h @@ -24,13 +24,13 @@ namespace gtsam { namespace equality_constraints { /** Typedefs for regular use */ - typedef NonlinearEquality1 UnaryEqualityConstraint; - typedef NonlinearEquality1 UnaryEqualityPointConstraint; - typedef BetweenConstraint OdoEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; + typedef BetweenConstraint OdoEqualityConstraint; /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 PointEqualityConstraint; + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; } // \namespace equality_constraints @@ -41,8 +41,8 @@ namespace gtsam { * Demo implementation: should be made more general using BoundingConstraint */ template - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; typedef boost::shared_ptr > shared_ptr; ScalarCoordConstraint1(const Key& key, double c, @@ -75,8 +75,8 @@ namespace gtsam { * to be less than or equal to a bound */ template - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0) : Base(key1, key2, range_bound, false, mu) {} @@ -98,8 +98,8 @@ namespace gtsam { * NOTE: this is not a convex function! Be careful with initialization. */ template - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0) : Base(key1, key2, range_bound, true, mu) {} diff --git a/slam/simulated2DOriented.cpp b/slam/simulated2DOriented.cpp index e4c0e4d54..a840f8bd3 100644 --- a/slam/simulated2DOriented.cpp +++ b/slam/simulated2DOriented.cpp @@ -10,7 +10,7 @@ namespace gtsam { using namespace simulated2DOriented; -// INSTANTIATE_LIE_CONFIG(PointKey, Point2) +// INSTANTIATE_LIE_CONFIG(PointKey) // INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) // INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/simulated2DOriented.h b/slam/simulated2DOriented.h index db378f465..28c4969e7 100644 --- a/slam/simulated2DOriented.h +++ b/slam/simulated2DOriented.h @@ -21,8 +21,8 @@ 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 LieConfig PoseConfig; + typedef LieConfig PointConfig; typedef TupleConfig2 Config; //TODO:: point prior is not implemented right now @@ -48,12 +48,12 @@ namespace gtsam { * Unary factor encoding a soft prior on a vector */ template - struct GenericPosePrior: public NonlinearFactor1 { + struct GenericPosePrior: public NonlinearFactor1 { Pose2 z_; GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) : - NonlinearFactor1 (model, key), z_(z) { + NonlinearFactor1 (model, key), z_(z) { } Vector evaluateError(const Pose2& x, boost::optional H = @@ -67,13 +67,12 @@ namespace gtsam { * Binary factor simulating "odometry" between two Vectors */ template - struct GenericOdometry: public NonlinearFactor2 { + struct GenericOdometry: public NonlinearFactor2 { Pose2 z_; GenericOdometry(const Pose2& z, const SharedGaussian& model, const Key& i1, const Key& i2) : - NonlinearFactor2 (model, i1, i2), z_(z) { + NonlinearFactor2 (model, i1, i2), z_(z) { } Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional< diff --git a/slam/smallExample.cpp b/slam/smallExample.cpp index d2941f0c6..e80dcaff0 100644 --- a/slam/smallExample.cpp +++ b/slam/smallExample.cpp @@ -183,14 +183,13 @@ namespace example { } struct UnaryFactor: public gtsam::NonlinearFactor1 { + simulated2D::PoseKey> { Point2 z_; UnaryFactor(const Point2& z, const SharedGaussian& model, const simulated2D::PoseKey& key) : - gtsam::NonlinearFactor1(model, key), z_(z) { + gtsam::NonlinearFactor1(model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional A = diff --git a/slam/visualSLAM.h b/slam/visualSLAM.h index 61833253c..a9b9fc33a 100644 --- a/slam/visualSLAM.h +++ b/slam/visualSLAM.h @@ -24,13 +24,13 @@ namespace gtsam { namespace visualSLAM { */ typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef LieConfig PoseConfig; - typedef LieConfig PointConfig; + typedef LieConfig PoseConfig; + typedef LieConfig PointConfig; typedef TupleConfig2 Config; typedef boost::shared_ptr shared_config; - typedef NonlinearEquality PoseConstraint; - typedef NonlinearEquality PointConstraint; + typedef NonlinearEquality PoseConstraint; + typedef NonlinearEquality PointConstraint; /** @@ -38,7 +38,7 @@ namespace gtsam { namespace visualSLAM { * i.e. the main building block for visual SLAM. */ template - class GenericProjectionFactor : public NonlinearFactor2, Testable > { + class GenericProjectionFactor : public NonlinearFactor2, Testable > { protected: // Keep a copy of measurement and calibration for I/O @@ -48,7 +48,7 @@ namespace gtsam { namespace visualSLAM { public: // shorthand for base class type - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; // shorthand for a smart pointer to a factor typedef boost::shared_ptr > shared_ptr; diff --git a/tests/testFusionTupleConfig.cpp b/tests/testFusionTupleConfig.cpp index b16994b81..3bb73ccad 100644 --- a/tests/testFusionTupleConfig.cpp +++ b/tests/testFusionTupleConfig.cpp @@ -31,8 +31,8 @@ static const double tol = 1e-5; typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; -typedef LieConfig PoseConfig; -typedef LieConfig PointConfig; +typedef LieConfig PoseConfig; +typedef LieConfig PointConfig; // some generic poses, points and keys PoseKey x1(1), x2(2); @@ -484,8 +484,8 @@ TEST( testFusionTupleConfig, basic_factor) // Factors // typedef PriorFactor Prior; // fails to add to graph - typedef PriorFactor Prior; - typedef BetweenFactor Odometry; + typedef PriorFactor Prior; + typedef BetweenFactor Odometry; typedef BearingRangeFactor BearingRange; PoseKey pose1k(1), pose2k(2), pose3k(3); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index f39bde016..40750eea7 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -5,14 +5,11 @@ #include -#define GTSAM_MAGIC_KEY - #include #include #include -#include -#include #include +#include #include #include @@ -21,37 +18,31 @@ using namespace std; using namespace gtsam; -typedef NonlinearEquality NLE; -typedef boost::shared_ptr shared_nle; - typedef TypedSymbol PoseKey; -typedef LieConfig PoseConfig; -typedef PriorFactor PosePrior; -typedef NonlinearEquality PoseNLE; +typedef LieConfig PoseConfig; +typedef PriorFactor PosePrior; +typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; typedef NonlinearFactorGraph PoseGraph; typedef NonlinearOptimizer PoseOptimizer; -bool vector_compare(const Vector& a, const Vector& b) { - return equal_with_abs_tol(a, b, 1e-5); -} +PoseKey key(1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { - Symbol key = "x"; - Vector value = Vector_(2, 1.0, 2.0); - VectorConfig linearize; + Pose2 value = Pose2(2.1, 1.0, 2.0); + PoseConfig linearize; linearize.insert(key, value); // create a nonlinear equality constraint - shared_nle nle(new NLE(key, value,vector_compare)); + shared_poseNLE nle(new PoseNLE(key, value)); // check linearize - SharedDiagonal constraintModel = noiseModel::Constrained::All(2); - GaussianFactor expLF(key, eye(2), zero(2), constraintModel); + SharedDiagonal constraintModel = noiseModel::Constrained::All(3); + GaussianFactor expLF(key, eye(3), zero(3), constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); - CHECK(assert_equal(*actualLF, expLF)); + EXPECT(assert_equal(*actualLF, expLF)); } /* ********************************************************************** */ @@ -66,19 +57,18 @@ TEST ( NonlinearEquality, linearization_pose ) { shared_poseNLE nle(new PoseNLE(key, value)); GaussianFactor::shared_ptr actualLF = nle->linearize(config); - CHECK(true); + EXPECT(true); } /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail ) { - Symbol key = "x"; - Vector value = Vector_(2, 1.0, 2.0); - Vector wrong = Vector_(2, 3.0, 4.0); - VectorConfig bad_linearize; + Pose2 value = Pose2(2.1, 1.0, 2.0); + Pose2 wrong = Pose2(2.1, 3.0, 4.0); + PoseConfig bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint - shared_nle nle(new NLE(key, value,vector_compare)); + shared_poseNLE nle(new PoseNLE(key, value)); // check linearize to ensure that it fails for bad linearization points CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); @@ -118,67 +108,37 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { /* ************************************************************************* */ TEST ( NonlinearEquality, error ) { - Symbol key = "x"; - Vector value = Vector_(2, 1.0, 2.0); - Vector wrong = Vector_(2, 3.0, 4.0); - VectorConfig feasible, bad_linearize; + Pose2 value = Pose2(2.1, 1.0, 2.0); + Pose2 wrong = Pose2(2.1, 3.0, 4.0); + PoseConfig feasible, bad_linearize; feasible.insert(key, value); bad_linearize.insert(key, wrong); // create a nonlinear equality constraint - shared_nle nle(new NLE(key, value,vector_compare)); + shared_poseNLE nle(new PoseNLE(key, value)); // check error function outputs Vector actual = nle->unwhitenedError(feasible); - CHECK(assert_equal(actual, zero(2))); + EXPECT(assert_equal(actual, zero(3))); actual = nle->unwhitenedError(bad_linearize); - CHECK(assert_equal(actual, repeat(2, std::numeric_limits::infinity()))); + EXPECT(assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); } /* ************************************************************************* */ TEST ( NonlinearEquality, equals ) { - string key1 = "x"; - Vector value1 = Vector_(2, 1.0, 2.0); - Vector value2 = Vector_(2, 3.0, 4.0); + Pose2 value1 = Pose2(2.1, 1.0, 2.0); + Pose2 value2 = Pose2(2.1, 3.0, 4.0); // create some constraints to compare - shared_nle nle1(new NLE(key1, value1,vector_compare)); - shared_nle nle2(new NLE(key1, value1,vector_compare)); - shared_nle nle3(new NLE(key1, value2,vector_compare)); + shared_poseNLE nle1(new PoseNLE(key, value1)); + shared_poseNLE nle2(new PoseNLE(key, value1)); + shared_poseNLE nle3(new PoseNLE(key, value2)); // verify - CHECK(nle1->equals(*nle2)); // basic equality = true - CHECK(nle2->equals(*nle1)); // test symmetry of equals() - CHECK(!nle1->equals(*nle3)); // test config -} - -/* ************************************************************************* */ -TEST ( NonlinearEquality, allow_error_vector ) { - Symbol key1 = "x"; - Vector feasible1 = Vector_(3, 1.0, 2.0, 3.0); - double error_gain = 500.0; - NLE nle(key1, feasible1, error_gain,vector_compare); - - // the unwhitened error should provide logmap to the feasible state - Vector badPoint1 = Vector_(3, 0.0, 2.0, 3.0); - Vector actVec = nle.evaluateError(badPoint1); - Vector expVec = Vector_(3, 1.0, 0.0, 0.0); - CHECK(assert_equal(expVec, actVec)); - - // the actual error should have a gain on it - VectorConfig config; - config.insert(key1, badPoint1); - double actError = nle.error(config); - DOUBLES_EQUAL(500.0, actError, 1e-9); - - // check linearization - GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 = eye(3,3); - Vector b = expVec; - SharedDiagonal model = noiseModel::Constrained::All(3); - GaussianFactor::shared_ptr expLinFactor(new GaussianFactor(key1, A1, b, model)); - CHECK(assert_equal(*expLinFactor, *actLinFactor)); + EXPECT(nle1->equals(*nle2)); // basic equality = true + EXPECT(nle2->equals(*nle1)); // test symmetry of equals() + EXPECT(!nle1->equals(*nle3)); // test config } /* ************************************************************************* */ @@ -192,7 +152,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { Pose2 badPoint1(0.0, 2.0, 3.0); Vector actVec = nle.evaluateError(badPoint1); Vector expVec = Vector_(3, -0.989992, -0.14112, 0.0); - CHECK(assert_equal(expVec, actVec, 1e-5)); + EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it PoseConfig config; @@ -206,7 +166,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); GaussianFactor::shared_ptr expLinFactor(new GaussianFactor(key1, A1, b, model)); - CHECK(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); + EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); } /* ************************************************************************* */ @@ -236,7 +196,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // verify PoseConfig expected; expected.insert(key1, feasible1); - CHECK(assert_equal(expected, *result.config())); + EXPECT(assert_equal(expected, *result.config())); } /* ************************************************************************* */ @@ -273,7 +233,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // verify PoseConfig expected; expected.insert(key1, feasible1); - CHECK(assert_equal(expected, *result.config())); + EXPECT(assert_equal(expected, *result.config())); } /* ************************************************************************* */ diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp index e59fa976f..a9bea9a1f 100644 --- a/tests/testNonlinearEqualityConstraint.cpp +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -285,7 +285,7 @@ typedef visualSLAM::Graph VGraph; typedef NonlinearOptimizer VOptimizer; // factors for visual slam -typedef NonlinearEquality2 Point3Equality; +typedef NonlinearEquality2 Point3Equality; /* ********************************************************************* */ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { diff --git a/tests/testTransformConstraint.cpp b/tests/testTransformConstraint.cpp index 219f2c161..03d5bc4e4 100644 --- a/tests/testTransformConstraint.cpp +++ b/tests/testTransformConstraint.cpp @@ -33,19 +33,19 @@ typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; typedef TypedSymbol TransformKey; -typedef LieConfig PoseConfig; -typedef LieConfig PointConfig; -typedef LieConfig TransformConfig; +typedef LieConfig PoseConfig; +typedef LieConfig PointConfig; +typedef LieConfig TransformConfig; typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig; typedef NonlinearFactorGraph DDFGraph; typedef NonlinearOptimizer Optimizer; -typedef NonlinearEquality PoseConstraint; -typedef NonlinearEquality PointConstraint; -typedef NonlinearEquality TransformPriorConstraint; +typedef NonlinearEquality PoseConstraint; +typedef NonlinearEquality PointConstraint; +typedef NonlinearEquality TransformPriorConstraint; -typedef TransformConstraint PointTransformConstraint; +typedef TransformConstraint PointTransformConstraint; PointKey lA1(1), lA2(2), lB1(3); TransformKey t1(1); diff --git a/tests/testTupleConfig.cpp b/tests/testTupleConfig.cpp index 78394ec63..ceeb0e33e 100644 --- a/tests/testTupleConfig.cpp +++ b/tests/testTupleConfig.cpp @@ -26,8 +26,8 @@ static const double tol = 1e-5; typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; -typedef LieConfig PoseConfig; -typedef LieConfig PointConfig; +typedef LieConfig PoseConfig; +typedef LieConfig PointConfig; typedef TupleConfig2 Config; /* ************************************************************************* */ @@ -202,12 +202,12 @@ 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 LieConfig PoseConfig; +typedef LieConfig PointConfig; +typedef LieConfig LamConfig; +typedef LieConfig Pose3Config; +typedef LieConfig Point3Config; +typedef LieConfig Point3Config2; // some TupleConfig types typedef TupleConfig > ConfigA;