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
Alex Cunningham
parent
86f812ebb5
commit
77eda5ab8c
|
|
@ -18,11 +18,6 @@
|
|||
template class Lie<T>;
|
||||
|
||||
namespace gtsam {
|
||||
// template<class T>
|
||||
// size_t Lie<T>::dim() const {
|
||||
// return gtsam::dim(*((T*)this));
|
||||
// }
|
||||
|
||||
/**
|
||||
* Returns Exponential mapy
|
||||
* This is for matlab wrapper
|
||||
|
|
|
|||
|
|
@ -169,7 +169,7 @@ public:
|
|||
/** insertion */
|
||||
template <class Key, class Value>
|
||||
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 */
|
||||
|
|
@ -204,25 +204,25 @@ public:
|
|||
*/
|
||||
template<class Key, class 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 */
|
||||
template<class Key>
|
||||
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 */
|
||||
template<class Key>
|
||||
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 */
|
||||
template<class Key>
|
||||
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 */
|
||||
|
|
@ -255,7 +255,7 @@ public:
|
|||
/** erases a specific key */
|
||||
template<class Key>
|
||||
void erase(const Key& j) {
|
||||
config_<LieConfig<Key,typename Key::Value_t> >().erase(j);
|
||||
config_<LieConfig<Key> >().erase(j);
|
||||
}
|
||||
|
||||
/** clears the config */
|
||||
|
|
|
|||
|
|
@ -19,27 +19,27 @@
|
|||
|
||||
#include <gtsam/nonlinear/LieConfig.h>
|
||||
|
||||
#define INSTANTIATE_LIE_CONFIG(J,T) \
|
||||
#define INSTANTIATE_LIE_CONFIG(J) \
|
||||
/*INSTANTIATE_LIE(T);*/ \
|
||||
template LieConfig<J,T> expmap(const LieConfig<J,T>&, const VectorConfig&); \
|
||||
template LieConfig<J,T> expmap(const LieConfig<J,T>&, const Vector&); \
|
||||
template VectorConfig logmap(const LieConfig<J,T>&, const LieConfig<J,T>&); \
|
||||
template class LieConfig<J,T>;
|
||||
template LieConfig<J> expmap(const LieConfig<J>&, const VectorConfig&); \
|
||||
template LieConfig<J> expmap(const LieConfig<J>&, const Vector&); \
|
||||
template VectorConfig logmap(const LieConfig<J>&, const LieConfig<J>&); \
|
||||
template class LieConfig<J>;
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<class J, class T>
|
||||
void LieConfig<J,T>::print(const string &s) const {
|
||||
template<class J>
|
||||
void LieConfig<J>::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<class J, class T>
|
||||
bool LieConfig<J,T>::equals(const LieConfig<J,T>& expected, double tol) const {
|
||||
template<class J>
|
||||
bool LieConfig<J>::equals(const LieConfig<J>& 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<class J, class T>
|
||||
const T& LieConfig<J,T>::at(const J& j) const {
|
||||
template<class J>
|
||||
const typename J::Value_t& LieConfig<J>::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<class J, class T>
|
||||
size_t LieConfig<J,T>::dim() const {
|
||||
template<class J>
|
||||
size_t LieConfig<J>::dim() const {
|
||||
size_t n = 0;
|
||||
BOOST_FOREACH(const typename Values::value_type& value, values_)
|
||||
n += gtsam::dim(value.second);
|
||||
return n;
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
VectorConfig LieConfig<J,T>::zero() const {
|
||||
template<class J>
|
||||
VectorConfig LieConfig<J>::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<class J, class T>
|
||||
void LieConfig<J,T>::insert(const J& name, const T& val) {
|
||||
template<class J>
|
||||
void LieConfig<J>::insert(const J& name, const typename J::Value_t& val) {
|
||||
values_.insert(make_pair(name, val));
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
void LieConfig<J,T>::insert(const LieConfig<J,T>& cfg) {
|
||||
template<class J>
|
||||
void LieConfig<J>::insert(const LieConfig<J>& cfg) {
|
||||
BOOST_FOREACH(const typename Values::value_type& v, cfg.values_)
|
||||
insert(v.first, v.second);
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
void LieConfig<J,T>::update(const LieConfig<J,T>& cfg) {
|
||||
template<class J>
|
||||
void LieConfig<J>::update(const LieConfig<J>& cfg) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
void LieConfig<J,T>::update(const J& j, const T& val) {
|
||||
template<class J>
|
||||
void LieConfig<J>::update(const J& j, const typename J::Value_t& val) {
|
||||
values_[j] = val;
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
std::list<J> LieConfig<J,T>::keys() const {
|
||||
template<class J>
|
||||
std::list<J> LieConfig<J>::keys() const {
|
||||
std::list<J> ret;
|
||||
BOOST_FOREACH(const typename Values::value_type& v, values_)
|
||||
ret.push_back(v.first);
|
||||
return ret;
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
void LieConfig<J,T>::erase(const J& j) {
|
||||
template<class J>
|
||||
void LieConfig<J>::erase(const J& j) {
|
||||
size_t dim; // unused
|
||||
erase(j, dim);
|
||||
}
|
||||
|
||||
template<class J, class T>
|
||||
void LieConfig<J,T>::erase(const J& j, size_t& dim) {
|
||||
template<class J>
|
||||
void LieConfig<J>::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<class J, class T>
|
||||
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) {
|
||||
LieConfig<J,T> newConfig;
|
||||
typedef pair<J,T> KeyValue;
|
||||
template<class J>
|
||||
LieConfig<J> expmap(const LieConfig<J>& c, const VectorConfig& delta) {
|
||||
LieConfig<J> newConfig;
|
||||
typedef pair<J,typename J::Value_t> 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<class J, class T>
|
||||
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta) {
|
||||
template<class J>
|
||||
LieConfig<J> expmap(const LieConfig<J>& 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<J,T> newConfig;
|
||||
LieConfig<J> newConfig;
|
||||
int delta_offset = 0;
|
||||
typedef pair<J,T> KeyValue;
|
||||
typedef pair<J,typename J::Value_t> 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<class J, class T>
|
||||
VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp) {
|
||||
template<class J>
|
||||
VectorConfig logmap(const LieConfig<J>& c0, const LieConfig<J>& cp) {
|
||||
VectorConfig delta;
|
||||
typedef pair<J,T> KeyValue;
|
||||
typedef pair<J,typename J::Value_t> KeyValue;
|
||||
BOOST_FOREACH(const KeyValue& value, cp) {
|
||||
if(c0.exists(value.first))
|
||||
delta.insert(value.first,
|
||||
|
|
|
|||
|
|
@ -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 J, class T>
|
||||
class LieConfig : public Testable<LieConfig<J, T> > {
|
||||
template<class J>
|
||||
class LieConfig : public Testable<LieConfig<J> > {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -44,8 +48,8 @@ namespace gtsam {
|
|||
* Typedefs
|
||||
*/
|
||||
typedef J Key;
|
||||
typedef T Value;
|
||||
typedef std::map<J, T> Values;
|
||||
typedef typename J::Value_t Value;
|
||||
typedef std::map<J,Value> 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<class J_alt, class T_alt>
|
||||
LieConfig(const LieConfig<J_alt,T_alt>& other) {} // do nothing when initializing with wrong type
|
||||
template<class J_alt>
|
||||
LieConfig(const LieConfig<J_alt>& 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<T> exists_(const J& i) const {
|
||||
boost::optional<Value> 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<class J, class T>
|
||||
inline size_t dim(const LieConfig<J,T>& c) { return c.dim(); }
|
||||
template<class J>
|
||||
inline size_t dim(const LieConfig<J>& c) { return c.dim(); }
|
||||
|
||||
/** Add a delta config */
|
||||
template<class J, class T>
|
||||
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta);
|
||||
template<class J>
|
||||
LieConfig<J> expmap(const LieConfig<J>& c, const VectorConfig& delta);
|
||||
|
||||
/** Add a delta vector, uses iterator order */
|
||||
template<class J, class T>
|
||||
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta);
|
||||
template<class J>
|
||||
LieConfig<J> expmap(const LieConfig<J>& c, const Vector& delta);
|
||||
|
||||
/** Get a delta config about a linearization point c0 */
|
||||
template<class J, class T>
|
||||
VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp);
|
||||
template<class J>
|
||||
VectorConfig logmap(const LieConfig<J>& c0, const LieConfig<J>& cp);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -95,11 +95,14 @@ public:
|
|||
/**
|
||||
* 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> {
|
||||
|
||||
public:
|
||||
typedef typename Key::Value_t X;
|
||||
|
||||
protected:
|
||||
typedef NonlinearConstraint1<Config,Key,X> This;
|
||||
typedef NonlinearConstraint1<Config,Key> This;
|
||||
typedef NonlinearConstraint<Config> Base;
|
||||
|
||||
/** key for the constrained variable */
|
||||
|
|
@ -165,12 +168,15 @@ public:
|
|||
/**
|
||||
* Unary Equality constraint - simply forces the value of active() to true
|
||||
*/
|
||||
template <class Config, class Key, class X>
|
||||
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key, X> {
|
||||
template <class Config, class Key>
|
||||
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key> {
|
||||
|
||||
public:
|
||||
typedef typename Key::Value_t X;
|
||||
|
||||
protected:
|
||||
typedef NonlinearEqualityConstraint1<Config,Key,X> This;
|
||||
typedef NonlinearConstraint1<Config,Key,X> Base;
|
||||
typedef NonlinearEqualityConstraint1<Config,Key> This;
|
||||
typedef NonlinearConstraint1<Config,Key> 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 <class Config, class Key1, class X1, class Key2, class X2>
|
||||
template <class Config, class Key1, class Key2>
|
||||
class NonlinearConstraint2 : public NonlinearConstraint<Config> {
|
||||
|
||||
public:
|
||||
typedef typename Key1::Value_t X1;
|
||||
typedef typename Key2::Value_t X2;
|
||||
|
||||
protected:
|
||||
typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> This;
|
||||
typedef NonlinearConstraint2<Config,Key1,Key2> This;
|
||||
typedef NonlinearConstraint<Config> Base;
|
||||
|
||||
/** keys for the constrained variables */
|
||||
|
|
@ -261,12 +271,16 @@ public:
|
|||
/**
|
||||
* Binary Equality constraint - simply forces the value of active() to true
|
||||
*/
|
||||
template <class Config, class Key1, class X1, class Key2, class X2>
|
||||
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, X1, Key2, X2> {
|
||||
template <class Config, class Key1, class Key2>
|
||||
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, Key2> {
|
||||
|
||||
public:
|
||||
typedef typename Key1::Value_t X1;
|
||||
typedef typename Key2::Value_t X2;
|
||||
|
||||
protected:
|
||||
typedef NonlinearEqualityConstraint2<Config,Key1,X1,Key2,X2> This;
|
||||
typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> Base;
|
||||
typedef NonlinearEqualityConstraint2<Config,Key1,Key2> This;
|
||||
typedef NonlinearConstraint2<Config,Key1,Key2> 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 <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> {
|
||||
|
||||
public:
|
||||
typedef typename Key1::Value_t X1;
|
||||
typedef typename Key2::Value_t X2;
|
||||
typedef typename Key3::Value_t X3;
|
||||
|
||||
protected:
|
||||
typedef NonlinearConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> This;
|
||||
typedef NonlinearConstraint3<Config,Key1,Key2,Key3> This;
|
||||
typedef NonlinearConstraint<Config> Base;
|
||||
|
||||
/** keys for the constrained variables */
|
||||
|
|
@ -366,12 +385,17 @@ public:
|
|||
/**
|
||||
* 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>
|
||||
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Config, Key1, X1, Key2, X2, Key3, X3> {
|
||||
template <class Config, class Key1, class Key2, class Key3>
|
||||
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:
|
||||
typedef NonlinearEqualityConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> This;
|
||||
typedef NonlinearConstraint3<Config,Key1,X1,Key2,X2,Key3,X3> Base;
|
||||
typedef NonlinearEqualityConstraint3<Config,Key1,Key2,Key3> This;
|
||||
typedef NonlinearConstraint3<Config,Key1,Key2,Key3> 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 Config, class Key, class X>
|
||||
class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key, X> {
|
||||
template<class Config, class Key>
|
||||
class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key> {
|
||||
|
||||
public:
|
||||
typedef typename Key::Value_t X;
|
||||
|
||||
protected:
|
||||
typedef NonlinearEqualityConstraint1<Config, Key, X> Base;
|
||||
typedef NonlinearEqualityConstraint1<Config, Key> Base;
|
||||
|
||||
X value_; /// fixed value for variable
|
||||
|
||||
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)
|
||||
: 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 Config, class Key, class X>
|
||||
class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> {
|
||||
template<class Config, class Key>
|
||||
class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, Key> {
|
||||
public:
|
||||
typedef typename Key::Value_t X;
|
||||
|
||||
protected:
|
||||
typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> Base;
|
||||
typedef NonlinearEqualityConstraint2<Config, Key, Key> Base;
|
||||
|
||||
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)
|
||||
: Base(key1, key2, X::Dim(), mu) {}
|
||||
|
|
|
|||
|
|
@ -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 Config, class Key, class T>
|
||||
class NonlinearEquality: public NonlinearFactor1<Config, Key, T> {
|
||||
template<class Config, class Key>
|
||||
class NonlinearEquality: public NonlinearFactor1<Config, Key> {
|
||||
|
||||
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<Config, Key, T> Base;
|
||||
typedef NonlinearFactor1<Config, Key> Base;
|
||||
|
||||
/**
|
||||
* Constructor - forces exact evaluation
|
||||
|
|
@ -78,8 +82,8 @@ namespace gtsam {
|
|||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
|
||||
const NonlinearEquality<Config,Key,T>* p =
|
||||
dynamic_cast<const NonlinearEquality<Config,Key,T>*> (&f);
|
||||
const NonlinearEquality<Config,Key>* p =
|
||||
dynamic_cast<const NonlinearEquality<Config,Key>*> (&f);
|
||||
if (p == NULL) return false;
|
||||
if (!Base::equals(*p)) return false;
|
||||
return compare_(feasible_, p->feasible_);
|
||||
|
|
|
|||
|
|
@ -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<class Config, class Key, class X>
|
||||
template<class Config, class Key>
|
||||
class NonlinearFactor1: public NonlinearFactor<Config> {
|
||||
|
||||
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<Config> Base;
|
||||
typedef NonlinearFactor1<Config, Key, X> This;
|
||||
typedef NonlinearFactor1<Config, Key> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -237,9 +242,15 @@ namespace gtsam {
|
|||
/**
|
||||
* 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> {
|
||||
|
||||
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<Config> Base;
|
||||
typedef NonlinearFactor2<Config, Key1, X1, Key2, X2> This;
|
||||
typedef NonlinearFactor2<Config, Key1, Key2> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -352,18 +363,25 @@ namespace gtsam {
|
|||
/**
|
||||
* 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> {
|
||||
|
||||
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<Config> Base;
|
||||
typedef NonlinearFactor3<Config, Key1, X1, Key2, X2, Key3, X3> This;
|
||||
typedef NonlinearFactor3<Config, Key1, Key2, Key3> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
|
|||
|
|
@ -11,34 +11,37 @@
|
|||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/nonlinear/LieConfig-inl.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
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 )
|
||||
{
|
||||
LieConfig<string,Vector> expected;
|
||||
Config expected;
|
||||
Vector v = Vector_(3, 5.0, 6.0, 7.0);
|
||||
expected.insert("a",v);
|
||||
LieConfig<string,Vector> actual;
|
||||
actual.insert("a",v);
|
||||
expected.insert(key1,v);
|
||||
Config actual;
|
||||
actual.insert(key1,v);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LieConfig, equals2 )
|
||||
{
|
||||
LieConfig<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<string,Vector> 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<Vector> v = config0.exists_("v1");
|
||||
boost::optional<Vector> v = config0.exists_(key1);
|
||||
CHECK(assert_equal(Vector_(1, 1.),*v));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LieConfig, update)
|
||||
{
|
||||
LieConfig<string,Vector> 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<string,Vector> 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<string,Vector> 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<Vector, 'z'> Key;
|
||||
typedef LieConfig<Key,Vector> Config1;
|
||||
typedef LieConfig<string,Vector> Config2;
|
||||
typedef LieConfig<Key> 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());
|
||||
|
|
|
|||
|
|
@ -16,13 +16,12 @@ namespace gtsam {
|
|||
* Binary factor for a bearing measurement
|
||||
*/
|
||||
template<class Config, class PoseKey, class PointKey>
|
||||
class BearingFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
|
||||
PointKey, Point2> {
|
||||
class BearingFactor: public NonlinearFactor2<Config, PoseKey, PointKey> {
|
||||
private:
|
||||
|
||||
Rot2 z_; /** measurement */
|
||||
|
||||
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
|
||||
typedef NonlinearFactor2<Config, PoseKey, PointKey> Base;
|
||||
|
||||
public:
|
||||
|
||||
|
|
|
|||
|
|
@ -17,15 +17,14 @@ namespace gtsam {
|
|||
* Binary factor for a bearing measurement
|
||||
*/
|
||||
template<class Config, class PoseKey, class PointKey>
|
||||
class BearingRangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
|
||||
PointKey, Point2> {
|
||||
class BearingRangeFactor: public NonlinearFactor2<Config, PoseKey, PointKey> {
|
||||
private:
|
||||
|
||||
// the measurement
|
||||
Rot2 bearing_;
|
||||
double range_;
|
||||
|
||||
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
|
||||
typedef NonlinearFactor2<Config, PoseKey, PointKey> Base;
|
||||
|
||||
public:
|
||||
|
||||
|
|
|
|||
|
|
@ -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 Config, class Key, class X>
|
||||
class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> {
|
||||
template<class Config, class Key>
|
||||
class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, Key> {
|
||||
public:
|
||||
typedef typename Key::Value_t X;
|
||||
|
||||
protected:
|
||||
typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> Base;
|
||||
typedef NonlinearEqualityConstraint2<Config, Key, Key> Base;
|
||||
|
||||
X measured_; /// fixed between value
|
||||
|
||||
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)
|
||||
: Base(key1, key2, X::Dim(), mu), measured_(measured) {}
|
||||
|
|
|
|||
|
|
@ -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 Config, class Key1, class T, class Key2 = Key1>
|
||||
class BetweenFactor: public NonlinearFactor2<Config, Key1, T, Key2, T> {
|
||||
template<class Config, class Key1, class Key2 = Key1>
|
||||
class BetweenFactor: public NonlinearFactor2<Config, Key1, Key2> {
|
||||
|
||||
public:
|
||||
typedef typename Key1::Value_t T;
|
||||
|
||||
private:
|
||||
|
||||
typedef NonlinearFactor2<Config, Key1, T, Key2, T> Base;
|
||||
typedef NonlinearFactor2<Config, Key1, Key2> Base;
|
||||
|
||||
T measured_; /** The measurement */
|
||||
|
||||
|
|
@ -47,8 +52,8 @@ namespace gtsam {
|
|||
|
||||
/** equals */
|
||||
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
|
||||
const BetweenFactor<Config, Key1, T, Key2> *e =
|
||||
dynamic_cast<const BetweenFactor<Config, Key1, T>*> (&expected);
|
||||
const BetweenFactor<Config, Key1, Key2> *e =
|
||||
dynamic_cast<const BetweenFactor<Config, Key1, Key2>*> (&expected);
|
||||
return e != NULL && Base::equals(expected) && this->measured_.equals(
|
||||
e->measured_, tol);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,10 +17,11 @@ namespace gtsam {
|
|||
* will need to have its value function implemented to return
|
||||
* a scalar for comparison.
|
||||
*/
|
||||
template<class Cfg, class Key, class X>
|
||||
struct BoundingConstraint1: public NonlinearConstraint1<Cfg, Key, X> {
|
||||
typedef NonlinearConstraint1<Cfg, Key, X> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint1<Cfg, Key, X> > shared_ptr;
|
||||
template<class Cfg, class Key>
|
||||
struct BoundingConstraint1: public NonlinearConstraint1<Cfg, Key> {
|
||||
typedef typename Key::Value_t X;
|
||||
typedef NonlinearConstraint1<Cfg, Key> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint1<Cfg, Key> > 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<class Cfg, class Key1, class X1, class Key2, class X2>
|
||||
struct BoundingConstraint2: public NonlinearConstraint2<Cfg, Key1, X1, Key2, X2> {
|
||||
typedef NonlinearConstraint2<Cfg, Key1, X1, Key2, X2> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint2<Cfg, Key1, X1, Key2, X2> > shared_ptr;
|
||||
template<class Cfg, class Key1, class Key2>
|
||||
struct BoundingConstraint2: public NonlinearConstraint2<Cfg, Key1, Key2> {
|
||||
typedef typename Key1::Value_t X1;
|
||||
typedef typename Key2::Value_t X2;
|
||||
|
||||
typedef NonlinearConstraint2<Cfg, Key1, Key2> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint2<Cfg, Key1, Key2> > shared_ptr;
|
||||
|
||||
double threshold_;
|
||||
bool isGreaterThan_; /// flag for greater/less than
|
||||
|
|
|
|||
|
|
@ -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 Config, class Key, class T>
|
||||
class PriorFactor: public NonlinearFactor1<Config, Key, T> {
|
||||
template<class Config, class Key>
|
||||
class PriorFactor: public NonlinearFactor1<Config, Key> {
|
||||
|
||||
public:
|
||||
typedef typename Key::Value_t T;
|
||||
|
||||
private:
|
||||
|
||||
typedef NonlinearFactor1<Config, Key, T> Base;
|
||||
typedef NonlinearFactor1<Config, Key> Base;
|
||||
|
||||
T prior_; /** The measurement */
|
||||
|
||||
|
|
@ -50,8 +53,8 @@ namespace gtsam {
|
|||
|
||||
/** equals */
|
||||
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
|
||||
const PriorFactor<Config, Key, T> *e = dynamic_cast<const PriorFactor<
|
||||
Config, Key, T>*> (&expected);
|
||||
const PriorFactor<Config, Key> *e = dynamic_cast<const PriorFactor<
|
||||
Config, Key>*> (&expected);
|
||||
return e != NULL && Base::equals(expected) && this->prior_.equals(
|
||||
e->prior_, tol);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,13 +15,12 @@ namespace gtsam {
|
|||
* Binary factor for a range measurement
|
||||
*/
|
||||
template<class Config, class PoseKey, class PointKey>
|
||||
class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey,
|
||||
Point2> {
|
||||
class RangeFactor: public NonlinearFactor2<Config, PoseKey, PointKey> {
|
||||
private:
|
||||
|
||||
double z_; /** measurement */
|
||||
|
||||
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
|
||||
typedef NonlinearFactor2<Config, PoseKey, PointKey> Base;
|
||||
|
||||
public:
|
||||
|
||||
|
|
|
|||
|
|
@ -21,8 +21,8 @@ namespace simulated3D {
|
|||
|
||||
typedef VectorConfig VectorConfig;
|
||||
|
||||
typedef gtsam::Symbol PoseKey;
|
||||
typedef gtsam::Symbol PointKey;
|
||||
typedef gtsam::TypedSymbol<Vector, 'x'> PoseKey;
|
||||
typedef gtsam::TypedSymbol<Vector, 'l'> 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<VectorConfig, PoseKey,
|
||||
Vector> {
|
||||
struct Point2Prior3D: public NonlinearFactor1<VectorConfig, PoseKey> {
|
||||
|
||||
Vector z_;
|
||||
|
||||
Point2Prior3D(const Vector& z,
|
||||
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 =
|
||||
|
|
@ -62,13 +61,13 @@ namespace simulated3D {
|
|||
};
|
||||
|
||||
struct Simulated3DMeasurement: public NonlinearFactor2<VectorConfig,
|
||||
PoseKey, Vector, PointKey, Vector> {
|
||||
PoseKey, PointKey> {
|
||||
|
||||
Vector z_;
|
||||
|
||||
Simulated3DMeasurement(const Vector& z,
|
||||
const SharedGaussian& model, PoseKey& j1, PointKey j2) :
|
||||
NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, Vector> (
|
||||
NonlinearFactor2<VectorConfig, PoseKey, PointKey> (
|
||||
model, j1, j2), z_(z) {
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -25,12 +25,15 @@ namespace gtsam {
|
|||
* This base class should be specialized to implement the cost function for
|
||||
* specific classes of landmarks
|
||||
*/
|
||||
template<class Config, class PKey, class Point, class TKey, class Transform>
|
||||
class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> {
|
||||
template<class Config, class PKey, class TKey>
|
||||
class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, TKey, PKey> {
|
||||
|
||||
public:
|
||||
typedef NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> Base;
|
||||
typedef TransformConstraint<Config, PKey, Point, TKey, Transform> This;
|
||||
typedef typename PKey::Value_t Point;
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -23,14 +23,14 @@ namespace gtsam {
|
|||
// Keys and Config
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef TupleConfig2<PoseConfig, PointConfig> Config;
|
||||
|
||||
// Factors
|
||||
typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint;
|
||||
typedef PriorFactor<Config, PoseKey, Pose2> Prior;
|
||||
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
|
||||
typedef NonlinearEquality<Config, PoseKey> Constraint;
|
||||
typedef PriorFactor<Config, PoseKey> Prior;
|
||||
typedef BetweenFactor<Config, PoseKey> Odometry;
|
||||
typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
|
||||
typedef RangeFactor<Config, PoseKey, PointKey> Range;
|
||||
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ namespace gtsam {
|
|||
|
||||
// Keys and Config
|
||||
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)
|
||||
|
|
@ -35,13 +35,13 @@ namespace gtsam {
|
|||
Config circle(size_t n, double R);
|
||||
|
||||
// Factors
|
||||
typedef PriorFactor<Config, Key, Pose2> Prior;
|
||||
typedef BetweenFactor<Config, Key, Pose2> Constraint;
|
||||
typedef NonlinearEquality<Config, Key, Pose2> HardConstraint;
|
||||
typedef PriorFactor<Config, Key> Prior;
|
||||
typedef BetweenFactor<Config, Key> Constraint;
|
||||
typedef NonlinearEquality<Config, Key> HardConstraint;
|
||||
|
||||
// Graph
|
||||
struct Graph: public NonlinearFactorGraph<Config> {
|
||||
typedef BetweenFactor<Config, Key, Pose2> Constraint;
|
||||
typedef BetweenFactor<Config, Key> 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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ namespace gtsam {
|
|||
|
||||
// Keys and Config
|
||||
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)
|
||||
|
|
@ -34,9 +34,9 @@ namespace gtsam {
|
|||
Config circle(size_t n, double R);
|
||||
|
||||
// Factors
|
||||
typedef PriorFactor<Config, Key, Pose3> Prior;
|
||||
typedef BetweenFactor<Config, Key, Pose3> Constraint;
|
||||
typedef NonlinearEquality<Config, Key, Pose3> HardConstraint;
|
||||
typedef PriorFactor<Config, Key> Prior;
|
||||
typedef BetweenFactor<Config, Key> Constraint;
|
||||
typedef NonlinearEquality<Config, Key> HardConstraint;
|
||||
|
||||
// Graph
|
||||
struct Graph: public NonlinearFactorGraph<Config> {
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -21,8 +21,8 @@ namespace gtsam {
|
|||
// Simulated2D robots have no orientation, just a position
|
||||
typedef TypedSymbol<Point2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef LieConfig<PoseKey, Point2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef TupleConfig2<PoseConfig, PointConfig> Config;
|
||||
|
||||
/**
|
||||
|
|
@ -55,13 +55,13 @@ namespace gtsam {
|
|||
* Unary factor encoding a soft prior on a vector
|
||||
*/
|
||||
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;
|
||||
|
||||
Point2 z_;
|
||||
|
||||
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 =
|
||||
|
|
@ -75,14 +75,13 @@ namespace gtsam {
|
|||
* Binary factor simulating "odometry" between two Vectors
|
||||
*/
|
||||
template<class Cfg = Config, class Key = PoseKey>
|
||||
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Point2, Key,
|
||||
Point2> {
|
||||
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> {
|
||||
typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr;
|
||||
Point2 z_;
|
||||
|
||||
GenericOdometry(const Point2& z, const SharedGaussian& model,
|
||||
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<
|
||||
|
|
@ -96,15 +95,14 @@ namespace gtsam {
|
|||
* Binary factor simulating "measurement" between two Vectors
|
||||
*/
|
||||
template<class Cfg = Config, class XKey = PoseKey, class LKey = PointKey>
|
||||
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey,
|
||||
Point2> {
|
||||
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, LKey> {
|
||||
public:
|
||||
typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr;
|
||||
Point2 z_;
|
||||
|
||||
GenericMeasurement(const Point2& z, const SharedGaussian& model,
|
||||
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<
|
||||
|
|
|
|||
|
|
@ -24,13 +24,13 @@ namespace gtsam {
|
|||
namespace equality_constraints {
|
||||
|
||||
/** Typedefs for regular use */
|
||||
typedef NonlinearEquality1<Config, PoseKey, Point2> UnaryEqualityConstraint;
|
||||
typedef NonlinearEquality1<Config, PointKey, Point2> UnaryEqualityPointConstraint;
|
||||
typedef BetweenConstraint<Config, PoseKey, Point2> OdoEqualityConstraint;
|
||||
typedef NonlinearEquality1<Config, PoseKey> UnaryEqualityConstraint;
|
||||
typedef NonlinearEquality1<Config, PointKey> UnaryEqualityPointConstraint;
|
||||
typedef BetweenConstraint<Config, PoseKey> OdoEqualityConstraint;
|
||||
|
||||
/** Equality between variables */
|
||||
typedef NonlinearEquality2<Config, PoseKey, Point2> PoseEqualityConstraint;
|
||||
typedef NonlinearEquality2<Config, PointKey, Point2> PointEqualityConstraint;
|
||||
typedef NonlinearEquality2<Config, PoseKey> PoseEqualityConstraint;
|
||||
typedef NonlinearEquality2<Config, PointKey> PointEqualityConstraint;
|
||||
|
||||
} // \namespace equality_constraints
|
||||
|
||||
|
|
@ -41,8 +41,8 @@ namespace gtsam {
|
|||
* Demo implementation: should be made more general using BoundingConstraint
|
||||
*/
|
||||
template<class Cfg, class Key, unsigned int Idx>
|
||||
struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key, Point2> {
|
||||
typedef BoundingConstraint1<Cfg, Key, Point2> Base;
|
||||
struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key> {
|
||||
typedef BoundingConstraint1<Cfg, Key> Base;
|
||||
typedef boost::shared_ptr<ScalarCoordConstraint1<Cfg, Key, Idx> > shared_ptr;
|
||||
|
||||
ScalarCoordConstraint1(const Key& key, double c,
|
||||
|
|
@ -75,8 +75,8 @@ namespace gtsam {
|
|||
* to be less than or equal to a bound
|
||||
*/
|
||||
template<class Cfg, class Key>
|
||||
struct MaxDistanceConstraint : public BoundingConstraint2<Cfg, Key, Point2, Key, Point2> {
|
||||
typedef BoundingConstraint2<Cfg, Key, Point2, Key, Point2> Base;
|
||||
struct MaxDistanceConstraint : public BoundingConstraint2<Cfg, Key, Key> {
|
||||
typedef BoundingConstraint2<Cfg, Key, Key> 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<class Cfg, class XKey, class PKey>
|
||||
struct MinDistanceConstraint : public BoundingConstraint2<Cfg, XKey, Point2, PKey, Point2> {
|
||||
typedef BoundingConstraint2<Cfg, XKey, Point2, PKey, Point2> Base;
|
||||
struct MinDistanceConstraint : public BoundingConstraint2<Cfg, XKey, PKey> {
|
||||
typedef BoundingConstraint2<Cfg, XKey, PKey> Base;
|
||||
|
||||
MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0)
|
||||
: Base(key1, key2, range_bound, true, mu) {}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -21,8 +21,8 @@ namespace gtsam {
|
|||
// The types that take an oriented pose2 rather than point2
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef TupleConfig2<PoseConfig, PointConfig> 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<class Cfg = Config, class Key = PoseKey>
|
||||
struct GenericPosePrior: public NonlinearFactor1<Cfg, Key, Pose2> {
|
||||
struct GenericPosePrior: public NonlinearFactor1<Cfg, Key> {
|
||||
|
||||
Pose2 z_;
|
||||
|
||||
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 =
|
||||
|
|
@ -67,13 +67,12 @@ namespace gtsam {
|
|||
* Binary factor simulating "odometry" between two Vectors
|
||||
*/
|
||||
template<class Cfg = Config, class Key = PoseKey>
|
||||
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Pose2, Key,
|
||||
Pose2> {
|
||||
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> {
|
||||
Pose2 z_;
|
||||
|
||||
GenericOdometry(const Pose2& z, const SharedGaussian& model,
|
||||
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<
|
||||
|
|
|
|||
|
|
@ -183,14 +183,13 @@ namespace example {
|
|||
}
|
||||
|
||||
struct UnaryFactor: public gtsam::NonlinearFactor1<Config,
|
||||
simulated2D::PoseKey, Point2> {
|
||||
simulated2D::PoseKey> {
|
||||
|
||||
Point2 z_;
|
||||
|
||||
UnaryFactor(const Point2& z, const SharedGaussian& model,
|
||||
const simulated2D::PoseKey& key) :
|
||||
gtsam::NonlinearFactor1<Config, simulated2D::PoseKey,
|
||||
Point2>(model, key), z_(z) {
|
||||
gtsam::NonlinearFactor1<Config, simulated2D::PoseKey>(model, key), z_(z) {
|
||||
}
|
||||
|
||||
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
|
||||
|
|
|
|||
|
|
@ -24,13 +24,13 @@ namespace gtsam { namespace visualSLAM {
|
|||
*/
|
||||
typedef TypedSymbol<Pose3,'x'> PoseKey;
|
||||
typedef TypedSymbol<Point3,'l'> PointKey;
|
||||
typedef LieConfig<PoseKey, Pose3> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point3> PointConfig;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef TupleConfig2<PoseConfig, PointConfig> Config;
|
||||
typedef boost::shared_ptr<Config> shared_config;
|
||||
|
||||
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;
|
||||
typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint;
|
||||
typedef NonlinearEquality<Config, PoseKey> PoseConstraint;
|
||||
typedef NonlinearEquality<Config, PointKey> PointConstraint;
|
||||
|
||||
|
||||
/**
|
||||
|
|
@ -38,7 +38,7 @@ namespace gtsam { namespace visualSLAM {
|
|||
* i.e. the main building block for visual SLAM.
|
||||
*/
|
||||
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:
|
||||
|
||||
// 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<Cfg, PosK, Pose3, LmK, Point3> Base;
|
||||
typedef NonlinearFactor2<Cfg, PosK, LmK> Base;
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<GenericProjectionFactor<Cfg, LmK, PosK> > shared_ptr;
|
||||
|
|
|
|||
|
|
@ -31,8 +31,8 @@ static const double tol = 1e-5;
|
|||
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
|
||||
// some generic poses, points and keys
|
||||
PoseKey x1(1), x2(2);
|
||||
|
|
@ -484,8 +484,8 @@ TEST( testFusionTupleConfig, basic_factor)
|
|||
|
||||
// Factors
|
||||
// typedef PriorFactor<TestPoseConfig, PoseKey, Pose2> Prior; // fails to add to graph
|
||||
typedef PriorFactor<Config, PoseKey, Pose2> Prior;
|
||||
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
|
||||
typedef PriorFactor<Config, PoseKey> Prior;
|
||||
typedef BetweenFactor<Config, PoseKey> Odometry;
|
||||
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;
|
||||
|
||||
PoseKey pose1k(1), pose2k(2), pose3k(3);
|
||||
|
|
|
|||
|
|
@ -5,14 +5,11 @@
|
|||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/linear/VectorConfig.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
|
||||
|
|
@ -21,37 +18,31 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
|
||||
typedef boost::shared_ptr<NLE> shared_nle;
|
||||
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef PriorFactor<PoseConfig, PoseKey, Pose2> PosePrior;
|
||||
typedef NonlinearEquality<PoseConfig, PoseKey, Pose2> PoseNLE;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef PriorFactor<PoseConfig, PoseKey> PosePrior;
|
||||
typedef NonlinearEquality<PoseConfig, PoseKey> PoseNLE;
|
||||
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
|
||||
|
||||
typedef NonlinearFactorGraph<PoseConfig> PoseGraph;
|
||||
typedef NonlinearOptimizer<PoseGraph,PoseConfig> 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<double>::infinity())));
|
||||
EXPECT(assert_equal(actual, repeat(3, std::numeric_limits<double>::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()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -285,7 +285,7 @@ typedef visualSLAM::Graph VGraph;
|
|||
typedef NonlinearOptimizer<VGraph,VConfig> VOptimizer;
|
||||
|
||||
// factors for visual slam
|
||||
typedef NonlinearEquality2<VConfig, visualSLAM::PointKey, Point3> Point3Equality;
|
||||
typedef NonlinearEquality2<VConfig, visualSLAM::PointKey> Point3Equality;
|
||||
|
||||
/* ********************************************************************* */
|
||||
TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
|
||||
|
|
|
|||
|
|
@ -33,19 +33,19 @@ typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
|||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef TypedSymbol<Pose2, 'T'> TransformKey;
|
||||
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<TransformKey, Pose2> TransformConfig;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef LieConfig<TransformKey> TransformConfig;
|
||||
|
||||
typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig;
|
||||
typedef NonlinearFactorGraph<DDFConfig> DDFGraph;
|
||||
typedef NonlinearOptimizer<DDFGraph, DDFConfig> Optimizer;
|
||||
|
||||
typedef NonlinearEquality<DDFConfig, PoseKey, Pose2> PoseConstraint;
|
||||
typedef NonlinearEquality<DDFConfig, PointKey, Point2> PointConstraint;
|
||||
typedef NonlinearEquality<DDFConfig, TransformKey, Pose2> TransformPriorConstraint;
|
||||
typedef NonlinearEquality<DDFConfig, PoseKey> PoseConstraint;
|
||||
typedef NonlinearEquality<DDFConfig, PointKey> PointConstraint;
|
||||
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);
|
||||
TransformKey t1(1);
|
||||
|
|
|
|||
|
|
@ -26,8 +26,8 @@ static const double tol = 1e-5;
|
|||
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef TupleConfig2<PoseConfig, PointConfig> Config;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -202,12 +202,12 @@ typedef TypedSymbol<Point3, 'b'> Point3Key;
|
|||
typedef TypedSymbol<Point3, 'c'> Point3Key2;
|
||||
|
||||
// some config types
|
||||
typedef LieConfig<PoseKey, Pose2> PoseConfig;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
typedef LieConfig<LamKey, Vector> LamConfig;
|
||||
typedef LieConfig<Pose3Key, Pose3> Pose3Config;
|
||||
typedef LieConfig<Point3Key, Point3> Point3Config;
|
||||
typedef LieConfig<Point3Key2, Point3> Point3Config2;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef LieConfig<LamKey> LamConfig;
|
||||
typedef LieConfig<Pose3Key> Pose3Config;
|
||||
typedef LieConfig<Point3Key> Point3Config;
|
||||
typedef LieConfig<Point3Key2> Point3Config2;
|
||||
|
||||
// some TupleConfig types
|
||||
typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA;
|
||||
|
|
|
|||
Loading…
Reference in New Issue