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

The recursive TupleConfig should be ready for use now.

release/4.3a0
Alex Cunningham 2010-02-04 02:04:45 +00:00
parent 550121013f
commit 4e70f14097
2 changed files with 146 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
cpp/TupleConfig.h
View File

@ -149,8 +149,6 @@ namespace gtsam {
* 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.
*
* STILL UNDER TESTING - DO NOT USE
*/
template<class Config1, class Config2>
class TupleConfig : public Testable<TupleConfig<Config1, Config2> > {
@ -166,7 +164,18 @@ namespace gtsam {
typedef class Config1::Value Value1;
public:
/** default constructor */
TupleConfig() {}
/** Copy constructor */
TupleConfig(const TupleConfig<Config1, Config2>& config) :
first_(config.first_), second_(config.second_) {}
/** Construct from configs */
TupleConfig(const Config1& cfg1, const Config2& cfg2) :
first_(cfg1), second_(cfg2) {}
virtual ~TupleConfig() {}
/** Print */
@ -174,7 +183,7 @@ namespace gtsam {
/** Test for equality in keys and values */
bool equals(const TupleConfig<Config1, Config2>& c, double tol=1e-9) const {
return false;
return first_.equals(c.first_) && second_.equals(c.second_);
}
// insert function that uses the second (recursive) config
@ -208,8 +217,25 @@ namespace gtsam {
// dim function
size_t dim() const { return first_.dim() + second_.dim(); }
// Expmap
TupleConfig<Config1, Config2> expmap(const VectorConfig& delta) const {
return TupleConfig(gtsam::expmap(first_, delta), second_.expmap(delta));
}
/** logmap each element */
VectorConfig logmap(const TupleConfig<Config1, Config2>& cp) const {
VectorConfig ret(gtsam::logmap(first_, cp.first_));
ret.insert(second_.logmap(cp.second_));
return ret;
}
};
/**
* End of a recursive TupleConfig - contains only one config
*
* This should not be used directly
*/
template<class Config>
class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > {
protected:
@ -223,6 +249,13 @@ namespace gtsam {
public:
TupleConfigEnd() {}
TupleConfigEnd(const TupleConfigEnd<Config>& config) :
first_(config.first_) {}
TupleConfigEnd(const Config& cfg) :
first_(cfg) {}
virtual ~TupleConfigEnd() {}
/** Print */
@ -230,7 +263,7 @@ namespace gtsam {
/** Test for equality in keys and values */
bool equals(const TupleConfigEnd<Config>& c, double tol=1e-9) const {
return false;
return first_.equals(c.first_);
}
void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
@ -247,5 +280,35 @@ namespace gtsam {
size_t dim() const { return first_.dim(); }
TupleConfigEnd<Config> expmap(const VectorConfig& delta) const {
return TupleConfigEnd(gtsam::expmap(first_, delta));
}
VectorConfig logmap(const TupleConfigEnd<Config>& cp) const {
VectorConfig ret(gtsam::logmap(first_, cp.first_));
return ret;
}
};
/** Exmap static functions */
template<class Config1, class Config2>
inline TupleConfig<Config1, Config2> expmap(const TupleConfig<Config1, Config2> c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class Config>
inline TupleConfigEnd<Config> expmap(const TupleConfigEnd<Config> c, const VectorConfig& delta) {
return c.expmap(delta);
}
/** logmap static functions */
template<class Config1, class Config2>
inline VectorConfig logmap(const TupleConfig<Config1, Config2> c0, const TupleConfig<Config1, Config2>& cp) {
return c0.logmap(cp);
}
template<class Config>
inline VectorConfig logmap(const TupleConfigEnd<Config> c0, const TupleConfigEnd<Config>& cp) {
return c0.logmap(cp);
}
}

79
cpp/testTupleConfig.cpp
View File

@ -231,6 +231,85 @@ TEST(TupleConfig, basic_functions) {
// dim
CHECK(configA.dim() == 5);
CHECK(configB.dim() == 6);
// erase
configA.erase(x1);
CHECK(!configA.exists(x1));
CHECK(configA.size() == 1);
configA.erase(l1);
CHECK(!configA.exists(l1));
CHECK(configA.size() == 0);
configB.erase(L1);
CHECK(!configB.exists(L1));
CHECK(configB.size() == 2);
}
/* ************************************************************************* */
TEST( TupleConfig, equals )
{
Pose2 x1(1,2,3), x2(6,7,8), x2_alt(5,6,7);
PoseKey x1k(1), x2k(2);
Point2 l1(4,5), l2(9,10);
PointKey l1k(1), l2k(2);
ConfigA cfg1, cfg2, cfg3, cfg4, cfg5;
cfg1.insert(x1k, x1);
cfg1.insert(x2k, x2);
cfg1.insert(l1k, l1);
cfg1.insert(l2k, l2);
cfg2.insert(x1k, x1);
cfg2.insert(x2k, x2);
cfg2.insert(l1k, l1);
cfg2.insert(l2k, l2);
cfg3.insert(x2k, x2);
cfg3.insert(l1k, l1);
cfg4.insert(x1k, x1);
cfg4.insert(x2k, x2_alt);
cfg4.insert(l1k, l1);
cfg4.insert(l2k, l2);
ConfigA cfg6(cfg1);
CHECK(assert_equal(cfg1,cfg2));
CHECK(assert_equal(cfg1,cfg1));
CHECK(!cfg1.equals(cfg3));
CHECK(!cfg1.equals(cfg4));
CHECK(!cfg1.equals(cfg5));
CHECK(assert_equal(cfg1, cfg6));
}
/* ************************************************************************* */
TEST(TupleConfig, expmap)
{
Pose2 x1(1,2,3), x2(6,7,8);
PoseKey x1k(1), x2k(2);
Point2 l1(4,5), l2(9,10);
PointKey l1k(1), l2k(2);
ConfigA cfg1;
cfg1.insert(x1k, x1);
cfg1.insert(x2k, x2);
cfg1.insert(l1k, l1);
cfg1.insert(l2k, l2);
VectorConfig increment;
increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
increment.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
increment.insert("l1", Vector_(2, 1.0, 1.1));
increment.insert("l2", Vector_(2, 1.3, 1.4));
ConfigA expected;
expected.insert(x1k, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(x2k, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(l1k, Point2(5.0, 6.1));
expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, expmap(cfg1, increment)));
CHECK(assert_equal(increment, logmap(cfg1, expected)));
}