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

Added more constructors, print functionality and formatting for TupleConfig

release/4.3a0
Alex Cunningham 2010-02-06 05:10:25 +00:00
parent bb74b5c882
commit d1267d1ef3
3 changed files with 137 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
cpp/TupleConfig-inl.h
View File

@ -7,8 +7,8 @@
#pragma once
#include <iostream>
#include "LieConfig-inl.h"
#include "TupleConfig.h"
#define INSTANTIATE_PAIR_CONFIG(J1,X1,J2,X2) \
@ -20,6 +20,7 @@
namespace gtsam {
/* ************************************************************************* */
/** PairConfig implementations */
/* ************************************************************************* */
template<class J1, class X1, class J2, class X2>
@ -40,8 +41,28 @@ void PairConfig<J1,X1,J2,X2>::insert(const PairConfig& config) {
}
}
/* ************************************************************************* */
/** 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 */
/* ************************************************************************* */
/* ************************************************************************* */
/** TupleConfig 2 */
template<class Config1, class Config2>
TupleConfig2<Config1, Config2>::TupleConfig2(const TupleConfig2<Config1, Config2>& config) :
TupleConfig<Config1, TupleConfigEnd<Config2> >(config) {}
@ -51,52 +72,117 @@ TupleConfig2<Config1, Config2>::TupleConfig2(const Config1& cfg1, const Config2&
TupleConfig<Config1, TupleConfigEnd<Config2> >(
cfg1, TupleConfigEnd<Config2>(cfg2)) {}
template<class Config1, class Config2>
TupleConfig2<Config1, Config2>::TupleConfig2(const TupleConfig<Config1, TupleConfigEnd<Config2> >& config) :
TupleConfig<Config1, TupleConfigEnd<Config2> >(config) {}
/* ************************************************************************* */
/** TupleConfig 3 */
template<class Config1, class Config2, class Config3>
TupleConfig3<Config1, Config2, Config3>::TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config) :
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) :
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>
TupleConfig3<Config1, Config2, Config3>::TupleConfig3(
const TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >& config) :
TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(config) {}
/* ************************************************************************* */
/** TupleConfig 4 */
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) {}
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> > > >(
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>
TupleConfig4<Config1, Config2, Config3, Config4>::TupleConfig4(
const TupleConfig<Config1, TupleConfig<Config2,
TupleConfig<Config3, TupleConfigEnd<Config4> > > >& config) :
TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3,
TupleConfigEnd<Config4> > > >(config) {}
/* ************************************************************************* */
/** TupleConfig 5 */
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) {}
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,
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> > > >(
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>
TupleConfig5<Config1, Config2, Config3, Config4, Config5>::TupleConfig5(
const TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3,
TupleConfig<Config4, TupleConfigEnd<Config5> > > > >& config) :
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3,
TupleConfig<Config4, TupleConfigEnd<Config5> > > > >(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)))))) {}
/* ************************************************************************* */
/** TupleConfig 6 */
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)))))) {}
template<class Config1, class Config2, class Config3,
class Config4, class Config5, class Config6>
TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleConfig6(
const TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3,
TupleConfig<Config4, TupleConfig<Config5,
TupleConfigEnd<Config6> > > > > >& config) :
TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3,
TupleConfig<Config4, TupleConfig<Config5,
TupleConfigEnd<Config6> > > > > >(config) {}
}

14
cpp/TupleConfig.h
View File

@ -47,11 +47,11 @@ namespace gtsam {
virtual ~TupleConfig() {}
/** Print */
void print(const std::string& s = "") const {}
void print(const std::string& s = "") const;
/** Test for equality in keys and values */
bool equals(const TupleConfig<Config1, Config2>& c, double tol=1e-9) const {
return first_.equals(c.first_) && second_.equals(c.second_);
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
}
// insert function that uses the second (recursive) config
@ -131,11 +131,11 @@ namespace gtsam {
virtual ~TupleConfigEnd() {}
/** Print */
void print(const std::string& s = "") const {}
void print(const std::string& s = "") const;
/** Test for equality in keys and values */
bool equals(const TupleConfigEnd<Config>& c, double tol=1e-9) const {
return first_.equals(c.first_);
return first_.equals(c.first_, tol);
}
void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
@ -202,6 +202,7 @@ namespace gtsam {
TupleConfig2() {}
TupleConfig2(const TupleConfig2<Config1, Config2>& config);
TupleConfig2(const TupleConfig<Config1, TupleConfigEnd<Config2> >& config);
TupleConfig2(const Config1& cfg1, const Config2& cfg2);
// access functions
@ -218,8 +219,10 @@ namespace gtsam {
typedef Config3 Config3_t;
TupleConfig3() {}
TupleConfig3(const TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >& config);
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(); }
@ -237,6 +240,7 @@ namespace gtsam {
TupleConfig4() {}
TupleConfig4(const TupleConfig4<Config1, Config2, Config3, Config4>& config);
TupleConfig4(const TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3, TupleConfigEnd<Config4> > > >& config);
TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4);
// access functions
@ -258,6 +262,7 @@ namespace gtsam {
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 Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5);
@ -282,6 +287,7 @@ namespace gtsam {
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 Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6);
// access functions

21
cpp/testTupleConfig.cpp
View File

@ -324,9 +324,9 @@ 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;
}
/* ************************************************************************* */
@ -350,9 +350,22 @@ TEST( TupleConfig, pairconfig_style )
CHECK(assert_equal(cfg1, config.first()));
CHECK(assert_equal(cfg2, config.second()));
CHECK(assert_equal(cfg3, config.third()));
}
/* ************************************************************************* */
#include "NonlinearFactorGraph-inl.h"
TEST( TupleConfig, graphs_and_factors )
{
typedef TupleConfig3<PoseConfig, PointConfig, LamConfig> ConfigC;
typedef NonlinearFactorGraph<ConfigC> GraphC;
typedef NonlinearFactor1<ConfigC, PoseKey, Pose2> FactorC;
// test creation
GraphC graph;
ConfigC config;
// FactorC::shared_ptr f1(new FactorC());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */