12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
Alex Cunningham 2010-08-23 19:44:17 +00:00
parent 86f812ebb5
commit 77eda5ab8c
35 changed files with 406 additions and 382 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
base/Lie-inl.h
View File

@ -18,11 +18,6 @@
template class Lie<T>; template class Lie<T>;
namespace gtsam { namespace gtsam {
// template<class T>
// size_t Lie<T>::dim() const {
// return gtsam::dim(*((T*)this));
// }
/** /**
* Returns Exponential mapy * Returns Exponential mapy
* This is for matlab wrapper * This is for matlab wrapper

12
nonlinear/FusionTupleConfig.h
View File

@ -169,7 +169,7 @@ public:
/** insertion */ /** insertion */
template <class Key, class Value> template <class Key, class Value>
void insert(const Key& j, const Value& x) { void insert(const Key& j, const Value& x) {
config_<LieConfig<Key, Value> >().insert(j,x); config_<LieConfig<Key> >().insert(j,x);
} }
/** insert a full config at a time */ /** insert a full config at a time */
@ -204,25 +204,25 @@ public:
*/ */
template<class Key, class Value> template<class Key, class Value>
void update(const Key& key, const Value& value) { void update(const Key& key, const Value& value) {
config_<LieConfig<Key,typename Key::Value_t> >().update(key,value); config_<LieConfig<Key> >().update(key,value);
} }
/** check if a given element exists */ /** check if a given element exists */
template<class Key> template<class Key>
bool exists(const Key& j) const { bool exists(const Key& j) const {
return config<LieConfig<Key,typename Key::Value_t> >().exists(j); return config<LieConfig<Key> >().exists(j);
} }
/** a variant of exists */ /** a variant of exists */
template<class Key> template<class Key>
boost::optional<typename Key::Value_t> exists_(const Key& j) const { boost::optional<typename Key::Value_t> exists_(const Key& j) const {
return config<LieConfig<Key,typename Key::Value_t> >().exists_(j); return config<LieConfig<Key> >().exists_(j);
} }
/** retrieve a point */ /** retrieve a point */
template<class Key> template<class Key>
const typename Key::Value_t & at(const Key& j) const { const typename Key::Value_t & at(const Key& j) const {
return config<LieConfig<Key, typename Key::Value_t> >().at(j); return config<LieConfig<Key> >().at(j);
} }
/** access operator */ /** access operator */
@ -255,7 +255,7 @@ public:
/** erases a specific key */ /** erases a specific key */
template<class Key> template<class Key>
void erase(const Key& j) { void erase(const Key& j) {
config_<LieConfig<Key,typename Key::Value_t> >().erase(j); config_<LieConfig<Key> >().erase(j);
} }
/** clears the config */ /** clears the config */

86
nonlinear/LieConfig-inl.h
View File

@ -19,27 +19,27 @@
#include <gtsam/nonlinear/LieConfig.h> #include <gtsam/nonlinear/LieConfig.h>
#define INSTANTIATE_LIE_CONFIG(J,T) \ #define INSTANTIATE_LIE_CONFIG(J) \
/*INSTANTIATE_LIE(T);*/ \ /*INSTANTIATE_LIE(T);*/ \
template LieConfig<J,T> expmap(const LieConfig<J,T>&, const VectorConfig&); \ template LieConfig<J> expmap(const LieConfig<J>&, const VectorConfig&); \
template LieConfig<J,T> expmap(const LieConfig<J,T>&, const Vector&); \ template LieConfig<J> expmap(const LieConfig<J>&, const Vector&); \
template VectorConfig logmap(const LieConfig<J,T>&, const LieConfig<J,T>&); \ template VectorConfig logmap(const LieConfig<J>&, const LieConfig<J>&); \
template class LieConfig<J,T>; template class LieConfig<J>;
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
template<class J, class T> template<class J>
void LieConfig<J,T>::print(const string &s) const { void LieConfig<J>::print(const string &s) const {
cout << "LieConfig " << s << ", size " << values_.size() << "\n"; cout << "LieConfig " << s << ", size " << values_.size() << "\n";
BOOST_FOREACH(const typename Values::value_type& v, values_) { BOOST_FOREACH(const typename Values::value_type& v, values_) {
gtsam::print(v.second, (string)(v.first)); gtsam::print(v.second, (string)(v.first));
} }
} }
template<class J, class T> template<class J>
bool LieConfig<J,T>::equals(const LieConfig<J,T>& expected, double tol) const { bool LieConfig<J>::equals(const LieConfig<J>& expected, double tol) const {
if (values_.size() != expected.values_.size()) return false; if (values_.size() != expected.values_.size()) return false;
BOOST_FOREACH(const typename Values::value_type& v, values_) { BOOST_FOREACH(const typename Values::value_type& v, values_) {
if (!expected.exists(v.first)) return false; if (!expected.exists(v.first)) return false;
@ -49,69 +49,69 @@ namespace gtsam {
return true; return true;
} }
template<class J, class T> template<class J>
const T& LieConfig<J,T>::at(const J& j) const { const typename J::Value_t& LieConfig<J>::at(const J& j) const {
const_iterator it = values_.find(j); const_iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
else return it->second; else return it->second;
} }
template<class J, class T> template<class J>
size_t LieConfig<J,T>::dim() const { size_t LieConfig<J>::dim() const {
size_t n = 0; size_t n = 0;
BOOST_FOREACH(const typename Values::value_type& value, values_) BOOST_FOREACH(const typename Values::value_type& value, values_)
n += gtsam::dim(value.second); n += gtsam::dim(value.second);
return n; return n;
} }
template<class J, class T> template<class J>
VectorConfig LieConfig<J,T>::zero() const { VectorConfig LieConfig<J>::zero() const {
VectorConfig z; VectorConfig z;
BOOST_FOREACH(const typename Values::value_type& value, values_) BOOST_FOREACH(const typename Values::value_type& value, values_)
z.insert(value.first,gtsam::zero(gtsam::dim(value.second))); z.insert(value.first,gtsam::zero(gtsam::dim(value.second)));
return z; return z;
} }
template<class J, class T> template<class J>
void LieConfig<J,T>::insert(const J& name, const T& val) { void LieConfig<J>::insert(const J& name, const typename J::Value_t& val) {
values_.insert(make_pair(name, val)); values_.insert(make_pair(name, val));
} }
template<class J, class T> template<class J>
void LieConfig<J,T>::insert(const LieConfig<J,T>& cfg) { void LieConfig<J>::insert(const LieConfig<J>& cfg) {
BOOST_FOREACH(const typename Values::value_type& v, cfg.values_) BOOST_FOREACH(const typename Values::value_type& v, cfg.values_)
insert(v.first, v.second); insert(v.first, v.second);
} }
template<class J, class T> template<class J>
void LieConfig<J,T>::update(const LieConfig<J,T>& cfg) { void LieConfig<J>::update(const LieConfig<J>& cfg) {
BOOST_FOREACH(const typename Values::value_type& v, values_) { BOOST_FOREACH(const typename Values::value_type& v, values_) {
boost::optional<T> t = cfg.exists_(v.first); boost::optional<typename J::Value_t> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t; if (t) values_[v.first] = *t;
} }
} }
template<class J, class T> template<class J>
void LieConfig<J,T>::update(const J& j, const T& val) { void LieConfig<J>::update(const J& j, const typename J::Value_t& val) {
values_[j] = val; values_[j] = val;
} }
template<class J, class T> template<class J>
std::list<J> LieConfig<J,T>::keys() const { std::list<J> LieConfig<J>::keys() const {
std::list<J> ret; std::list<J> ret;
BOOST_FOREACH(const typename Values::value_type& v, values_) BOOST_FOREACH(const typename Values::value_type& v, values_)
ret.push_back(v.first); ret.push_back(v.first);
return ret; return ret;
} }
template<class J, class T> template<class J>
void LieConfig<J,T>::erase(const J& j) { void LieConfig<J>::erase(const J& j) {
size_t dim; // unused size_t dim; // unused
erase(j, dim); erase(j, dim);
} }
template<class J, class T> template<class J>
void LieConfig<J,T>::erase(const J& j, size_t& dim) { void LieConfig<J>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j); iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
dim = gtsam::dim(it->second); dim = gtsam::dim(it->second);
@ -119,13 +119,13 @@ namespace gtsam {
} }
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
template<class J, class T> template<class J>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) { LieConfig<J> expmap(const LieConfig<J>& c, const VectorConfig& delta) {
LieConfig<J,T> newConfig; LieConfig<J> newConfig;
typedef pair<J,T> KeyValue; typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, c) { BOOST_FOREACH(const KeyValue& value, c) {
const J& j = value.first; const J& j = value.first;
const T& pj = value.second; const typename J::Value_t& pj = value.second;
Symbol jkey = (Symbol)j; Symbol jkey = (Symbol)j;
if (delta.contains(jkey)) { if (delta.contains(jkey)) {
const Vector& dj = delta[jkey]; const Vector& dj = delta[jkey];
@ -137,18 +137,18 @@ namespace gtsam {
} }
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
template<class J, class T> template<class J>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta) { LieConfig<J> expmap(const LieConfig<J>& c, const Vector& delta) {
if(delta.size() != dim(c)) { if(delta.size() != dim(c)) {
cout << "LieConfig::dim (" << dim(c) << ") <> delta.size (" << delta.size() << ")" << endl; cout << "LieConfig::dim (" << dim(c) << ") <> delta.size (" << delta.size() << ")" << endl;
throw invalid_argument("Delta vector length does not match config dimensionality."); throw invalid_argument("Delta vector length does not match config dimensionality.");
} }
LieConfig<J,T> newConfig; LieConfig<J> newConfig;
int delta_offset = 0; int delta_offset = 0;
typedef pair<J,T> KeyValue; typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, c) { BOOST_FOREACH(const KeyValue& value, c) {
const J& j = value.first; const J& j = value.first;
const T& pj = value.second; const typename J::Value_t& pj = value.second;
int cur_dim = dim(pj); int cur_dim = dim(pj);
newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim))); newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim)));
delta_offset += cur_dim; delta_offset += cur_dim;
@ -158,10 +158,10 @@ namespace gtsam {
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs // todo: currently only logmaps elements in both configs
template<class J, class T> template<class J>
VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp) { VectorConfig logmap(const LieConfig<J>& c0, const LieConfig<J>& cp) {
VectorConfig delta; VectorConfig delta;
typedef pair<J,T> KeyValue; typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
if(c0.exists(value.first)) if(c0.exists(value.first))
delta.insert(value.first, delta.insert(value.first,

46
nonlinear/LieConfig.h
View File

@ -32,11 +32,15 @@ namespace gtsam {
/** /**
* Lie type configuration * Lie type configuration
* Takes two template types * Takes two template types
* J: a type to look up values in the configuration (need to be sortable) * J: a key type to look up values in the configuration (need to be sortable)
* T: the type of values being stored in the configuration *
* 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 J, class T> template<class J>
class LieConfig : public Testable<LieConfig<J, T> > { class LieConfig : public Testable<LieConfig<J> > {
public: public:
@ -44,8 +48,8 @@ namespace gtsam {
* Typedefs * Typedefs
*/ */
typedef J Key; typedef J Key;
typedef T Value; typedef typename J::Value_t Value;
typedef std::map<J, T> Values; typedef std::map<J,Value> Values;
typedef typename Values::iterator iterator; typedef typename Values::iterator iterator;
typedef typename Values::const_iterator const_iterator; typedef typename Values::const_iterator const_iterator;
@ -58,8 +62,8 @@ namespace gtsam {
LieConfig() {} LieConfig() {}
LieConfig(const LieConfig& config) : LieConfig(const LieConfig& config) :
values_(config.values_) {} values_(config.values_) {}
template<class J_alt, class T_alt> template<class J_alt>
LieConfig(const LieConfig<J_alt,T_alt>& other) {} // do nothing when initializing with wrong type LieConfig(const LieConfig<J_alt>& other) {} // do nothing when initializing with wrong type
virtual ~LieConfig() {} virtual ~LieConfig() {}
/** print */ /** print */
@ -69,16 +73,16 @@ namespace gtsam {
bool equals(const LieConfig& expected, double tol=1e-9) const; bool equals(const LieConfig& expected, double tol=1e-9) const;
/** Retrieve a variable by j, throws std::invalid_argument if not found */ /** 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 */ /** 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 */ /** Check if a variable exists */
bool exists(const J& i) const { return values_.find(i)!=values_.end(); } bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
/** Check if a variable exists and return it if so */ /** Check if a variable exists and return it if so */
boost::optional<T> exists_(const J& i) const { boost::optional<Value> exists_(const J& i) const {
const_iterator it = values_.find(i); const_iterator it = values_.find(i);
if (it==values_.end()) return boost::none; else return it->second; if (it==values_.end()) return boost::none; else return it->second;
} }
@ -103,7 +107,7 @@ namespace gtsam {
// imperative methods: // imperative methods:
/** Add a variable with the given j - does not replace existing values */ /** 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 */ /** Add a set of variables - does note replace existing values */
void insert(const LieConfig& cfg); void insert(const LieConfig& cfg);
@ -112,7 +116,7 @@ namespace gtsam {
void update(const LieConfig& cfg); void update(const LieConfig& cfg);
/** single element change of existing element */ /** 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 */ /** Remove a variable from the config */
void erase(const J& j); void erase(const J& j);
@ -150,20 +154,20 @@ namespace gtsam {
}; };
/** Dimensionality of the tangent space */ /** Dimensionality of the tangent space */
template<class J, class T> template<class J>
inline size_t dim(const LieConfig<J,T>& c) { return c.dim(); } inline size_t dim(const LieConfig<J>& c) { return c.dim(); }
/** Add a delta config */ /** Add a delta config */
template<class J, class T> template<class J>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta); LieConfig<J> expmap(const LieConfig<J>& c, const VectorConfig& delta);
/** Add a delta vector, uses iterator order */ /** Add a delta vector, uses iterator order */
template<class J, class T> template<class J>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta); LieConfig<J> expmap(const LieConfig<J>& c, const Vector& delta);
/** Get a delta config about a linearization point c0 */ /** Get a delta config about a linearization point c0 */
template<class J, class T> template<class J>
VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp); VectorConfig logmap(const LieConfig<J>& c0, const LieConfig<J>& cp);
} }

83
nonlinear/NonlinearConstraint.h
View File

@ -95,11 +95,14 @@ public:
/** /**
* A unary constraint that defaults to an equality constraint * A unary constraint that defaults to an equality constraint
*/ */
template <class Config, class Key, class X> template <class Config, class Key>
class NonlinearConstraint1 : public NonlinearConstraint<Config> { class NonlinearConstraint1 : public NonlinearConstraint<Config> {
public:
typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearConstraint1<Config,Key,X> This; typedef NonlinearConstraint1<Config,Key> This;
typedef NonlinearConstraint<Config> Base; typedef NonlinearConstraint<Config> Base;
/** key for the constrained variable */ /** key for the constrained variable */
@ -165,12 +168,15 @@ public:
/** /**
* Unary Equality constraint - simply forces the value of active() to true * Unary Equality constraint - simply forces the value of active() to true
*/ */
template <class Config, class Key, class X> template <class Config, class Key>
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key, X> { class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key> {
public:
typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint1<Config,Key,X> This; typedef NonlinearEqualityConstraint1<Config,Key> This;
typedef NonlinearConstraint1<Config,Key,X> Base; typedef NonlinearConstraint1<Config,Key> Base;
public: public:
NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0) 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 * A binary constraint with arbitrary cost and jacobian functions
*/ */
template <class Config, class Key1, class X1, class Key2, class X2> template <class Config, class Key1, class Key2>
class NonlinearConstraint2 : public NonlinearConstraint<Config> { class NonlinearConstraint2 : public NonlinearConstraint<Config> {
public:
typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2;
protected: protected:
typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> This; typedef NonlinearConstraint2<Config,Key1,Key2> This;
typedef NonlinearConstraint<Config> Base; typedef NonlinearConstraint<Config> Base;
/** keys for the constrained variables */ /** keys for the constrained variables */
@ -261,12 +271,16 @@ public:
/** /**
* Binary Equality constraint - simply forces the value of active() to true * Binary Equality constraint - simply forces the value of active() to true
*/ */
template <class Config, class Key1, class X1, class Key2, class X2> template <class Config, class Key1, class Key2>
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, X1, Key2, X2> { class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, Key2> {
public:
typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2;
protected: protected:
typedef NonlinearEqualityConstraint2<Config,Key1,X1,Key2,X2> This; typedef NonlinearEqualityConstraint2<Config,Key1,Key2> This;
typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> Base; typedef NonlinearConstraint2<Config,Key1,Key2> Base;
public: public:
NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0)
@ -281,11 +295,16 @@ public:
/** /**
* A ternary constraint * A ternary constraint
*/ */
template <class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3> template <class Config, class Key1, class Key2, class Key3>
class NonlinearConstraint3 : public NonlinearConstraint<Config> { class NonlinearConstraint3 : public NonlinearConstraint<Config> {
public:
typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2;
typedef typename Key3::Value_t X3;
protected: protected:
typedef NonlinearConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> This; typedef NonlinearConstraint3<Config,Key1,Key2,Key3> This;
typedef NonlinearConstraint<Config> Base; typedef NonlinearConstraint<Config> Base;
/** keys for the constrained variables */ /** keys for the constrained variables */
@ -366,12 +385,17 @@ public:
/** /**
* Ternary Equality constraint - simply forces the value of active() to true * Ternary Equality constraint - simply forces the value of active() to true
*/ */
template <class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3> template <class Config, class Key1, class Key2, class Key3>
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Config, Key1, X1, Key2, X2, Key3, X3> { class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Config, Key1, Key2, Key3> {
public:
typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2;
typedef typename Key3::Value_t X3;
protected: protected:
typedef NonlinearEqualityConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> This; typedef NonlinearEqualityConstraint3<Config,Key1,Key2,Key3> This;
typedef NonlinearConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> Base; typedef NonlinearConstraint3<Config,Key1,Key2,Key3> Base;
public: public:
NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3, 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 * Simple unary equality constraint - fixes a value for a variable
*/ */
template<class Config, class Key, class X> template<class Config, class Key>
class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key, X> { class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key> {
public:
typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint1<Config, Key, X> Base; typedef NonlinearEqualityConstraint1<Config, Key> Base;
X value_; /// fixed value for variable X value_; /// fixed value for variable
public: public:
typedef boost::shared_ptr<NonlinearEquality1<Config, Key, X> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality1<Config, Key> > shared_ptr;
NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0)
: Base(key1, X::Dim(), mu), value_(value) {} : Base(key1, X::Dim(), mu), value_(value) {}
@ -415,14 +443,17 @@ public:
* Simple binary equality constraint - this constraint forces two factors to * Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type * be the same. This constraint requires the underlying type to a Lie type
*/ */
template<class Config, class Key, class X> template<class Config, class Key>
class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> { class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, Key> {
public:
typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> Base; typedef NonlinearEqualityConstraint2<Config, Key, Key> Base;
public: public:
typedef boost::shared_ptr<NonlinearEquality2<Config, Key, X> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality2<Config, Key> > shared_ptr;
NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu) {} : Base(key1, key2, X::Dim(), mu) {}

14
nonlinear/NonlinearEquality.h
View File

@ -30,8 +30,12 @@ namespace gtsam {
* - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain * - 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 * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
*/ */
template<class Config, class Key, class T> template<class Config, class Key>
class NonlinearEquality: public NonlinearFactor1<Config, Key, T> { class NonlinearEquality: public NonlinearFactor1<Config, Key> {
public:
typedef typename Key::Value_t T;
private: private:
// feasible value // feasible value
@ -50,7 +54,7 @@ namespace gtsam {
*/ */
bool (*compare_)(const T& a, const T& b); bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<Config, Key, T> Base; typedef NonlinearFactor1<Config, Key> Base;
/** /**
* Constructor - forces exact evaluation * Constructor - forces exact evaluation
@ -78,8 +82,8 @@ namespace gtsam {
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const { bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const NonlinearEquality<Config,Key,T>* p = const NonlinearEquality<Config,Key>* p =
dynamic_cast<const NonlinearEquality<Config,Key,T>*> (&f); dynamic_cast<const NonlinearEquality<Config,Key>*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
if (!Base::equals(*p)) return false; if (!Base::equals(*p)) return false;
return compare_(feasible_, p->feasible_); return compare_(feasible_, p->feasible_);

32
nonlinear/NonlinearFactor.h
View File

@ -139,16 +139,21 @@ namespace gtsam {
* the derived class implements error_vector(c) = h(x)-z \approx Ax-b * 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. * This allows a graph to have factors with measurements of mixed type.
*/ */
template<class Config, class Key, class X> template<class Config, class Key>
class NonlinearFactor1: public NonlinearFactor<Config> { class NonlinearFactor1: public NonlinearFactor<Config> {
public:
// typedefs for value types pulled from keys
typedef typename Key::Value_t X;
protected: protected:
// The value of the key. Not const to allow serialization // The value of the key. Not const to allow serialization
Key key_; Key key_;
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor1<Config, Key, X> This; typedef NonlinearFactor1<Config, Key> This;
public: public:
@ -237,9 +242,15 @@ namespace gtsam {
/** /**
* A Gaussian nonlinear factor that takes 2 parameters * A Gaussian nonlinear factor that takes 2 parameters
*/ */
template<class Config, class Key1, class X1, class Key2, class X2> template<class Config, class Key1, class Key2>
class NonlinearFactor2: public NonlinearFactor<Config> { class NonlinearFactor2: public NonlinearFactor<Config> {
public:
// typedefs for value types pulled from keys
typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2;
protected: protected:
// The values of the keys. Not const to allow serialization // The values of the keys. Not const to allow serialization
@ -247,7 +258,7 @@ namespace gtsam {
Key2 key2_; Key2 key2_;
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor2<Config, Key1, X1, Key2, X2> This; typedef NonlinearFactor2<Config, Key1, Key2> This;
public: public:
@ -352,18 +363,25 @@ namespace gtsam {
/** /**
* A Gaussian nonlinear factor that takes 3 parameters * A Gaussian nonlinear factor that takes 3 parameters
*/ */
template<class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3> template<class Config, class Key1, class Key2, class Key3>
class NonlinearFactor3: public NonlinearFactor<Config> { class NonlinearFactor3: public NonlinearFactor<Config> {
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: protected:
// The values of the keys. Not const to allow serialization // The values of the keys. Not const to allow serialization
Key1 key1_; Key1 key1_;
Key2 key2_; Key2 key2_;
Key3 key3_; Key3 key3_;
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor3<Config, Key1, X1, Key2, X2, Key3, X3> This; typedef NonlinearFactor3<Config, Key1, Key2, Key3> This;
public: public:

154
nonlinear/tests/testLieConfig.cpp
View File

@ -11,34 +11,37 @@
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/LieConfig-inl.h> #include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/LieVector.h>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
static double inf = std::numeric_limits<double>::infinity(); static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<Vector, 'v'> VecKey;
typedef LieConfig<VecKey> Config;
VecKey key1(1), key2(2), key3(3), key4(4);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieConfig, equals1 ) TEST( LieConfig, equals1 )
{ {
LieConfig<string,Vector> expected; Config expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0); Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert("a",v); expected.insert(key1,v);
LieConfig<string,Vector> actual; Config actual;
actual.insert("a",v); actual.insert(key1,v);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieConfig, equals2 ) TEST( LieConfig, equals2 )
{ {
LieConfig<string,Vector> cfg1, cfg2; Config cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0); Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
cfg1.insert("x", v1); cfg1.insert(key1, v1);
cfg2.insert("x", v2); cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2)); CHECK(!cfg1.equals(cfg2));
CHECK(!cfg2.equals(cfg1)); CHECK(!cfg2.equals(cfg1));
} }
@ -46,11 +49,11 @@ TEST( LieConfig, equals2 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieConfig, equals_nan ) TEST( LieConfig, equals_nan )
{ {
LieConfig<string,Vector> cfg1, cfg2; Config cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf); Vector v2 = Vector_(3, inf, inf, inf);
cfg1.insert("x", v1); cfg1.insert(key1, v1);
cfg2.insert("x", v2); cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2)); CHECK(!cfg1.equals(cfg2));
CHECK(!cfg2.equals(cfg1)); CHECK(!cfg2.equals(cfg1));
} }
@ -58,22 +61,22 @@ TEST( LieConfig, equals_nan )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieConfig, insert_config ) TEST( LieConfig, insert_config )
{ {
LieConfig<string,Vector> cfg1, cfg2, expected; Config cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0); Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
Vector v4 = Vector_(3, 8.0, 3.0, 7.0); Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
cfg1.insert("x1", v1); cfg1.insert(key1, v1);
cfg1.insert("x2", v2); cfg1.insert(key2, v2);
cfg2.insert("x2", v3); cfg2.insert(key2, v3);
cfg2.insert("x3", v4); cfg2.insert(key3, v4);
cfg1.insert(cfg2); cfg1.insert(cfg2);
expected.insert("x1", v1); expected.insert(key1, v1);
expected.insert("x2", v2); expected.insert(key2, v2);
expected.insert("x2", v3); expected.insert(key2, v3);
expected.insert("x3", v4); expected.insert(key3, v4);
CHECK(assert_equal(cfg1, expected)); CHECK(assert_equal(cfg1, expected));
} }
@ -81,47 +84,47 @@ TEST( LieConfig, insert_config )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieConfig, update_element ) TEST( LieConfig, update_element )
{ {
LieConfig<string,Vector> cfg; Config cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
cfg.insert("x1", v1); cfg.insert(key1, v1);
CHECK(cfg.size() == 1); 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(cfg.size() == 1);
CHECK(assert_equal(v2, cfg.at("x1"))); CHECK(assert_equal(v2, cfg.at(key1)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, dim_zero) TEST(LieConfig, dim_zero)
{ {
LieConfig<string,Vector> config0; Config config0;
config0.insert("v1", Vector_(2, 2.0, 3.0)); config0.insert(key1, Vector_(2, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
LONGS_EQUAL(5,config0.dim()); LONGS_EQUAL(5,config0.dim());
VectorConfig expected; VectorConfig expected;
expected.insert("v1", zero(2)); expected.insert(key1, zero(2));
expected.insert("v2", zero(3)); expected.insert(key2, zero(3));
CHECK(assert_equal(expected, config0.zero())); CHECK(assert_equal(expected, config0.zero()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, expmap_a) TEST(LieConfig, expmap_a)
{ {
LieConfig<string,Vector> config0; Config config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
VectorConfig increment; VectorConfig increment;
increment.insert("v1", Vector_(3, 1.0, 1.1, 1.2)); increment.insert(key1, Vector_(3, 1.0, 1.1, 1.2));
increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5)); increment.insert(key2, Vector_(3, 1.3, 1.4, 1.5));
LieConfig<string,Vector> expected; Config expected;
expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, expmap(config0, increment))); CHECK(assert_equal(expected, expmap(config0, increment)));
} }
@ -129,16 +132,16 @@ TEST(LieConfig, expmap_a)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, expmap_b) TEST(LieConfig, expmap_b)
{ {
LieConfig<string,Vector> config0; Config config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
VectorConfig increment; 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<string,Vector> expected; Config expected;
expected.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, expmap(config0, increment))); CHECK(assert_equal(expected, expmap(config0, increment)));
} }
@ -146,17 +149,17 @@ TEST(LieConfig, expmap_b)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, expmap_c) TEST(LieConfig, expmap_c)
{ {
LieConfig<string,Vector> config0; Config config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
Vector increment = Vector_(6, Vector increment = Vector_(6,
1.0, 1.1, 1.2, 1.0, 1.1, 1.2,
1.3, 1.4, 1.5); 1.3, 1.4, 1.5);
LieConfig<string,Vector> expected; Config expected;
expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, expmap(config0, increment))); CHECK(assert_equal(expected, expmap(config0, increment)));
} }
@ -164,9 +167,9 @@ TEST(LieConfig, expmap_c)
/* ************************************************************************* */ /* ************************************************************************* */
/*TEST(LieConfig, expmap_d) /*TEST(LieConfig, expmap_d)
{ {
LieConfig<string,Vector> config0; Config config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
//config0.print("config0"); //config0.print("config0");
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
@ -204,30 +207,30 @@ TEST(LieConfig, expmap_c)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, exists_) TEST(LieConfig, exists_)
{ {
LieConfig<string,Vector> config0; Config config0;
config0.insert("v1", Vector_(1, 1.)); config0.insert(key1, Vector_(1, 1.));
config0.insert("v2", Vector_(1, 2.)); config0.insert(key2, Vector_(1, 2.));
boost::optional<Vector> v = config0.exists_("v1"); boost::optional<Vector> v = config0.exists_(key1);
CHECK(assert_equal(Vector_(1, 1.),*v)); CHECK(assert_equal(Vector_(1, 1.),*v));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, update) TEST(LieConfig, update)
{ {
LieConfig<string,Vector> config0; Config config0;
config0.insert("v1", Vector_(1, 1.)); config0.insert(key1, Vector_(1, 1.));
config0.insert("v2", Vector_(1, 2.)); config0.insert(key2, Vector_(1, 2.));
LieConfig<string,Vector> superset; Config superset;
superset.insert("v1", Vector_(1, -1.)); superset.insert(key1, Vector_(1, -1.));
superset.insert("v2", Vector_(1, -2.)); superset.insert(key2, Vector_(1, -2.));
superset.insert("v3", Vector_(1, -3.)); superset.insert(key3, Vector_(1, -3.));
config0.update(superset); config0.update(superset);
LieConfig<string,Vector> expected; Config expected;
expected.insert("v1", Vector_(1, -1.)); expected.insert(key1, Vector_(1, -1.));
expected.insert("v2", Vector_(1, -2.)); expected.insert(key2, Vector_(1, -2.));
CHECK(assert_equal(expected,config0)); CHECK(assert_equal(expected,config0));
} }
@ -235,19 +238,18 @@ TEST(LieConfig, update)
TEST(LieConfig, dummy_initialization) TEST(LieConfig, dummy_initialization)
{ {
typedef TypedSymbol<Vector, 'z'> Key; typedef TypedSymbol<Vector, 'z'> Key;
typedef LieConfig<Key,Vector> Config1; typedef LieConfig<Key> Config1;
typedef LieConfig<string,Vector> Config2;
Config1 init1; Config1 init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0)); init1.insert(Key(1), Vector_(2, 1.0, 2.0));
init1.insert(Key(2), Vector_(2, 4.0, 3.0)); init1.insert(Key(2), Vector_(2, 4.0, 3.0));
Config2 init2; Config init2;
init2.insert("v1", Vector_(2, 1.0, 2.0)); init2.insert(key1, Vector_(2, 1.0, 2.0));
init2.insert("v2", Vector_(2, 4.0, 3.0)); init2.insert(key2, Vector_(2, 4.0, 3.0));
Config1 actual1(init2); Config1 actual1(init2);
Config2 actual2(init1); Config actual2(init1);
EXPECT(actual1.empty()); EXPECT(actual1.empty());
EXPECT(actual2.empty()); EXPECT(actual2.empty());

5
slam/BearingFactor.h
View File

@ -16,13 +16,12 @@ namespace gtsam {
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class Config, class PoseKey, class PointKey> template<class Config, class PoseKey, class PointKey>
class BearingFactor: public NonlinearFactor2<Config, PoseKey, Pose2, class BearingFactor: public NonlinearFactor2<Config, PoseKey, PointKey> {
PointKey, Point2> {
private: private:
Rot2 z_; /** measurement */ Rot2 z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base; typedef NonlinearFactor2<Config, PoseKey, PointKey> Base;
public: public:

5
slam/BearingRangeFactor.h
View File

@ -17,15 +17,14 @@ namespace gtsam {
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class Config, class PoseKey, class PointKey> template<class Config, class PoseKey, class PointKey>
class BearingRangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, class BearingRangeFactor: public NonlinearFactor2<Config, PoseKey, PointKey> {
PointKey, Point2> {
private: private:
// the measurement // the measurement
Rot2 bearing_; Rot2 bearing_;
double range_; double range_;
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base; typedef NonlinearFactor2<Config, PoseKey, PointKey> Base;
public: public:

11
slam/BetweenConstraint.h
View File

@ -14,16 +14,19 @@ namespace gtsam {
* Binary between constraint - forces between to a given value * Binary between constraint - forces between to a given value
* This constraint requires the underlying type to a Lie type * This constraint requires the underlying type to a Lie type
*/ */
template<class Config, class Key, class X> template<class Config, class Key>
class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> { class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, Key> {
public:
typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> Base; typedef NonlinearEqualityConstraint2<Config, Key, Key> Base;
X measured_; /// fixed between value X measured_; /// fixed between value
public: public:
typedef boost::shared_ptr<BetweenConstraint<Config, Key, X> > shared_ptr; typedef boost::shared_ptr<BetweenConstraint<Config, Key> > shared_ptr;
BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu), measured_(measured) {} : Base(key1, key2, X::Dim(), mu), measured_(measured) {}

15
slam/BetweenFactor.h
View File

@ -16,13 +16,18 @@ namespace gtsam {
/** /**
* A class for a measurement predicted by "between(config[key1],config[key2])" * 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 * 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 Config, class Key1, class T, class Key2 = Key1> template<class Config, class Key1, class Key2 = Key1>
class BetweenFactor: public NonlinearFactor2<Config, Key1, T, Key2, T> { class BetweenFactor: public NonlinearFactor2<Config, Key1, Key2> {
public:
typedef typename Key1::Value_t T;
private: private:
typedef NonlinearFactor2<Config, Key1, T, Key2, T> Base; typedef NonlinearFactor2<Config, Key1, Key2> Base;
T measured_; /** The measurement */ T measured_; /** The measurement */
@ -47,8 +52,8 @@ namespace gtsam {
/** equals */ /** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const { bool equals(const NonlinearFactor<Config>& expected, double tol) const {
const BetweenFactor<Config, Key1, T, Key2> *e = const BetweenFactor<Config, Key1, Key2> *e =
dynamic_cast<const BetweenFactor<Config, Key1, T>*> (&expected); dynamic_cast<const BetweenFactor<Config, Key1, Key2>*> (&expected);
return e != NULL && Base::equals(expected) && this->measured_.equals( return e != NULL && Base::equals(expected) && this->measured_.equals(
e->measured_, tol); e->measured_, tol);
} }

20
slam/BoundingConstraint.h
View File

@ -17,10 +17,11 @@ namespace gtsam {
* will need to have its value function implemented to return * will need to have its value function implemented to return
* a scalar for comparison. * a scalar for comparison.
*/ */
template<class Cfg, class Key, class X> template<class Cfg, class Key>
struct BoundingConstraint1: public NonlinearConstraint1<Cfg, Key, X> { struct BoundingConstraint1: public NonlinearConstraint1<Cfg, Key> {
typedef NonlinearConstraint1<Cfg, Key, X> Base; typedef typename Key::Value_t X;
typedef boost::shared_ptr<BoundingConstraint1<Cfg, Key, X> > shared_ptr; typedef NonlinearConstraint1<Cfg, Key> Base;
typedef boost::shared_ptr<BoundingConstraint1<Cfg, Key> > shared_ptr;
double threshold_; double threshold_;
bool isGreaterThan_; /// flag for greater/less than bool isGreaterThan_; /// flag for greater/less than
@ -61,10 +62,13 @@ namespace gtsam {
* Binary scalar inequality constraint, with a similar value() function * Binary scalar inequality constraint, with a similar value() function
* to implement for specific systems * to implement for specific systems
*/ */
template<class Cfg, class Key1, class X1, class Key2, class X2> template<class Cfg, class Key1, class Key2>
struct BoundingConstraint2: public NonlinearConstraint2<Cfg, Key1, X1, Key2, X2> { struct BoundingConstraint2: public NonlinearConstraint2<Cfg, Key1, Key2> {
typedef NonlinearConstraint2<Cfg, Key1, X1, Key2, X2> Base; typedef typename Key1::Value_t X1;
typedef boost::shared_ptr<BoundingConstraint2<Cfg, Key1, X1, Key2, X2> > shared_ptr; typedef typename Key2::Value_t X2;
typedef NonlinearConstraint2<Cfg, Key1, Key2> Base;
typedef boost::shared_ptr<BoundingConstraint2<Cfg, Key1, Key2> > shared_ptr;
double threshold_; double threshold_;
bool isGreaterThan_; /// flag for greater/less than bool isGreaterThan_; /// flag for greater/less than

13
slam/PriorFactor.h
View File

@ -20,12 +20,15 @@ namespace gtsam {
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work * a simple type like int will not work
*/ */
template<class Config, class Key, class T> template<class Config, class Key>
class PriorFactor: public NonlinearFactor1<Config, Key, T> { class PriorFactor: public NonlinearFactor1<Config, Key> {
public:
typedef typename Key::Value_t T;
private: private:
typedef NonlinearFactor1<Config, Key, T> Base; typedef NonlinearFactor1<Config, Key> Base;
T prior_; /** The measurement */ T prior_; /** The measurement */
@ -50,8 +53,8 @@ namespace gtsam {
/** equals */ /** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const { bool equals(const NonlinearFactor<Config>& expected, double tol) const {
const PriorFactor<Config, Key, T> *e = dynamic_cast<const PriorFactor< const PriorFactor<Config, Key> *e = dynamic_cast<const PriorFactor<
Config, Key, T>*> (&expected); Config, Key>*> (&expected);
return e != NULL && Base::equals(expected) && this->prior_.equals( return e != NULL && Base::equals(expected) && this->prior_.equals(
e->prior_, tol); e->prior_, tol);
} }

5
slam/RangeFactor.h
View File

@ -15,13 +15,12 @@ namespace gtsam {
* Binary factor for a range measurement * Binary factor for a range measurement
*/ */
template<class Config, class PoseKey, class PointKey> template<class Config, class PoseKey, class PointKey>
class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey, class RangeFactor: public NonlinearFactor2<Config, PoseKey, PointKey> {
Point2> {
private: private:
double z_; /** measurement */ double z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base; typedef NonlinearFactor2<Config, PoseKey, PointKey> Base;
public: public:

13
slam/Simulated3D.h
View File

@ -21,8 +21,8 @@ namespace simulated3D {
typedef VectorConfig VectorConfig; typedef VectorConfig VectorConfig;
typedef gtsam::Symbol PoseKey; typedef gtsam::TypedSymbol<Vector, 'x'> PoseKey;
typedef gtsam::Symbol PointKey; typedef gtsam::TypedSymbol<Vector, 'l'> PointKey;
/** /**
* Prior on a single pose * Prior on a single pose
@ -44,14 +44,13 @@ namespace simulated3D {
Matrix Dmea1(const Vector& x, const Vector& l); Matrix Dmea1(const Vector& x, const Vector& l);
Matrix Dmea2(const Vector& x, const Vector& l); Matrix Dmea2(const Vector& x, const Vector& l);
struct Point2Prior3D: public NonlinearFactor1<VectorConfig, PoseKey, struct Point2Prior3D: public NonlinearFactor1<VectorConfig, PoseKey> {
Vector> {
Vector z_; Vector z_;
Point2Prior3D(const Vector& z, Point2Prior3D(const Vector& z,
const SharedGaussian& model, const PoseKey& j) : const SharedGaussian& model, const PoseKey& j) :
NonlinearFactor1<VectorConfig, PoseKey, Vector> (model, j), z_(z) { NonlinearFactor1<VectorConfig, PoseKey> (model, j), z_(z) {
} }
Vector evaluateError(const Vector& x, boost::optional<Matrix&> H = Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
@ -62,13 +61,13 @@ namespace simulated3D {
}; };
struct Simulated3DMeasurement: public NonlinearFactor2<VectorConfig, struct Simulated3DMeasurement: public NonlinearFactor2<VectorConfig,
PoseKey, Vector, PointKey, Vector> { PoseKey, PointKey> {
Vector z_; Vector z_;
Simulated3DMeasurement(const Vector& z, Simulated3DMeasurement(const Vector& z,
const SharedGaussian& model, PoseKey& j1, PointKey j2) : const SharedGaussian& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, Vector> ( NonlinearFactor2<VectorConfig, PoseKey, PointKey> (
model, j1, j2), z_(z) { model, j1, j2), z_(z) {
} }

11
slam/TransformConstraint.h
View File

@ -25,12 +25,15 @@ namespace gtsam {
* This base class should be specialized to implement the cost function for * This base class should be specialized to implement the cost function for
* specific classes of landmarks * specific classes of landmarks
*/ */
template<class Config, class PKey, class Point, class TKey, class Transform> template<class Config, class PKey, class TKey>
class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> { class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, TKey, PKey> {
public: public:
typedef NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> Base; typedef typename PKey::Value_t Point;
typedef TransformConstraint<Config, PKey, Point, TKey, Transform> This; typedef typename TKey::Value_t Transform;
typedef NonlinearEqualityConstraint3<Config, PKey, TKey, PKey> Base;
typedef TransformConstraint<Config, PKey, TKey> This;
/** /**
* General constructor with all of the keys to variables in the map * General constructor with all of the keys to variables in the map

2
slam/planarSLAM.cpp
View File

@ -13,7 +13,7 @@
namespace gtsam { namespace gtsam {
using namespace planarSLAM; using namespace planarSLAM;
INSTANTIATE_LIE_CONFIG(PointKey, Point2) INSTANTIATE_LIE_CONFIG(PointKey)
INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)

10
slam/planarSLAM.h
View File

@ -23,14 +23,14 @@ namespace gtsam {
// Keys and Config // Keys and Config
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig; typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config; typedef TupleConfig2<PoseConfig, PointConfig> Config;
// Factors // Factors
typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint; typedef NonlinearEquality<Config, PoseKey> Constraint;
typedef PriorFactor<Config, PoseKey, Pose2> Prior; typedef PriorFactor<Config, PoseKey> Prior;
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry; typedef BetweenFactor<Config, PoseKey> Odometry;
typedef BearingFactor<Config, PoseKey, PointKey> Bearing; typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
typedef RangeFactor<Config, PoseKey, PointKey> Range; typedef RangeFactor<Config, PoseKey, PointKey> Range;
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange; typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;

2
slam/pose2SLAM.cpp
View File

@ -13,7 +13,7 @@
namespace gtsam { namespace gtsam {
using namespace pose2SLAM; using namespace pose2SLAM;
INSTANTIATE_LIE_CONFIG(Key, Pose2) INSTANTIATE_LIE_CONFIG(Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)

10
slam/pose2SLAM.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
// Keys and Config // Keys and Config
typedef TypedSymbol<Pose2, 'x'> Key; typedef TypedSymbol<Pose2, 'x'> Key;
typedef LieConfig<Key, Pose2> Config; typedef LieConfig<Key> Config;
/** /**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) * 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); Config circle(size_t n, double R);
// Factors // Factors
typedef PriorFactor<Config, Key, Pose2> Prior; typedef PriorFactor<Config, Key> Prior;
typedef BetweenFactor<Config, Key, Pose2> Constraint; typedef BetweenFactor<Config, Key> Constraint;
typedef NonlinearEquality<Config, Key, Pose2> HardConstraint; typedef NonlinearEquality<Config, Key> HardConstraint;
// Graph // Graph
struct Graph: public NonlinearFactorGraph<Config> { struct Graph: public NonlinearFactorGraph<Config> {
typedef BetweenFactor<Config, Key, Pose2> Constraint; typedef BetweenFactor<Config, Key> Constraint;
typedef Pose2 Pose; typedef Pose2 Pose;
void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model); 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); void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model);

2
slam/pose3SLAM.cpp
View File

@ -13,7 +13,7 @@
namespace gtsam { namespace gtsam {
using namespace pose3SLAM; using namespace pose3SLAM;
INSTANTIATE_LIE_CONFIG(Key, Pose3) INSTANTIATE_LIE_CONFIG(Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)

8
slam/pose3SLAM.h
View File

@ -22,7 +22,7 @@ namespace gtsam {
// Keys and Config // Keys and Config
typedef TypedSymbol<Pose3, 'x'> Key; typedef TypedSymbol<Pose3, 'x'> Key;
typedef LieConfig<Key, Pose3> Config; typedef LieConfig<Key> Config;
/** /**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) * 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); Config circle(size_t n, double R);
// Factors // Factors
typedef PriorFactor<Config, Key, Pose3> Prior; typedef PriorFactor<Config, Key> Prior;
typedef BetweenFactor<Config, Key, Pose3> Constraint; typedef BetweenFactor<Config, Key> Constraint;
typedef NonlinearEquality<Config, Key, Pose3> HardConstraint; typedef NonlinearEquality<Config, Key> HardConstraint;
// Graph // Graph
struct Graph: public NonlinearFactorGraph<Config> { struct Graph: public NonlinearFactorGraph<Config> {

4
slam/simulated2D.cpp
View File

@ -11,8 +11,8 @@
namespace gtsam { namespace gtsam {
using namespace simulated2D; using namespace simulated2D;
// INSTANTIATE_LIE_CONFIG(PointKey, Point2) // INSTANTIATE_LIE_CONFIG(PointKey)
INSTANTIATE_LIE_CONFIG(PoseKey, Point2) INSTANTIATE_LIE_CONFIG(PoseKey)
INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) // INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) // INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)

18
slam/simulated2D.h
View File

@ -21,8 +21,8 @@ namespace gtsam {
// Simulated2D robots have no orientation, just a position // Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey; typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey, Point2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig; typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config; typedef TupleConfig2<PoseConfig, PointConfig> Config;
/** /**
@ -55,13 +55,13 @@ namespace gtsam {
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class Cfg = Config, class Key = PoseKey> template<class Cfg = Config, class Key = PoseKey>
struct GenericPrior: public NonlinearFactor1<Cfg, Key, Point2> { struct GenericPrior: public NonlinearFactor1<Cfg, Key> {
typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr; typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr;
Point2 z_; Point2 z_;
GenericPrior(const Point2& z, const SharedGaussian& model, const Key& key) : GenericPrior(const Point2& z, const SharedGaussian& model, const Key& key) :
NonlinearFactor1<Cfg, Key, Point2> (model, key), z_(z) { NonlinearFactor1<Cfg, Key> (model, key), z_(z) {
} }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H = Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
@ -75,14 +75,13 @@ namespace gtsam {
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class Cfg = Config, class Key = PoseKey> template<class Cfg = Config, class Key = PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Point2, Key, struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> {
Point2> {
typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr;
Point2 z_; Point2 z_;
GenericOdometry(const Point2& z, const SharedGaussian& model, GenericOdometry(const Point2& z, const SharedGaussian& model,
const Key& i1, const Key& i2) : const Key& i1, const Key& i2) :
NonlinearFactor2<Cfg, Key, Point2, Key, Point2> (model, i1, i2), z_(z) { NonlinearFactor2<Cfg, Key, Key> (model, i1, i2), z_(z) {
} }
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
@ -96,15 +95,14 @@ namespace gtsam {
* Binary factor simulating "measurement" between two Vectors * Binary factor simulating "measurement" between two Vectors
*/ */
template<class Cfg = Config, class XKey = PoseKey, class LKey = PointKey> template<class Cfg = Config, class XKey = PoseKey, class LKey = PointKey>
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey, class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, LKey> {
Point2> {
public: public:
typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr;
Point2 z_; Point2 z_;
GenericMeasurement(const Point2& z, const SharedGaussian& model, GenericMeasurement(const Point2& z, const SharedGaussian& model,
const XKey& i, const LKey& j) : const XKey& i, const LKey& j) :
NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> (model, i, j), z_(z) { NonlinearFactor2<Cfg, XKey, LKey> (model, i, j), z_(z) {
} }
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<

22
slam/simulated2DConstraints.h
View File

@ -24,13 +24,13 @@ namespace gtsam {
namespace equality_constraints { namespace equality_constraints {
/** Typedefs for regular use */ /** Typedefs for regular use */
typedef NonlinearEquality1<Config, PoseKey, Point2> UnaryEqualityConstraint; typedef NonlinearEquality1<Config, PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<Config, PointKey, Point2> UnaryEqualityPointConstraint; typedef NonlinearEquality1<Config, PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Config, PoseKey, Point2> OdoEqualityConstraint; typedef BetweenConstraint<Config, PoseKey> OdoEqualityConstraint;
/** Equality between variables */ /** Equality between variables */
typedef NonlinearEquality2<Config, PoseKey, Point2> PoseEqualityConstraint; typedef NonlinearEquality2<Config, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<Config, PointKey, Point2> PointEqualityConstraint; typedef NonlinearEquality2<Config, PointKey> PointEqualityConstraint;
} // \namespace equality_constraints } // \namespace equality_constraints
@ -41,8 +41,8 @@ namespace gtsam {
* Demo implementation: should be made more general using BoundingConstraint * Demo implementation: should be made more general using BoundingConstraint
*/ */
template<class Cfg, class Key, unsigned int Idx> template<class Cfg, class Key, unsigned int Idx>
struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key, Point2> { struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key> {
typedef BoundingConstraint1<Cfg, Key, Point2> Base; typedef BoundingConstraint1<Cfg, Key> Base;
typedef boost::shared_ptr<ScalarCoordConstraint1<Cfg, Key, Idx> > shared_ptr; typedef boost::shared_ptr<ScalarCoordConstraint1<Cfg, Key, Idx> > shared_ptr;
ScalarCoordConstraint1(const Key& key, double c, ScalarCoordConstraint1(const Key& key, double c,
@ -75,8 +75,8 @@ namespace gtsam {
* to be less than or equal to a bound * to be less than or equal to a bound
*/ */
template<class Cfg, class Key> template<class Cfg, class Key>
struct MaxDistanceConstraint : public BoundingConstraint2<Cfg, Key, Point2, Key, Point2> { struct MaxDistanceConstraint : public BoundingConstraint2<Cfg, Key, Key> {
typedef BoundingConstraint2<Cfg, Key, Point2, Key, Point2> Base; typedef BoundingConstraint2<Cfg, Key, Key> Base;
MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0) MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, false, mu) {} : Base(key1, key2, range_bound, false, mu) {}
@ -98,8 +98,8 @@ namespace gtsam {
* NOTE: this is not a convex function! Be careful with initialization. * NOTE: this is not a convex function! Be careful with initialization.
*/ */
template<class Cfg, class XKey, class PKey> template<class Cfg, class XKey, class PKey>
struct MinDistanceConstraint : public BoundingConstraint2<Cfg, XKey, Point2, PKey, Point2> { struct MinDistanceConstraint : public BoundingConstraint2<Cfg, XKey, PKey> {
typedef BoundingConstraint2<Cfg, XKey, Point2, PKey, Point2> Base; typedef BoundingConstraint2<Cfg, XKey, PKey> Base;
MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0) MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {} : Base(key1, key2, range_bound, true, mu) {}

2
slam/simulated2DOriented.cpp
View File

@ -10,7 +10,7 @@
namespace gtsam { namespace gtsam {
using namespace simulated2DOriented; using namespace simulated2DOriented;
// INSTANTIATE_LIE_CONFIG(PointKey, Point2) // INSTANTIATE_LIE_CONFIG(PointKey)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) // INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) // INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)

13
slam/simulated2DOriented.h
View File

@ -21,8 +21,8 @@ namespace gtsam {
// The types that take an oriented pose2 rather than point2 // The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig; typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config; typedef TupleConfig2<PoseConfig, PointConfig> Config;
//TODO:: point prior is not implemented right now //TODO:: point prior is not implemented right now
@ -48,12 +48,12 @@ namespace gtsam {
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class Cfg = Config, class Key = PoseKey> template<class Cfg = Config, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<Cfg, Key, Pose2> { struct GenericPosePrior: public NonlinearFactor1<Cfg, Key> {
Pose2 z_; Pose2 z_;
GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) : GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) :
NonlinearFactor1<Cfg, Key, Pose2> (model, key), z_(z) { NonlinearFactor1<Cfg, Key> (model, key), z_(z) {
} }
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
@ -67,13 +67,12 @@ namespace gtsam {
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class Cfg = Config, class Key = PoseKey> template<class Cfg = Config, class Key = PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Pose2, Key, struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> {
Pose2> {
Pose2 z_; Pose2 z_;
GenericOdometry(const Pose2& z, const SharedGaussian& model, GenericOdometry(const Pose2& z, const SharedGaussian& model,
const Key& i1, const Key& i2) : const Key& i1, const Key& i2) :
NonlinearFactor2<Cfg, Key, Pose2, Key, Pose2> (model, i1, i2), z_(z) { NonlinearFactor2<Cfg, Key, Key> (model, i1, i2), z_(z) {
} }
Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional< Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional<

5
slam/smallExample.cpp
View File

@ -183,14 +183,13 @@ namespace example {
} }
struct UnaryFactor: public gtsam::NonlinearFactor1<Config, struct UnaryFactor: public gtsam::NonlinearFactor1<Config,
simulated2D::PoseKey, Point2> { simulated2D::PoseKey> {
Point2 z_; Point2 z_;
UnaryFactor(const Point2& z, const SharedGaussian& model, UnaryFactor(const Point2& z, const SharedGaussian& model,
const simulated2D::PoseKey& key) : const simulated2D::PoseKey& key) :
gtsam::NonlinearFactor1<Config, simulated2D::PoseKey, gtsam::NonlinearFactor1<Config, simulated2D::PoseKey>(model, key), z_(z) {
Point2>(model, key), z_(z) {
} }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =

12
slam/visualSLAM.h
View File

@ -24,13 +24,13 @@ namespace gtsam { namespace visualSLAM {
*/ */
typedef TypedSymbol<Pose3,'x'> PoseKey; typedef TypedSymbol<Pose3,'x'> PoseKey;
typedef TypedSymbol<Point3,'l'> PointKey; typedef TypedSymbol<Point3,'l'> PointKey;
typedef LieConfig<PoseKey, Pose3> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point3> PointConfig; typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config; typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef boost::shared_ptr<Config> shared_config; typedef boost::shared_ptr<Config> shared_config;
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint; typedef NonlinearEquality<Config, PoseKey> PoseConstraint;
typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint; typedef NonlinearEquality<Config, PointKey> PointConstraint;
/** /**
@ -38,7 +38,7 @@ namespace gtsam { namespace visualSLAM {
* i.e. the main building block for visual SLAM. * i.e. the main building block for visual SLAM.
*/ */
template <class Cfg=Config, class LmK=PointKey, class PosK=PoseKey> template <class Cfg=Config, class LmK=PointKey, class PosK=PoseKey>
class GenericProjectionFactor : public NonlinearFactor2<Cfg, PosK, Pose3, LmK, Point3>, Testable<GenericProjectionFactor<Cfg, LmK, PosK> > { class GenericProjectionFactor : public NonlinearFactor2<Cfg, PosK, LmK>, Testable<GenericProjectionFactor<Cfg, LmK, PosK> > {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -48,7 +48,7 @@ namespace gtsam { namespace visualSLAM {
public: public:
// shorthand for base class type // shorthand for base class type
typedef NonlinearFactor2<Cfg, PosK, Pose3, LmK, Point3> Base; typedef NonlinearFactor2<Cfg, PosK, LmK> Base;
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<Cfg, LmK, PosK> > shared_ptr; typedef boost::shared_ptr<GenericProjectionFactor<Cfg, LmK, PosK> > shared_ptr;

8
tests/testFusionTupleConfig.cpp
View File

@ -31,8 +31,8 @@ static const double tol = 1e-5;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig; typedef LieConfig<PointKey> PointConfig;
// some generic poses, points and keys // some generic poses, points and keys
PoseKey x1(1), x2(2); PoseKey x1(1), x2(2);
@ -484,8 +484,8 @@ TEST( testFusionTupleConfig, basic_factor)
// Factors // Factors
// typedef PriorFactor<TestPoseConfig, PoseKey, Pose2> Prior; // fails to add to graph // typedef PriorFactor<TestPoseConfig, PoseKey, Pose2> Prior; // fails to add to graph
typedef PriorFactor<Config, PoseKey, Pose2> Prior; typedef PriorFactor<Config, PoseKey> Prior;
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry; typedef BetweenFactor<Config, PoseKey> Odometry;
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange; typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;
PoseKey pose1k(1), pose2k(2), pose3k(3); PoseKey pose1k(1), pose2k(2), pose3k(3);

108
tests/testNonlinearEquality.cpp
View File

@ -5,14 +5,11 @@
#include <gtsam/CppUnitLite/TestHarness.h> #include <gtsam/CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
@ -21,37 +18,31 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_nle;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef PriorFactor<PoseConfig, PoseKey, Pose2> PosePrior; typedef PriorFactor<PoseConfig, PoseKey> PosePrior;
typedef NonlinearEquality<PoseConfig, PoseKey, Pose2> PoseNLE; typedef NonlinearEquality<PoseConfig, PoseKey> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE; typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
typedef NonlinearFactorGraph<PoseConfig> PoseGraph; typedef NonlinearFactorGraph<PoseConfig> PoseGraph;
typedef NonlinearOptimizer<PoseGraph,PoseConfig> PoseOptimizer; typedef NonlinearOptimizer<PoseGraph,PoseConfig> PoseOptimizer;
bool vector_compare(const Vector& a, const Vector& b) { PoseKey key(1);
return equal_with_abs_tol(a, b, 1e-5);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( NonlinearEquality, linearization ) { TEST ( NonlinearEquality, linearization ) {
Symbol key = "x"; Pose2 value = Pose2(2.1, 1.0, 2.0);
Vector value = Vector_(2, 1.0, 2.0); PoseConfig linearize;
VectorConfig linearize;
linearize.insert(key, value); linearize.insert(key, value);
// create a nonlinear equality constraint // create a nonlinear equality constraint
shared_nle nle(new NLE(key, value,vector_compare)); shared_poseNLE nle(new PoseNLE(key, value));
// check linearize // check linearize
SharedDiagonal constraintModel = noiseModel::Constrained::All(2); SharedDiagonal constraintModel = noiseModel::Constrained::All(3);
GaussianFactor expLF(key, eye(2), zero(2), constraintModel); GaussianFactor expLF(key, eye(3), zero(3), constraintModel);
GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); 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)); shared_poseNLE nle(new PoseNLE(key, value));
GaussianFactor::shared_ptr actualLF = nle->linearize(config); GaussianFactor::shared_ptr actualLF = nle->linearize(config);
CHECK(true); EXPECT(true);
} }
/* ********************************************************************** */ /* ********************************************************************** */
TEST ( NonlinearEquality, linearization_fail ) { TEST ( NonlinearEquality, linearization_fail ) {
Symbol key = "x"; Pose2 value = Pose2(2.1, 1.0, 2.0);
Vector value = Vector_(2, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0);
Vector wrong = Vector_(2, 3.0, 4.0); PoseConfig bad_linearize;
VectorConfig bad_linearize;
bad_linearize.insert(key, wrong); bad_linearize.insert(key, wrong);
// create a nonlinear equality constraint // 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 linearize to ensure that it fails for bad linearization points
CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
@ -118,67 +108,37 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( NonlinearEquality, error ) { TEST ( NonlinearEquality, error ) {
Symbol key = "x"; Pose2 value = Pose2(2.1, 1.0, 2.0);
Vector value = Vector_(2, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0);
Vector wrong = Vector_(2, 3.0, 4.0); PoseConfig feasible, bad_linearize;
VectorConfig feasible, bad_linearize;
feasible.insert(key, value); feasible.insert(key, value);
bad_linearize.insert(key, wrong); bad_linearize.insert(key, wrong);
// create a nonlinear equality constraint // 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 // check error function outputs
Vector actual = nle->unwhitenedError(feasible); Vector actual = nle->unwhitenedError(feasible);
CHECK(assert_equal(actual, zero(2))); EXPECT(assert_equal(actual, zero(3)));
actual = nle->unwhitenedError(bad_linearize); actual = nle->unwhitenedError(bad_linearize);
CHECK(assert_equal(actual, repeat(2, std::numeric_limits<double>::infinity()))); EXPECT(assert_equal(actual, repeat(3, std::numeric_limits<double>::infinity())));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( NonlinearEquality, equals ) { TEST ( NonlinearEquality, equals ) {
string key1 = "x"; Pose2 value1 = Pose2(2.1, 1.0, 2.0);
Vector value1 = Vector_(2, 1.0, 2.0); Pose2 value2 = Pose2(2.1, 3.0, 4.0);
Vector value2 = Vector_(2, 3.0, 4.0);
// create some constraints to compare // create some constraints to compare
shared_nle nle1(new NLE(key1, value1,vector_compare)); shared_poseNLE nle1(new PoseNLE(key, value1));
shared_nle nle2(new NLE(key1, value1,vector_compare)); shared_poseNLE nle2(new PoseNLE(key, value1));
shared_nle nle3(new NLE(key1, value2,vector_compare)); shared_poseNLE nle3(new PoseNLE(key, value2));
// verify // verify
CHECK(nle1->equals(*nle2)); // basic equality = true EXPECT(nle1->equals(*nle2)); // basic equality = true
CHECK(nle2->equals(*nle1)); // test symmetry of equals() EXPECT(nle2->equals(*nle1)); // test symmetry of equals()
CHECK(!nle1->equals(*nle3)); // test config EXPECT(!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));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -192,7 +152,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
Pose2 badPoint1(0.0, 2.0, 3.0); Pose2 badPoint1(0.0, 2.0, 3.0);
Vector actVec = nle.evaluateError(badPoint1); Vector actVec = nle.evaluateError(badPoint1);
Vector expVec = Vector_(3, -0.989992, -0.14112, 0.0); 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 // the actual error should have a gain on it
PoseConfig config; PoseConfig config;
@ -206,7 +166,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
Vector b = expVec; Vector b = expVec;
SharedDiagonal model = noiseModel::Constrained::All(3); SharedDiagonal model = noiseModel::Constrained::All(3);
GaussianFactor::shared_ptr expLinFactor(new GaussianFactor(key1, A1, b, model)); 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 // verify
PoseConfig expected; PoseConfig expected;
expected.insert(key1, feasible1); 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 // verify
PoseConfig expected; PoseConfig expected;
expected.insert(key1, feasible1); expected.insert(key1, feasible1);
CHECK(assert_equal(expected, *result.config())); EXPECT(assert_equal(expected, *result.config()));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
tests/testNonlinearEqualityConstraint.cpp
View File

@ -285,7 +285,7 @@ typedef visualSLAM::Graph VGraph;
typedef NonlinearOptimizer<VGraph,VConfig> VOptimizer; typedef NonlinearOptimizer<VGraph,VConfig> VOptimizer;
// factors for visual slam // factors for visual slam
typedef NonlinearEquality2<VConfig, visualSLAM::PointKey, Point3> Point3Equality; typedef NonlinearEquality2<VConfig, visualSLAM::PointKey> Point3Equality;
/* ********************************************************************* */ /* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, stereo_constrained ) { TEST (testNonlinearEqualityConstraint, stereo_constrained ) {

14
tests/testTransformConstraint.cpp
View File

@ -33,19 +33,19 @@ typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'T'> TransformKey; typedef TypedSymbol<Pose2, 'T'> TransformKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig; typedef LieConfig<PointKey> PointConfig;
typedef LieConfig<TransformKey, Pose2> TransformConfig; typedef LieConfig<TransformKey> TransformConfig;
typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig; typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig;
typedef NonlinearFactorGraph<DDFConfig> DDFGraph; typedef NonlinearFactorGraph<DDFConfig> DDFGraph;
typedef NonlinearOptimizer<DDFGraph, DDFConfig> Optimizer; typedef NonlinearOptimizer<DDFGraph, DDFConfig> Optimizer;
typedef NonlinearEquality<DDFConfig, PoseKey, Pose2> PoseConstraint; typedef NonlinearEquality<DDFConfig, PoseKey> PoseConstraint;
typedef NonlinearEquality<DDFConfig, PointKey, Point2> PointConstraint; typedef NonlinearEquality<DDFConfig, PointKey> PointConstraint;
typedef NonlinearEquality<DDFConfig, TransformKey, Pose2> TransformPriorConstraint; typedef NonlinearEquality<DDFConfig, TransformKey> TransformPriorConstraint;
typedef TransformConstraint<DDFConfig, PointKey, Point2, TransformKey, Pose2> PointTransformConstraint; typedef TransformConstraint<DDFConfig, PointKey, TransformKey> PointTransformConstraint;
PointKey lA1(1), lA2(2), lB1(3); PointKey lA1(1), lA2(2), lB1(3);
TransformKey t1(1); TransformKey t1(1);

16
tests/testTupleConfig.cpp
View File

@ -26,8 +26,8 @@ static const double tol = 1e-5;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig; typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config; typedef TupleConfig2<PoseConfig, PointConfig> Config;
/* ************************************************************************* */ /* ************************************************************************* */
@ -202,12 +202,12 @@ typedef TypedSymbol<Point3, 'b'> Point3Key;
typedef TypedSymbol<Point3, 'c'> Point3Key2; typedef TypedSymbol<Point3, 'c'> Point3Key2;
// some config types // some config types
typedef LieConfig<PoseKey, Pose2> PoseConfig; typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig; typedef LieConfig<PointKey> PointConfig;
typedef LieConfig<LamKey, Vector> LamConfig; typedef LieConfig<LamKey> LamConfig;
typedef LieConfig<Pose3Key, Pose3> Pose3Config; typedef LieConfig<Pose3Key> Pose3Config;
typedef LieConfig<Point3Key, Point3> Point3Config; typedef LieConfig<Point3Key> Point3Config;
typedef LieConfig<Point3Key2, Point3> Point3Config2; typedef LieConfig<Point3Key2> Point3Config2;
// some TupleConfig types // some TupleConfig types
typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA; typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA;