Added wrapper classes for TupleConfig for ease of use.
Alex Cunningham
parent
8fe0795dd0
commit
040d45cb45
|
|
@ -20,21 +20,83 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
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: ");
|
||||
}
|
||||
/** 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);
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/** TupleConfigN Implementations */
|
||||
/* ************************************************************************* */
|
||||
template<class Config1, class Config2>
|
||||
TupleConfig2<Config1, Config2>::TupleConfig2(const TupleConfig2<Config1, Config2>& config) :
|
||||
TupleConfig<Config1, TupleConfigEnd<Config2> >(config) {}
|
||||
|
||||
template<class Config1, class Config2>
|
||||
TupleConfig2<Config1, Config2>::TupleConfig2(const Config1& cfg1, const Config2& cfg2) :
|
||||
TupleConfig<Config1, TupleConfigEnd<Config2> >(
|
||||
cfg1, TupleConfigEnd<Config2>(cfg2)) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3>
|
||||
TupleConfig3<Config1, Config2, Config3>::TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(config) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3>
|
||||
TupleConfig3<Config1, Config2, Config3>::TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(
|
||||
cfg1, TupleConfig<Config2, TupleConfigEnd<Config3> >(
|
||||
cfg2, TupleConfigEnd<Config3>(cfg3))) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4>
|
||||
TupleConfig4<Config1, Config2, Config3, Config4>::TupleConfig4(const TupleConfig4<Config1, Config2, Config3, Config4>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > > >(config) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4>
|
||||
TupleConfig4<Config1, Config2, Config3, Config4>::TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > > >(
|
||||
cfg1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > >(
|
||||
cfg2, TupleConfig<Config3, TupleConfigEnd<Config4> >(
|
||||
cfg3, TupleConfigEnd<Config4>(cfg4)))) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4, class Config5>
|
||||
TupleConfig5<Config1, Config2, Config3, Config4, Config5>::TupleConfig5(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > >(config) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4, class Config5>
|
||||
TupleConfig5<Config1, Config2, Config3, Config4, Config5>::TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
|
||||
const Config4& cfg4, const Config5& cfg5) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > >(
|
||||
cfg1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > >(
|
||||
cfg2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > >(
|
||||
cfg3, TupleConfig<Config4, TupleConfigEnd<Config5> >(
|
||||
cfg4, TupleConfigEnd<Config5>(cfg5))))) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
|
||||
TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >(config) {}
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
|
||||
TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
|
||||
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >(
|
||||
cfg1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > >(
|
||||
cfg2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > >(
|
||||
cfg3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > >(
|
||||
cfg4, TupleConfig<Config5, TupleConfigEnd<Config6> >(
|
||||
cfg5, TupleConfigEnd<Config6>(cfg6)))))) {}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,8 +33,6 @@ namespace gtsam {
|
|||
typedef class Config1::Key Key1;
|
||||
typedef class Config1::Value Value1;
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor */
|
||||
TupleConfig() {}
|
||||
|
||||
|
|
@ -81,6 +79,10 @@ namespace gtsam {
|
|||
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
|
||||
const Config1& config() const { return first_; }
|
||||
const Config2& rest() const { return second_; }
|
||||
|
||||
// size function - adds recursively
|
||||
size_t size() const { return first_.size() + second_.size(); }
|
||||
|
||||
|
|
@ -108,6 +110,7 @@ namespace gtsam {
|
|||
*/
|
||||
template<class Config>
|
||||
class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > {
|
||||
|
||||
protected:
|
||||
// Data for internal configs
|
||||
Config first_;
|
||||
|
|
@ -117,7 +120,6 @@ namespace gtsam {
|
|||
typedef class Config::Key Key1;
|
||||
typedef class Config::Value Value1;
|
||||
|
||||
public:
|
||||
TupleConfigEnd() {}
|
||||
|
||||
TupleConfigEnd(const TupleConfigEnd<Config>& config) :
|
||||
|
|
@ -140,6 +142,8 @@ namespace gtsam {
|
|||
|
||||
const Value1& operator[](const Key1& j) const { return first_[j]; }
|
||||
|
||||
const Config& config() const { return first_; }
|
||||
|
||||
void erase(const Key1& j) { first_.erase(j); }
|
||||
|
||||
bool exists(const Key1& j) const { return first_.exists(j); }
|
||||
|
|
@ -183,45 +187,110 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/**
|
||||
* Typedefs for existing config types
|
||||
* Wrapper classes to act as containers for configs. Note that these can be cascaded
|
||||
* recursively, as they are TupleConfigs, and are primarily a short form of the config
|
||||
* structure to make use of the TupleConfigs easier.
|
||||
*
|
||||
* The interface is designed to mimic PairConfig, but for 2-6 config types.
|
||||
*/
|
||||
template<class Config1, class Config2>
|
||||
struct TupleConfig2 : TupleConfig<Config1, TupleConfigEnd<Config2> > {
|
||||
class TupleConfig2 : public TupleConfig<Config1, TupleConfigEnd<Config2> > {
|
||||
public:
|
||||
// typedefs
|
||||
typedef Config1 Config1_t;
|
||||
typedef Config2 Config2_t;
|
||||
|
||||
TupleConfig2() {}
|
||||
TupleConfig2(const TupleConfig2<Config1, Config2>& config) :
|
||||
TupleConfig<Config1, TupleConfigEnd<Config2> >(config) {}
|
||||
TupleConfig2(const Config1& cfg1, const Config2& cfg2) :
|
||||
TupleConfig<Config1, TupleConfigEnd<Config2> >(cfg1, TupleConfigEnd<Config2>(cfg2)) {}
|
||||
TupleConfig2(const TupleConfig2<Config1, Config2>& config);
|
||||
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(); }
|
||||
};
|
||||
|
||||
template<class Config1, class Config2, class Config3>
|
||||
struct TupleConfig3 : TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > > {
|
||||
class TupleConfig3 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > > {
|
||||
public:
|
||||
// typedefs
|
||||
typedef Config1 Config1_t;
|
||||
typedef Config2 Config2_t;
|
||||
typedef Config3 Config3_t;
|
||||
|
||||
TupleConfig3() {}
|
||||
TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(config) {}
|
||||
TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(cfg1, TupleConfig<Config2, TupleConfigEnd<Config3> >(cfg2, TupleConfigEnd<Config3>(cfg3))) {}
|
||||
TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& 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(); }
|
||||
};
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4>
|
||||
struct TupleConfig4 : TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3, TupleConfigEnd<Config4> > > > {
|
||||
class TupleConfig4 : public TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3, TupleConfigEnd<Config4> > > > {
|
||||
public:
|
||||
// typedefs
|
||||
typedef Config1 Config1_t;
|
||||
typedef Config2 Config2_t;
|
||||
typedef Config3 Config3_t;
|
||||
typedef Config4 Config4_t;
|
||||
|
||||
TupleConfig4() {}
|
||||
TupleConfig4(const TupleConfig4<Config1, Config2, Config3, Config4>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > > >(config) {}
|
||||
TupleConfig4(const TupleConfig4<Config1, Config2, Config3, Config4>& config);
|
||||
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(); }
|
||||
};
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4, class Config5>
|
||||
struct TupleConfig5 : TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > > {
|
||||
class TupleConfig5 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > > {
|
||||
public:
|
||||
// typedefs
|
||||
typedef Config1 Config1_t;
|
||||
typedef Config2 Config2_t;
|
||||
typedef Config3 Config3_t;
|
||||
typedef Config4 Config4_t;
|
||||
typedef Config5 Config5_t;
|
||||
|
||||
TupleConfig5() {}
|
||||
TupleConfig5(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > >(config) {}
|
||||
TupleConfig5(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& 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(); }
|
||||
};
|
||||
|
||||
template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
|
||||
struct TupleConfig6 : TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > > {
|
||||
class TupleConfig6 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > > {
|
||||
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;
|
||||
|
||||
TupleConfig6() {}
|
||||
TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& config) :
|
||||
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >(config) {}
|
||||
TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& 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(); }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -229,7 +298,8 @@ namespace gtsam {
|
|||
* STILL IN TESTING - will soon replace PairConfig
|
||||
*/
|
||||
// template<class J1, class X1, class J2, class X2>
|
||||
// struct PairConfig : TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, 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) {}
|
||||
|
|
@ -239,6 +309,7 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* 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> > {
|
||||
|
|
@ -288,7 +359,7 @@ namespace gtsam {
|
|||
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); }
|
||||
|
||||
/** Direct access functions */
|
||||
/** Returns the real config */
|
||||
inline const Config1& first() const { return first_; }
|
||||
inline const Config2& second() const { return second_; }
|
||||
|
||||
|
|
|
|||
|
|
@ -34,16 +34,16 @@ TEST( PairConfig, insert_equals1 )
|
|||
Point2 l1(4,5), l2(9,10);
|
||||
|
||||
Config expected;
|
||||
expected.insert(1, x1);
|
||||
expected.insert(2, x2);
|
||||
expected.insert(1, l1);
|
||||
expected.insert(2, l2);
|
||||
expected.insert(PoseKey(1), x1);
|
||||
expected.insert(PoseKey(2), x2);
|
||||
expected.insert(PointKey(1), l1);
|
||||
expected.insert(PointKey(2), l2);
|
||||
|
||||
Config actual;
|
||||
actual.insert(1, x1);
|
||||
actual.insert(2, x2);
|
||||
actual.insert(1, l1);
|
||||
actual.insert(2, l2);
|
||||
actual.insert(PoseKey(1), x1);
|
||||
actual.insert(PoseKey(2), x2);
|
||||
actual.insert(PointKey(1), l1);
|
||||
actual.insert(PointKey(2), l2);
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
|
@ -55,15 +55,15 @@ TEST( PairConfig, insert_equals2 )
|
|||
Point2 l1(4,5), l2(9,10);
|
||||
|
||||
Config cfg1;
|
||||
cfg1.insert(1, x1);
|
||||
cfg1.insert(2, x2);
|
||||
cfg1.insert(1, l1);
|
||||
cfg1.insert(2, l2);
|
||||
cfg1.insert(PoseKey(1), x1);
|
||||
cfg1.insert(PoseKey(2), x2);
|
||||
cfg1.insert(PointKey(1), l1);
|
||||
cfg1.insert(PointKey(2), l2);
|
||||
|
||||
Config cfg2;
|
||||
cfg2.insert(1, x1);
|
||||
cfg2.insert(2, x2);
|
||||
cfg2.insert(1, l1);
|
||||
cfg2.insert(PoseKey(1), x1);
|
||||
cfg2.insert(PoseKey(2), x2);
|
||||
cfg2.insert(PointKey(1), l1);
|
||||
|
||||
CHECK(!cfg1.equals(cfg2));
|
||||
|
||||
|
|
@ -98,10 +98,10 @@ TEST( PairConfig, size_dim )
|
|||
Point2 l1(4,5), l2(9,10);
|
||||
|
||||
Config cfg1;
|
||||
cfg1.insert(1, x1);
|
||||
cfg1.insert(2, x2);
|
||||
cfg1.insert(1, l1);
|
||||
cfg1.insert(2, l2);
|
||||
cfg1.insert(PoseKey(1), x1);
|
||||
cfg1.insert(PoseKey(2), x2);
|
||||
cfg1.insert(PointKey(1), l1);
|
||||
cfg1.insert(PointKey(2), l2);
|
||||
|
||||
CHECK(cfg1.size() == 4);
|
||||
CHECK(cfg1.dim() == 10);
|
||||
|
|
@ -114,10 +114,10 @@ TEST(PairConfig, at)
|
|||
Point2 l1(4,5), l2(9,10);
|
||||
|
||||
Config cfg1;
|
||||
cfg1.insert(1, x1);
|
||||
cfg1.insert(2, x2);
|
||||
cfg1.insert(1, l1);
|
||||
cfg1.insert(2, l2);
|
||||
cfg1.insert(PoseKey(1), x1);
|
||||
cfg1.insert(PoseKey(2), x2);
|
||||
cfg1.insert(PointKey(1), l1);
|
||||
cfg1.insert(PointKey(2), l2);
|
||||
|
||||
CHECK(assert_equal(x1, cfg1[PoseKey(1)]));
|
||||
CHECK(assert_equal(x2, cfg1[PoseKey(2)]));
|
||||
|
|
@ -148,10 +148,10 @@ TEST(PairConfig, expmap)
|
|||
Point2 l1(4,5), l2(9,10);
|
||||
|
||||
Config cfg1;
|
||||
cfg1.insert(1, x1);
|
||||
cfg1.insert(2, x2);
|
||||
cfg1.insert(1, l1);
|
||||
cfg1.insert(2, l2);
|
||||
cfg1.insert(PoseKey(1), x1);
|
||||
cfg1.insert(PoseKey(2), x2);
|
||||
cfg1.insert(PointKey(1), l1);
|
||||
cfg1.insert(PointKey(2), l2);
|
||||
|
||||
VectorConfig increment;
|
||||
increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
|
||||
|
|
@ -160,12 +160,13 @@ TEST(PairConfig, expmap)
|
|||
increment.insert("l2", Vector_(2, 1.3, 1.4));
|
||||
|
||||
Config expected;
|
||||
expected.insert(1, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
|
||||
expected.insert(2, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
|
||||
expected.insert(1, Point2(5.0, 6.1));
|
||||
expected.insert(2, Point2(10.3, 11.4));
|
||||
expected.insert(PoseKey(1), expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
|
||||
expected.insert(PoseKey(2), expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
|
||||
expected.insert(PointKey(1), Point2(5.0, 6.1));
|
||||
expected.insert(PointKey(2), Point2(10.3, 11.4));
|
||||
|
||||
CHECK(assert_equal(expected, expmap(cfg1, increment)));
|
||||
Config actual = expmap(cfg1, increment);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -323,18 +324,33 @@ TEST(TupleConfig, typedefs)
|
|||
{
|
||||
TupleConfig2<PoseConfig, PointConfig> cfg1;
|
||||
TupleConfig3<PoseConfig, PointConfig, LamConfig> cfg2;
|
||||
TupleConfig4<PoseConfig, PointConfig, LamConfig, Point3Config> cfg3;
|
||||
TupleConfig5<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config> cfg4;
|
||||
TupleConfig6<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config, Point3Config2> cfg5;
|
||||
// TupleConfig4<PoseConfig, PointConfig, LamConfig, Point3Config> cfg3;
|
||||
// TupleConfig5<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config> cfg4;
|
||||
// TupleConfig6<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config, Point3Config2> cfg5;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TupleConfig, constructor_insert )
|
||||
TEST( TupleConfig, pairconfig_style )
|
||||
{
|
||||
PoseConfig cfg1;
|
||||
PointConfig cfg2;
|
||||
LamConfig cfg3;
|
||||
PoseKey x1(1);
|
||||
PointKey l1(1);
|
||||
LamKey L1(1);
|
||||
Pose2 pose1(1.0, 2.0, 0.3);
|
||||
Point2 point1(2.0, 3.0);
|
||||
Vector lam1 = Vector_(1, 2.3);
|
||||
|
||||
PoseConfig cfg1; cfg1.insert(x1, pose1);
|
||||
PointConfig cfg2; cfg2.insert(l1, point1);
|
||||
LamConfig cfg3; cfg3.insert(L1, lam1);
|
||||
|
||||
// Constructor
|
||||
TupleConfig3<PoseConfig, PointConfig, LamConfig> config(cfg1, cfg2, cfg3);
|
||||
|
||||
// access
|
||||
CHECK(assert_equal(cfg1, config.first()));
|
||||
CHECK(assert_equal(cfg2, config.second()));
|
||||
CHECK(assert_equal(cfg3, config.third()));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue