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

Replaced instances of PairConfig with TupleConfig2, cleaned up documentation on TupleConfigs

release/4.3a0
Alex Cunningham 2010-07-19 19:55:24 +00:00
parent 750269e986
commit d938f92844
14 changed files with 469 additions and 510 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

418
.cproject
View File

File diff suppressed because it is too large Load Diff

4
nonlinear/LieConfig.h
View File

@ -100,10 +100,10 @@ namespace gtsam {
// imperative methods:
/** Add a variable with the given j */
/** Add a variable with the given j - does not replace existing values */
void insert(const J& j, const T& val);
/** Add a set of variables */
/** Add a set of variables - does note replace existing values */
void insert(const LieConfig& cfg);
/** update the current available values without adding new ones */

55
nonlinear/TupleConfig-inl.h
View File

@ -1,23 +1,15 @@
/*
* TupleConfig-inl.h
*
* Created on: Jan 14, 2010
* Author: richard
/**
* @file TupleConfig-inl.h
* @author Richard Roberts
* @author Manohar Paluri
* @author Alex Cunningham
*/
#pragma once
#include <iostream>
#include "LieConfig-inl.h"
#include "TupleConfig.h"
#define INSTANTIATE_PAIR_CONFIG(J1,X1,J2,X2) \
/*INSTANTIATE_LIE_CONFIG(J1,X1);*/ \
/*INSTANTIATE_LIE_CONFIG(J2,X2);*/ \
template class PairConfig<J1,X1,J2,X2>; \
/*template void PairConfig<J1,X1,J2,X2>::print(const std::string&) const;*/ \
template PairConfig<J1,X1,J2,X2> expmap(const PairConfig<J1,X1,J2,X2>&, const VectorConfig&);
// TupleConfig instantiations for N = 1-6
#define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \
template class TupleConfig2<Config1, Config2>;
@ -37,43 +29,6 @@
namespace gtsam {
/* ************************************************************************* */
/** PairConfig implementations */
/* ************************************************************************* */
template<class J1, class X1, class J2, class X2>
void PairConfig<J1,X1,J2,X2>::print(const std::string& s) const {
std::cout << "TupleConfig " << s << ", size " << size() << "\n";
first().print(s + "Config1: ");
second().print(s + "Config2: ");
}
/* ************************************************************************* */
template<class J1, class X1, class J2, class X2>
void PairConfig<J1,X1,J2,X2>::insert(const PairConfig& config) {
for (typename Config1::const_iterator it = config.first().begin(); it!=config.first().end(); it++) {
insert(it->first, it->second);
}
for (typename Config2::const_iterator it = config.second().begin(); it!=config.second().end(); it++) {
insert(it->first, it->second);
}
}
/* ************************************************************************* */
/** TupleConfig Implementations */
/* ************************************************************************* */
template<class Config1, class Config2>
void TupleConfig<Config1, Config2>::print(const std::string& s) const {
std::cout << s << " : " << std::endl;
first_.print();
second_.print();
}
template<class Config1>
void TupleConfigEnd<Config1>::print(const std::string& s ) const {
first_.print();
}
/* ************************************************************************* */
/** TupleConfigN Implementations */
/* ************************************************************************* */

453
nonlinear/TupleConfig.h
View File

@ -1,8 +1,8 @@
/*
* TupleConfig.h
*
* Created on: Jan 13, 2010
* Author: Richard Roberts and Manohar Paluri
/**
* @file TupleConfig.h
* @author Richard Roberts
* @author Manohar Paluri
* @author Alex Cunningham
*/
#include "LieConfig.h"
@ -13,23 +13,36 @@
namespace gtsam {
/**
* Tuple configs to handle subconfigs of LieConfigs
* TupleConfigs are a structure to manage heterogenous LieConfigs, so as to
* enable different types of variables/keys to be used simultaneously. The
* interface is designed to mimic that of a single LieConfig.
*
* This uses a recursive structure of config pairs to form a lisp-like
* list, with a special case (TupleConfigEnd) that contains only one config
* at the end. In a final use case, this should be aliased to something clearer
* but still with the same recursive type machinery.
* at the end. Because this recursion is done at compile time, there is no
* runtime performance hit to using this structure.
*
* For an easier to use approach, there are TupleConfigN classes, which wrap
* the recursive TupleConfigs together as a single class, so you can have
* mixed-type classes from 2-6 types. Note that a TupleConfig2 is equivalent
* to the previously used PairConfig.
*
* Design and extension note:
* To implement a recursively templated data structure, note that most operations
* have two versions: one with templates and one without. The templated one allows
* for the arguments to be passed to the next config, while the specialized one
* operates on the "first" config. TupleConfigEnd contains only the specialized version.
*/
template<class Config1, class Config2>
class TupleConfig : public Testable<TupleConfig<Config1, Config2> > {
protected:
// Data for internal configs
Config1 first_;
Config2 second_;
Config1 first_; /// Arbitrary config
Config2 second_; /// A TupleConfig or TupleConfigEnd, which wraps an arbitrary config
public:
// typedefs
// typedefs for config subtypes
typedef class Config1::Key Key1;
typedef class Config1::Value Value1;
@ -47,19 +60,34 @@ namespace gtsam {
virtual ~TupleConfig() {}
/** Print */
void print(const std::string& s = "") const;
void print(const std::string& s = "") const {
first_.print(s);
second_.print();
}
/** Test for equality in keys and values */
/** Equality with tolerance for keys and values */
bool equals(const TupleConfig<Config1, Config2>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
}
// insert function that uses the second (recursive) config
/**
* Insert a key/value pair to the config.
* Note: if the key is already in the config, the config will not be changed.
* Use update() to allow for changing existing values.
* @param key is the key - can be an int (second version) if the can can be initialized from an int
* @param value is the value to insert
*/
template<class Key, class Value>
void insert(const Key& key, const Value& value) {second_.insert(key, value);}
void insert(const int& key, const Value1& value) {first_.insert(Key1(key), value);}
void insert(const Key1& key, const Value1& value) {first_.insert(key, value);}
// insert function for whole configs
/**
* Insert a complete config at a time.
* Note: if the key is already in the config, the config will not be changed.
* Use update() to allow for changing existing values.
* @param config is a full config to add
*/
template<class Cfg1, class Cfg2>
void insert(const TupleConfig<Cfg1, Cfg2>& config) { second_.insert(config); }
void insert(const TupleConfig<Config1, Config2>& config) {
@ -67,7 +95,10 @@ namespace gtsam {
second_.insert(config.second_);
}
// update function for whole configs
/**
* Update function for whole configs - this will change existing values
* @param config is a config to add
*/
template<class Cfg1, class Cfg2>
void update(const TupleConfig<Cfg1, Cfg2>& config) { second_.update(config); }
void update(const TupleConfig<Config1, Config2>& config) {
@ -75,52 +106,66 @@ namespace gtsam {
second_.update(config.second_);
}
// update function for single elements
/**
* Update function for single key/value pairs - will change existing values
* @param key is the variable identifier
* @param value is the variable value to update
*/
template<class Key, class Value>
void update(const Key& key, const Value& value) { second_.update(key, value); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); }
// insert a subconfig
/**
* Insert a subconfig
* @param config is the config to insert
*/
template<class Cfg>
void insertSub(const Cfg& config) { second_.insertSub(config); }
void insertSub(const Config1& config) { first_.insert(config); }
// erase an element by key
/** erase an element by key */
template<class Key>
void erase(const Key& j) { second_.erase(j); }
void erase(const Key1& j) { first_.erase(j); }
// determine whether an element exists
/** determine whether an element exists */
template<class Key>
bool exists(const Key& j) const { return second_.exists(j); }
bool exists(const Key1& j) const { return first_.exists(j); }
// a variant of exists
/** a variant of exists */
template<class Key>
boost::optional<typename Key::Value_t> exists_(const Key& j) const { return second_.exists_(j); }
boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); }
// access operator
/** access operator */
template<class Key>
const typename Key::Value_t & operator[](const Key& j) const { return second_[j]; }
const Value1& operator[](const Key1& j) const { return first_[j]; }
// at access function
/** at access function */
template<class Key>
const typename Key::Value_t & at(const Key& j) const { return second_.at(j); }
const Value1& at(const Key1& j) const { return first_.at(j); }
// direct config access
/** direct config access */
const Config1& config() const { return first_; }
const Config2& rest() const { return second_; }
// size function - adds recursively
/** zero: create VectorConfig of appropriate structure */
VectorConfig zero() const {
VectorConfig z1 = first_.zero(), z2 = second_.zero();
z2.insert(z1);
return z2;
}
/** @return number of key/value pairs stored */
size_t size() const { return first_.size() + second_.size(); }
// dim function
/** @return The dimensionality of the tangent space */
size_t dim() const { return first_.dim() + second_.dim(); }
// Expmap
/** Expmap */
TupleConfig<Config1, Config2> expmap(const VectorConfig& delta) const {
return TupleConfig(gtsam::expmap(first_, delta), second_.expmap(delta));
}
@ -132,12 +177,22 @@ namespace gtsam {
return ret;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(first_);
ar & BOOST_SERIALIZATION_NVP(second_);
}
};
/**
* End of a recursive TupleConfig - contains only one config
*
* This should not be used directly
* Do not use this class directly - it should only be used as a part
* of a recursive structure
*/
template<class Config>
class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > {
@ -161,25 +216,23 @@ namespace gtsam {
virtual ~TupleConfigEnd() {}
/** Print */
void print(const std::string& s = "") const;
void print(const std::string& s = "") const {
first_.print();
}
/** Test for equality in keys and values */
bool equals(const TupleConfigEnd<Config>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol);
}
void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
void insert(const int& key, const Value1& value) {first_.insert(Key1(key), value);}
// insert function for whole configs
void insert(const TupleConfigEnd<Config>& config) {first_.insert(config.first_); }
// update function for whole configs
void update(const TupleConfigEnd<Config>& config) {first_.update(config.first_); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); }
// insert function for sub configs
void insertSub(const Config& config) {first_.insert(config); }
const Value1& operator[](const Key1& j) const { return first_[j]; }
@ -194,6 +247,11 @@ namespace gtsam {
const Value1& at(const Key1& j) const { return first_.at(j); }
VectorConfig zero() const {
VectorConfig z = first_.zero();
return z;
}
size_t size() const { return first_.size(); }
size_t dim() const { return first_.dim(); }
@ -206,6 +264,13 @@ namespace gtsam {
VectorConfig ret(gtsam::logmap(first_, cp.first_));
return ret;
}
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(first_);
}
};
/** Exmap static functions */
@ -227,15 +292,15 @@ namespace gtsam {
*
* The interface is designed to mimic PairConfig, but for 2-6 config types.
*/
template<class Config1, class Config2>
class TupleConfig2 : public TupleConfig<Config1, TupleConfigEnd<Config2> > {
template<class C1, class C2>
class TupleConfig2 : public TupleConfig<C1, TupleConfigEnd<C2> > {
public:
// typedefs
typedef Config1 Config1_t;
typedef Config2 Config2_t;
typedef C1 Config1;
typedef C2 Config2;
typedef TupleConfig<Config1, TupleConfigEnd<Config2> > Base;
typedef TupleConfig2<Config1, Config2> This;
typedef TupleConfig<C1, TupleConfigEnd<C2> > Base;
typedef TupleConfig2<C1, C2> This;
TupleConfig2() {}
TupleConfig2(const This& config);
@ -243,50 +308,55 @@ namespace gtsam {
TupleConfig2(const Config1& cfg1, const Config2& cfg2);
// access functions
inline const Config1_t& first() const { return this->config(); }
inline const Config2_t& second() const { return this->rest().config(); }
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
};
template<class Config1, class Config2>
TupleConfig2<Config1, Config2> expmap(const TupleConfig2<Config1, Config2>& c, const VectorConfig& delta) {
template<class C1, class C2>
TupleConfig2<C1, C2> expmap(const TupleConfig2<C1, C2>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class Config1, class Config2, class Config3>
class TupleConfig3 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > > {
template<class C1, class C2>
VectorConfig logmap(const TupleConfig2<C1, C2>& c1, const TupleConfig2<C1, C2>& c2) {
return c1.logmap(c2);
}
template<class C1, class C2, class C3>
class TupleConfig3 : public TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > > {
public:
// typedefs
typedef Config1 Config1_t;
typedef Config2 Config2_t;
typedef Config3 Config3_t;
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
TupleConfig3() {}
TupleConfig3(const TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >& config);
TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config);
TupleConfig3(const TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > >& config);
TupleConfig3(const TupleConfig3<C1, C2, C3>& config);
TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3);
// access functions
inline const Config1_t& first() const { return this->config(); }
inline const Config2_t& second() const { return this->rest().config(); }
inline const Config3_t& third() const { return this->rest().rest().config(); }
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
};
template<class Config1, class Config2, class Config3>
TupleConfig3<Config1, Config2, Config3> expmap(const TupleConfig3<Config1, Config2, Config3>& c, const VectorConfig& delta) {
template<class C1, class C2, class C3>
TupleConfig3<C1, C2, C3> expmap(const TupleConfig3<C1, C2, C3>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class Config1, class Config2, class Config3, class Config4>
class TupleConfig4 : public TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3, TupleConfigEnd<Config4> > > > {
template<class C1, class C2, class C3, class C4>
class TupleConfig4 : public TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > {
public:
// typedefs
typedef Config1 Config1_t;
typedef Config2 Config2_t;
typedef Config3 Config3_t;
typedef Config4 Config4_t;
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
typedef C4 Config4;
typedef TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3, TupleConfigEnd<Config4> > > > Base;
typedef TupleConfig4<Config1, Config2, Config3, Config4> This;
typedef TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > Base;
typedef TupleConfig4<C1, C2, C3, C4> This;
TupleConfig4() {}
TupleConfig4(const This& config);
@ -294,249 +364,74 @@ namespace gtsam {
TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4);
// access functions
inline const Config1_t& first() const { return this->config(); }
inline const Config2_t& second() const { return this->rest().config(); }
inline const Config3_t& third() const { return this->rest().rest().config(); }
inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); }
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
};
template<class Config1, class Config2, class Config3, class Config4>
TupleConfig4<Config1, Config2, Config3, Config4> expmap(const TupleConfig4<Config1, Config2, Config3, Config4>& c, const VectorConfig& delta) {
template<class C1, class C2, class C3, class C4>
TupleConfig4<C1, C2, C3, C4> expmap(const TupleConfig4<C1, C2, C3, C4>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class Config1, class Config2, class Config3, class Config4, class Config5>
class TupleConfig5 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > > {
template<class C1, class C2, class C3, class C4, class C5>
class TupleConfig5 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > > {
public:
// typedefs
typedef Config1 Config1_t;
typedef Config2 Config2_t;
typedef Config3 Config3_t;
typedef Config4 Config4_t;
typedef Config5 Config5_t;
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
typedef C4 Config4;
typedef C5 Config5;
TupleConfig5() {}
TupleConfig5(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& config);
TupleConfig5(const TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > >& config);
TupleConfig5(const TupleConfig5<C1, C2, C3, C4, C5>& config);
TupleConfig5(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > >& config);
TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5);
// access functions
inline const Config1_t& first() const { return this->config(); }
inline const Config2_t& second() const { return this->rest().config(); }
inline const Config3_t& third() const { return this->rest().rest().config(); }
inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5_t& fifth() const { return this->rest().rest().rest().rest().config(); }
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); }
};
template<class Config1, class Config2, class Config3, class Config4, class Config5>
TupleConfig5<Config1, Config2, Config3, Config4, Config5> expmap(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& c, const VectorConfig& delta) {
template<class C1, class C2, class C3, class C4, class C5>
TupleConfig5<C1, C2, C3, C4, C5> expmap(const TupleConfig5<C1, C2, C3, C4, C5>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
class TupleConfig6 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > > {
template<class C1, class C2, class C3, class C4, class C5, class C6>
class TupleConfig6 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > > {
public:
// typedefs
typedef Config1 Config1_t;
typedef Config2 Config2_t;
typedef Config3 Config3_t;
typedef Config4 Config4_t;
typedef Config5 Config5_t;
typedef Config6 Config6_t;
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
typedef C4 Config4;
typedef C5 Config5;
typedef C6 Config6;
TupleConfig6() {}
TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& config);
TupleConfig6(const TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >& config);
TupleConfig6(const TupleConfig6<C1, C2, C3, C4, C5, C6>& config);
TupleConfig6(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > >& config);
TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6);
// access functions
inline const Config1_t& first() const { return this->config(); }
inline const Config2_t& second() const { return this->rest().config(); }
inline const Config3_t& third() const { return this->rest().rest().config(); }
inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5_t& fifth() const { return this->rest().rest().rest().rest().config(); }
inline const Config6_t& sixth() const { return this->rest().rest().rest().rest().rest().config(); }
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); }
inline const Config6& sixth() const { return this->rest().rest().rest().rest().rest().config(); }
};
template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6> expmap(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& c, const VectorConfig& delta) {
template<class C1, class C2, class C3, class C4, class C5, class C6>
TupleConfig6<C1, C2, C3, C4, C5, C6> expmap(const TupleConfig6<C1, C2, C3, C4, C5, C6>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
/**
* PairConfig: an alias for a pair of configs using TupleConfig2
* STILL IN TESTING - will soon replace PairConfig
*/
// template<class J1, class X1, class J2, class X2>
// class PairConfig : public TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> > {
// public:
// PairConfig() {}
// PairConfig(const PairConfig<J1, X1, J2, X2>& config) :
// TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> >(config) {}
// PairConfig(const LieConfig<J1, X1>& cfg1,const LieConfig<J2, X2>& cfg2) :
// TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> >(cfg1, cfg2) {}
// };
/**
* PairConfig: a config that holds two data types.
* Note: this should eventually be replaced with a wrapper on TupleConfig2
*/
template<class J1, class X1, class J2, class X2>
class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > {
public:
// publicly available types
typedef LieConfig<J1, X1> Config1;
typedef LieConfig<J2, X2> Config2;
protected:
// Two configs in the pair as in std:pair
LieConfig<J1, X1> first_;
LieConfig<J2, X2> second_;
public:
PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& config2) :
first_(config1), second_(config2){}
/**
* Default constructor creates an empty config.
*/
PairConfig(){}
/**
* Copy constructor
*/
PairConfig(const PairConfig<J1, X1, J2, X2>& c):
first_(c.first_), second_(c.second_){}
/**
* Print
*/
void print(const std::string& s = "") const;
/**
* Test for equality in keys and values
*/
bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); }
/** Returns the real config */
inline const Config1& first() const { return first_; }
inline const Config2& second() const { return second_; }
/**
* operator[] syntax to get a value by j, throws invalid_argument if
* value with specified j is not present. Will generate compile-time
* errors if j type does not match that on which the Config is templated.
*/
const X1& operator[](const J1& j) const { return first_[j]; }
const X2& operator[](const J2& j) const { return second_[j]; }
/** member function version of access function */
const X1& at(const J1& j) const { return first_[j]; }
const X2& at(const J2& j) const { return second_[j]; }
/**
* size is the total number of variables in this config.
*/
size_t size() const { return first_.size() + second_.size(); }
/**
* dim is the dimensionality of the tangent space
*/
size_t dim() const { return first_.dim() + second_.dim(); }
private:
template<class Config, class Key, class Value>
void insert_helper(Config& config, const Key& j, const Value& value) {
config.insert(j, value);
}
template<class Config, class Key>
void erase_helper(Config& config, const Key& j) {
size_t dim;
config.erase(j, dim);
}
public:
/** zero: create VectorConfig of appropriate structure */
VectorConfig zero() const {
VectorConfig z1 = first_.zero(), z2 = second_.zero();
z1.insert(z2);
return z1;
}
/**
* Exponential map: expmap each element
*/
PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) const {
return PairConfig(gtsam::expmap(first_, delta), gtsam::expmap(second_, delta)); }
/**
* Logarithm: logmap each element
*/
VectorConfig logmap(const PairConfig<J1,X1,J2,X2>& cp) const {
VectorConfig ret(gtsam::logmap(first_, cp.first_));
ret.insert(gtsam::logmap(second_, cp.second_));
return ret;
}
/**
* Insert a variable with the given j
*/
void insert(const J1& j, const X1& value) { insert_helper(first_, j, value); }
void insert(const J2& j, const X2& value) { insert_helper(second_, j, value); }
void insert(const PairConfig& config);
/** Insert a subconfig */
void insertSub(const Config1& config) { first_.insert(config); }
void insertSub(const Config2& config) { second_.insert(config); }
/**
* Remove the variable with the given j. Throws invalid_argument if the
* j is not present in the config.
*/
void erase(const J1& j) { erase_helper(first_, j); }
void erase(const J2& j) { erase_helper(second_, j); }
/**
* Check if a variable exists
*/
bool exists(const J1& j) const { return first_.exists(j); }
bool exists(const J2& j) const { return second_.exists(j); }
boost::optional<X1> exists_(const J1& j) const { return first_.exists_(j); }
boost::optional<X2> exists_(const J2& j) const { return second_.exists_(j); }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(first_);
ar & BOOST_SERIALIZATION_NVP(second_);
}
};
/** exponential map */
template<class J1, class X1, class J2, class X2>
inline PairConfig<J1, X1, J2, X2> expmap(const PairConfig<J1, X1, J2, X2>& c,
const VectorConfig& delta) {
return c.expmap(delta);
}
/** log, inverse of exponential map */
template<class J1, class X1, class J2, class X2>
inline VectorConfig logmap(const PairConfig<J1, X1, J2, X2>& c0,
const PairConfig<J1, X1, J2, X2>& cp) {
return c0.logmap(cp);
}
}

2
slam/planarSLAM.cpp
View File

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

2
slam/planarSLAM.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
// Factors
typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint;

6
slam/simulated2D.cpp
View File

@ -5,13 +5,15 @@
*/
#include "simulated2D.h"
#include "LieConfig-inl.h"
#include "TupleConfig-inl.h"
namespace gtsam {
using namespace simulated2D;
// INSTANTIATE_LIE_CONFIG(PointKey, Point2)
INSTANTIATE_PAIR_CONFIG(PoseKey, Point2, PointKey, Point2)
INSTANTIATE_LIE_CONFIG(PointKey, Point2)
INSTANTIATE_LIE_CONFIG(PoseKey, Point2)
INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)

4
slam/simulated2D.h
View File

@ -21,7 +21,9 @@ namespace gtsam {
// Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Point2, PointKey, Point2> Config;
typedef LieConfig<PoseKey, Point2> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
/**
* Prior on a single pose, and optional derivative version

1
slam/simulated2DOriented.cpp
View File

@ -11,7 +11,6 @@ namespace gtsam {
using namespace simulated2DOriented;
// INSTANTIATE_LIE_CONFIG(PointKey, Point2)
// INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)

6
slam/simulated2DOriented.h
View File

@ -21,7 +21,9 @@ namespace gtsam {
// The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
typedef LieConfig<PoseKey, Pose2> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
//TODO:: point prior is not implemented right now
@ -71,7 +73,7 @@ namespace gtsam {
GenericOdometry(const Pose2& z, const SharedGaussian& model,
const Key& i1, const Key& i2) :
z_(z), NonlinearFactor2<Cfg, Key, Pose2, Key, Pose2> (model, i1, i2) {
NonlinearFactor2<Cfg, Key, Pose2, Key, Pose2> (model, i1, i2), z_(z) {
}
Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional<

1
slam/smallExample.cpp
View File

@ -21,6 +21,7 @@ using namespace std;
// template definitions
#include "FactorGraph-inl.h"
#include "TupleConfig-inl.h"
#include "NonlinearFactorGraph-inl.h"
namespace gtsam {

2
slam/visualSLAM.cpp
View File

@ -11,7 +11,7 @@
#include "NonlinearFactorGraph-inl.h"
namespace gtsam {
INSTANTIATE_PAIR_CONFIG(visualSLAM::PoseKey, Pose3, visualSLAM::PointKey, Point3)
INSTANTIATE_TUPLE_CONFIG2(visualSLAM::PoseConfig, visualSLAM::PointConfig)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Config)

5
slam/visualSLAM.h
View File

@ -24,7 +24,10 @@ namespace gtsam { namespace visualSLAM {
*/
typedef TypedSymbol<Pose3,'x'> PoseKey;
typedef TypedSymbol<Point3,'l'> PointKey;
typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config;
typedef LieConfig<PoseKey, Pose3> PoseConfig;
typedef LieConfig<PointKey, Point3> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;
typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint;

18
tests/testTupleConfig.cpp
View File

@ -25,10 +25,12 @@ using namespace std;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
typedef LieConfig<PoseKey, Pose2> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
/* ************************************************************************* */
TEST( PairConfig, constructors )
TEST( TupleConfig, constructors )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -50,7 +52,7 @@ TEST( PairConfig, constructors )
}
/* ************************************************************************* */
TEST( PairConfig, insert_equals1 )
TEST( TupleConfig, insert_equals1 )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -70,7 +72,7 @@ TEST( PairConfig, insert_equals1 )
CHECK(assert_equal(expected,actual));
}
TEST( PairConfig, insert_equals2 )
TEST( TupleConfig, insert_equals2 )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -94,7 +96,7 @@ TEST( PairConfig, insert_equals2 )
}
///* ************************************************************************* */
TEST( PairConfig, insert_duplicate )
TEST( TupleConfig, insert_duplicate )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -112,7 +114,7 @@ TEST( PairConfig, insert_duplicate )
}
/* ************************************************************************* */
TEST( PairConfig, size_dim )
TEST( TupleConfig, size_dim )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -128,7 +130,7 @@ TEST( PairConfig, size_dim )
}
/* ************************************************************************* */
TEST(PairConfig, at)
TEST(TupleConfig, at)
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -162,7 +164,7 @@ TEST(PairConfig, at)
}
/* ************************************************************************* */
TEST(PairConfig, zero_expmap_logmap)
TEST(TupleConfig, zero_expmap_logmap)
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);