Added TupleConfig1 wrapper

release/4.3a0
Alex Cunningham 2010-08-12 12:44:36 +00:00
parent 6aff189008
commit 3fd36bca53
3 changed files with 89 additions and 5 deletions

View File

@ -11,6 +11,9 @@
#include "TupleConfig.h"
// TupleConfig instantiations for N = 1-6
#define INSTANTIATE_TUPLE_CONFIG1(Config1) \
template class TupleConfig1<Config1>;
#define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \
template class TupleConfig2<Config1, Config2>;
@ -33,6 +36,20 @@ namespace gtsam {
/** TupleConfigN Implementations */
/* ************************************************************************* */
/* ************************************************************************* */
/** TupleConfig 1 */
template<class Config1>
TupleConfig1<Config1>::TupleConfig1(const TupleConfig1<Config1>& config) :
TupleConfigEnd<Config1> (config) {}
template<class Config1>
TupleConfig1<Config1>::TupleConfig1(const Config1& cfg1) :
TupleConfigEnd<Config1> (cfg1) {}
template<class Config1>
TupleConfig1<Config1>::TupleConfig1(const TupleConfigEnd<Config1>& config) :
TupleConfigEnd<Config1>(config) {}
/* ************************************************************************* */
/** TupleConfig 2 */
template<class Config1, class Config2>

View File

@ -302,6 +302,35 @@ namespace gtsam {
*
* The interface is designed to mimic PairConfig, but for 2-6 config types.
*/
template<class C1>
class TupleConfig1 : public TupleConfigEnd<C1> {
public:
// typedefs
typedef C1 Config1;
typedef TupleConfigEnd<C1> Base;
typedef TupleConfig1<C1> This;
TupleConfig1() {}
TupleConfig1(const This& config);
TupleConfig1(const Base& config);
TupleConfig1(const Config1& cfg1);
// access functions
inline const Config1& first() const { return this->config(); }
};
template<class C1>
TupleConfig1<C1> expmap(const TupleConfig1<C1>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class C1>
VectorConfig logmap(const TupleConfig1<C1>& c1, const TupleConfig1<C1>& c2) {
return c1.logmap(c2);
}
template<class C1, class C2>
class TupleConfig2 : public TupleConfig<C1, TupleConfigEnd<C2> > {
public:

View File

@ -1,8 +1,7 @@
/*
* testTupleConfig.cpp
*
* Created on: Jan 13, 2010
* Author: richard
/**
* @file testTupleConfig.cpp
* @author Richard Roberts
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
@ -23,6 +22,8 @@
using namespace gtsam;
using namespace std;
static const double tol = 1e-5;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey, Pose2> PoseConfig;
@ -211,6 +212,43 @@ typedef LieConfig<Point3Key2, Point3> Point3Config2;
typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA;
typedef TupleConfig<PoseConfig, TupleConfig<PointConfig, TupleConfigEnd<LamConfig> > > ConfigB;
typedef TupleConfig1<PoseConfig> TuplePoseConfig;
typedef TupleConfig1<PointConfig> TuplePointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> SimpleConfig;
/* ************************************************************************* */
TEST(TupleConfig, slicing) {
PointKey l1(1), l2(2);
Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0);
PoseKey x1(1), x2(2);
Pose2 x1_val(1.0, 2.0, 0.3), x2_val(3.0, 4.0, 0.4);
PoseConfig liePoseConfig;
liePoseConfig.insert(x1, x1_val);
liePoseConfig.insert(x2, x2_val);
PointConfig liePointConfig;
liePointConfig.insert(l1, l1_val);
liePointConfig.insert(l2, l2_val);
// construct TupleConfig1 from the base config
TuplePoseConfig tupPoseConfig1(liePoseConfig);
EXPECT(assert_equal(liePoseConfig, tupPoseConfig1.first(), tol));
TuplePointConfig tupPointConfig1(liePointConfig);
EXPECT(assert_equal(liePointConfig, tupPointConfig1.first(), tol));
// // construct a TupleConfig2 from a TupleConfig1
// SimpleConfig pairConfig1(tupPoseConfig1);
// EXPECT(assert_equal(liePoseConfig, pairConfig1.first(), tol));
// EXPECT(pairConfig1.second().empty());
//
// SimpleConfig pairConfig2(tupPointConfig1);
// EXPECT(assert_equal(liePointConfig, pairConfig2.second(), tol));
// EXPECT(pairConfig1.first().empty());
}
/* ************************************************************************* */
TEST(TupleConfig, basic_functions) {
// create some tuple configs