Added experimental replacement for TupleConfig using boost.fusion - this should eventually make it possible to make nonlinear factors interchangable regardless of config type.
Alex Cunningham
parent
3fd36bca53
commit
c2a83759cb
|
|
@ -0,0 +1,107 @@
|
|||
/**
|
||||
* @file FusionTupleConfig.h
|
||||
* @brief Experimental tuple config using boost.Fusion
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/fusion/container/set.hpp>
|
||||
#include <boost/fusion/include/make_set.hpp>
|
||||
#include <boost/fusion/include/at_key.hpp>
|
||||
#include <boost/fusion/algorithm/iteration.hpp>
|
||||
#include <boost/fusion/algorithm/query.hpp>
|
||||
|
||||
#include <LieConfig.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* This config uses a real tuple to store types
|
||||
* The template parameter should be a boost fusion structure, like
|
||||
* set<Config1, Config2>
|
||||
*/
|
||||
template<class Configs>
|
||||
class FusionTupleConfig {
|
||||
|
||||
public:
|
||||
/** useful types */
|
||||
typedef Configs BaseTuple;
|
||||
|
||||
protected:
|
||||
/** the underlying tuple storing everything */
|
||||
Configs base_tuple_;
|
||||
|
||||
public:
|
||||
/** create an empty config */
|
||||
FusionTupleConfig() {}
|
||||
|
||||
/** copy constructor */
|
||||
FusionTupleConfig(const FusionTupleConfig<Configs>& other) : base_tuple_(other.base_tuple_) {}
|
||||
|
||||
/** direct initialization of the underlying structure */
|
||||
FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {}
|
||||
|
||||
/** initialization by slicing a larger config */
|
||||
template<class Configs2>
|
||||
FusionTupleConfig(const FusionTupleConfig<Configs2>& other) {
|
||||
// use config subinsert and fusion::foreach
|
||||
}
|
||||
|
||||
virtual ~FusionTupleConfig() {}
|
||||
|
||||
/** insertion */
|
||||
template <class Key, class Value>
|
||||
void insert(const Key& j, const Value& x) {
|
||||
boost::fusion::at_key<LieConfig<Key, Value> >(base_tuple_).insert(j,x);
|
||||
}
|
||||
|
||||
/** retrieve a point */
|
||||
template<class Key>
|
||||
const typename Key::Value_t & at(const Key& j) const {
|
||||
return boost::fusion::at_key<LieConfig<Key, typename Key::Value_t> >(base_tuple_).at(j);
|
||||
}
|
||||
|
||||
/** retrieve a full config */
|
||||
template<class Config>
|
||||
const Config & config() const {
|
||||
return boost::fusion::at_key<Config>(base_tuple_);
|
||||
}
|
||||
|
||||
/** size of the config - sum all sizes from subconfigs */
|
||||
size_t size() const {
|
||||
return boost::fusion::accumulate(base_tuple_, 0, FusionTupleConfig<Configs>::size_helper());
|
||||
}
|
||||
|
||||
/** returns true if the config is empty */
|
||||
bool empty() const {
|
||||
return boost::fusion::all(base_tuple_, FusionTupleConfig<Configs>::empty_helper());
|
||||
}
|
||||
|
||||
private:
|
||||
/** helper structs to make use of fusion algorithms */
|
||||
struct size_helper {
|
||||
typedef size_t result_type;
|
||||
|
||||
template<typename T>
|
||||
size_t operator()(const T& t, const size_t& s) const
|
||||
{
|
||||
return s + t.size();
|
||||
}
|
||||
};
|
||||
|
||||
struct empty_helper
|
||||
{
|
||||
template<typename T>
|
||||
bool operator()(T t) const
|
||||
{
|
||||
return t.empty();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -12,7 +12,8 @@ check_PROGRAMS =
|
|||
#----------------------------------------------------------------------------------------------------
|
||||
|
||||
# Lie Groups
|
||||
headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
|
||||
headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
|
||||
headers += FusionTupleConfig.h
|
||||
check_PROGRAMS += tests/testLieConfig
|
||||
|
||||
# Nonlinear nonlinear
|
||||
|
|
|
|||
|
|
@ -13,6 +13,9 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
|
|||
check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint
|
||||
check_PROGRAMS += testTransformConstraint testLinearApproxFactor
|
||||
|
||||
# experimental
|
||||
check_PROGRAMS += testFusionTupleConfig
|
||||
|
||||
if USE_LDL
|
||||
check_PROGRAMS += testConstraintOptimizer
|
||||
endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,119 @@
|
|||
/**
|
||||
* @file testFusionTupleConfig.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <Pose2.h>
|
||||
#include <Point2.h>
|
||||
#include <Pose3.h>
|
||||
#include <Point3.h>
|
||||
#include "Key.h"
|
||||
#include <LieConfig-inl.h>
|
||||
|
||||
#include <FusionTupleConfig.h>
|
||||
|
||||
using namespace boost;
|
||||
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;
|
||||
typedef LieConfig<PointKey, Point2> PointConfig;
|
||||
|
||||
// some generic poses, points and keys
|
||||
PoseKey x1(1), x2(2);
|
||||
Pose2 x1_val(1.0, 2.0, 0.3), x2_val(2.0, 3.0, 0.4);
|
||||
PointKey l1(1), l2(2);
|
||||
Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0);
|
||||
|
||||
typedef FusionTupleConfig<fusion::set<PointConfig> > TupPointConfig;
|
||||
typedef FusionTupleConfig<fusion::set<PoseConfig> > TupPoseConfig;
|
||||
typedef FusionTupleConfig<fusion::set<PoseConfig, PointConfig> > TupPairConfig;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, basic_config ) {
|
||||
|
||||
// some initialized configs to start with
|
||||
PoseConfig poseConfig1;
|
||||
poseConfig1.insert(x1, x1_val);
|
||||
PointConfig pointConfig1;
|
||||
pointConfig1.insert(l1, l1_val);
|
||||
|
||||
// basic initialization
|
||||
TupPointConfig cfg1A, cfg2A(pointConfig1);
|
||||
EXPECT(cfg1A.size() == 0);
|
||||
EXPECT(cfg2A.size() == 1);
|
||||
EXPECT(cfg1A.empty());
|
||||
EXPECT(!cfg2A.empty());
|
||||
|
||||
TupPoseConfig cfg1B, cfg2B(poseConfig1);
|
||||
EXPECT(cfg1B.size() == 0);
|
||||
EXPECT(cfg2B.size() == 1);
|
||||
EXPECT(cfg1B.empty());
|
||||
EXPECT(!cfg2B.empty());
|
||||
|
||||
TupPairConfig cfg1, cfg2(fusion::make_set(poseConfig1, pointConfig1));
|
||||
EXPECT(cfg1.size() == 0);
|
||||
EXPECT(cfg1.empty());
|
||||
EXPECT(cfg2.size() == 2);
|
||||
EXPECT(!cfg2.empty());
|
||||
|
||||
// insertion
|
||||
cfg1A.insert(l1, l1_val);
|
||||
EXPECT(cfg1A.size() == 1);
|
||||
cfg1A.insert(l2, l2_val);
|
||||
EXPECT(cfg1A.size() == 2);
|
||||
cfg2A.insert(l2, l2_val);
|
||||
EXPECT(cfg2A.size() == 2);
|
||||
|
||||
cfg1B.insert(x1, x1_val);
|
||||
EXPECT(cfg1B.size() == 1);
|
||||
cfg1B.insert(x2, x2_val);
|
||||
EXPECT(cfg1B.size() == 2);
|
||||
cfg2B.insert(x2, x2_val);
|
||||
EXPECT(cfg2B.size() == 2);
|
||||
|
||||
cfg1.insert(x1, x1_val);
|
||||
cfg1.insert(x2, x2_val);
|
||||
cfg1.insert(l1, l1_val);
|
||||
cfg1.insert(l2, l2_val);
|
||||
EXPECT(cfg1.size() == 4);
|
||||
|
||||
// retrieval of elements
|
||||
EXPECT(assert_equal(l1_val, cfg1A.at(l1)));
|
||||
EXPECT(assert_equal(l2_val, cfg1A.at(l2)));
|
||||
|
||||
EXPECT(assert_equal(x1_val, cfg1B.at(x1)));
|
||||
EXPECT(assert_equal(x2_val, cfg1B.at(x2)));
|
||||
|
||||
EXPECT(assert_equal(l1_val, cfg1.at(l1)));
|
||||
EXPECT(assert_equal(l2_val, cfg1.at(l2)));
|
||||
EXPECT(assert_equal(x1_val, cfg1.at(x1)));
|
||||
EXPECT(assert_equal(x2_val, cfg1.at(x2)));
|
||||
|
||||
// config extraction
|
||||
PointConfig expPointConfig;
|
||||
expPointConfig.insert(l1, l1_val);
|
||||
expPointConfig.insert(l2, l2_val);
|
||||
|
||||
PoseConfig expPoseConfig;
|
||||
expPoseConfig.insert(x1, x1_val);
|
||||
expPoseConfig.insert(x2, x2_val);
|
||||
|
||||
EXPECT(assert_equal(expPointConfig, cfg1A.config<PointConfig>()));
|
||||
EXPECT(assert_equal(expPoseConfig, cfg1B.config<PoseConfig>()));
|
||||
|
||||
EXPECT(assert_equal(expPointConfig, cfg1.config<PointConfig>()));
|
||||
EXPECT(assert_equal(expPoseConfig, cfg1.config<PoseConfig>()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue