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;