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

Renamed LieConfig and TupleConfig to use Values

release/4.3a0
Alex Cunningham 2010-10-09 03:09:55 +00:00
parent 7610b1acb7
commit 6002931e12
33 changed files with 421 additions and 424 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
examples/PlanarSLAMSelfContained.cpp
View File

@ -24,16 +24,16 @@
#include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included
typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included
typedef gtsam::LieConfig<PoseKey> PoseConfig; // config type for poses
typedef gtsam::LieConfig<PointKey> PointConfig; // config type for points
typedef gtsam::TupleConfig2<PoseConfig, PointConfig> Config; // main config with two variable classes
typedef gtsam::LieValues<PoseKey> PoseConfig; // config type for poses
typedef gtsam::LieValues<PointKey> PointConfig; // config type for points
typedef gtsam::TupleValues2<PoseConfig, PointConfig> Config; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Config> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Config> Optimizer; // optimization engine for this domain

4
examples/SimpleRotation.cpp
View File

@ -16,7 +16,7 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
@ -31,7 +31,7 @@ using namespace std;
using namespace gtsam;
typedef TypedSymbol<Rot2, 'x'> Key;
typedef LieConfig<Key> Config;
typedef LieValues<Key> Config;
typedef NonlinearFactorGraph<Config> Graph;
typedef Factorization<Graph,Config> Solver;
typedef NonlinearOptimizer<Graph,Config> Optimizer;

5
nonlinear/Key.h
View File

@ -1,5 +1,5 @@
/*
* Key.h
/**
* @file Key.h
*
* Created on: Jan 12, 2010
* @author: Frank Dellaert
@ -11,7 +11,6 @@
#include <list>
#include <iostream>
#include <boost/format.hpp>
//#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#ifdef GTSAM_MAGIC_KEY
#include <boost/lexical_cast.hpp>

56
nonlinear/LieConfig-inl.h → nonlinear/LieValues-inl.h
View File

@ -1,5 +1,5 @@
/**
* @file LieConfig.cpp
* @file LieValues.cpp
* @author Richard Roberts
*/
@ -16,14 +16,14 @@
#include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/LieConfig.h>
#include <gtsam/nonlinear/LieValues.h>
#define INSTANTIATE_LIE_CONFIG(J) \
/*INSTANTIATE_LIE(T);*/ \
/*template LieConfig<J> expmap(const LieConfig<J>&, const VectorConfig&);*/ \
/*template LieConfig<J> expmap(const LieConfig<J>&, const Vector&);*/ \
/*template VectorConfig logmap(const LieConfig<J>&, const LieConfig<J>&);*/ \
template class LieConfig<J>;
/*template LieValues<J> expmap(const LieValues<J>&, const VectorConfig&);*/ \
/*template LieValues<J> expmap(const LieValues<J>&, const Vector&);*/ \
/*template VectorConfig logmap(const LieValues<J>&, const LieValues<J>&);*/ \
template class LieValues<J>;
using namespace std;
@ -31,8 +31,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieConfig<J>::print(const string &s) const {
cout << "LieConfig " << s << ", size " << values_.size() << "\n";
void LieValues<J>::print(const string &s) const {
cout << "LieValues " << s << ", size " << values_.size() << "\n";
BOOST_FOREACH(const KeyValuePair& v, values_) {
gtsam::print(v.second, (string)(v.first));
}
@ -40,7 +40,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
bool LieConfig<J>::equals(const LieConfig<J>& expected, double tol) const {
bool LieValues<J>::equals(const LieValues<J>& expected, double tol) const {
if (values_.size() != expected.values_.size()) return false;
BOOST_FOREACH(const KeyValuePair& v, values_) {
if (!expected.exists(v.first)) return false;
@ -52,7 +52,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
const typename J::Value_t& LieConfig<J>::at(const J& j) const {
const typename J::Value_t& LieValues<J>::at(const J& j) const {
const_iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
else return it->second;
@ -60,7 +60,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
size_t LieConfig<J>::dim() const {
size_t LieValues<J>::dim() const {
size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim();
@ -69,7 +69,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
VectorConfig LieConfig<J>::zero(const Ordering& ordering) const {
VectorConfig LieValues<J>::zero(const Ordering& ordering) const {
VectorConfig z(this->dims(ordering));
z.makeZero();
return z;
@ -77,20 +77,20 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieConfig<J>::insert(const J& name, const typename J::Value_t& val) {
void LieValues<J>::insert(const J& name, const typename J::Value_t& val) {
values_.insert(make_pair(name, val));
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::insert(const LieConfig<J>& cfg) {
void LieValues<J>::insert(const LieValues<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
insert(v.first, v.second);
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::update(const LieConfig<J>& cfg) {
void LieValues<J>::update(const LieValues<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, values_) {
boost::optional<typename J::Value_t> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t;
@ -99,13 +99,13 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieConfig<J>::update(const J& j, const typename J::Value_t& val) {
void LieValues<J>::update(const J& j, const typename J::Value_t& val) {
values_[j] = val;
}
/* ************************************************************************* */
template<class J>
std::list<J> LieConfig<J>::keys() const {
std::list<J> LieValues<J>::keys() const {
std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first);
@ -114,14 +114,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieConfig<J>::erase(const J& j) {
void LieValues<J>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::erase(const J& j, size_t& dim) {
void LieValues<J>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
dim = it->second.dim();
@ -131,8 +131,8 @@ namespace gtsam {
/* ************************************************************************* */
// todo: insert for every element is inefficient
template<class J>
LieConfig<J> LieConfig<J>::expmap(const VectorConfig& delta, const Ordering& ordering) const {
LieConfig<J> newConfig;
LieValues<J> LieValues<J>::expmap(const VectorConfig& delta, const Ordering& ordering) const {
LieValues<J> newConfig;
typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first;
@ -145,7 +145,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
std::vector<size_t> LieConfig<J>::dims(const Ordering& ordering) const {
std::vector<size_t> LieValues<J>::dims(const Ordering& ordering) const {
_ConfigDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
@ -153,7 +153,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
Ordering::shared_ptr LieConfig<J>::orderingArbitrary(varid_t firstVar) const {
Ordering::shared_ptr LieValues<J>::orderingArbitrary(varid_t firstVar) const {
// Generate an initial key ordering in iterator order
_ConfigKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);
@ -163,12 +163,12 @@ namespace gtsam {
// /* ************************************************************************* */
// // todo: insert for every element is inefficient
// template<class J>
// LieConfig<J> LieConfig<J>::expmap(const Vector& delta) const {
// LieValues<J> LieValues<J>::expmap(const Vector& delta) const {
// if(delta.size() != dim()) {
// cout << "LieConfig::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
// cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
// throw invalid_argument("Delta vector length does not match config dimensionality.");
// }
// LieConfig<J> newConfig;
// LieValues<J> newConfig;
// int delta_offset = 0;
// typedef pair<J,typename J::Value_t> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) {
@ -185,7 +185,7 @@ namespace gtsam {
// todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs
template<class J>
inline VectorConfig LieConfig<J>::logmap(const LieConfig<J>& cp, const Ordering& ordering) const {
inline VectorConfig LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering));
logmap(cp, ordering, delta);
return delta;
@ -193,7 +193,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieConfig<J>::logmap(const LieConfig<J>& cp, const Ordering& ordering, VectorConfig& delta) const {
void LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering, VectorConfig& delta) const {
typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first));

32
nonlinear/LieConfig.h → nonlinear/LieValues.h
View File

@ -1,12 +1,12 @@
/**
* @file LieConfig.h
* @file LieValues.h
* @Author: Richard Roberts
*
* @brief A templated config for Lie-group elements
*
* Detailed story:
* A configuration is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A LieConfig is a configuration which can hold variables that
* of variables in a factor graph. A LieValues is a configuration which can hold variables that
* are elements of Lie groups, not just vectors. It then, as a whole, implements a aggregate type
* which is also a Lie group, and hence supports operations dim, expmap, and logmap.
*/
@ -18,8 +18,6 @@
#include <boost/pool/pool_alloc.hpp>
//#include <boost/serialization/map.hpp>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/Ordering.h>
@ -40,7 +38,7 @@ namespace gtsam {
* labels (example: Pose2, Point2, etc)
*/
template<class J>
class LieConfig : public Testable<LieConfig<J> > {
class LieValues : public Testable<LieValues<J> > {
public:
@ -60,18 +58,18 @@ namespace gtsam {
public:
LieConfig() {}
LieConfig(const LieConfig& config) :
LieValues() {}
LieValues(const LieValues& config) :
values_(config.values_) {}
template<class J_alt>
LieConfig(const LieConfig<J_alt>& other) {} // do nothing when initializing with wrong type
virtual ~LieConfig() {}
LieValues(const LieValues<J_alt>& other) {} // do nothing when initializing with wrong type
virtual ~LieValues() {}
/** print */
void print(const std::string &s="") const;
/** Test whether configs are identical in keys and values */
bool equals(const LieConfig& expected, double tol=1e-9) const;
bool equals(const LieValues& expected, double tol=1e-9) const;
/** Retrieve a variable by j, throws std::invalid_argument if not found */
const Value& at(const J& j) const;
@ -108,16 +106,16 @@ namespace gtsam {
// Lie operations
/** Add a delta config to current config and returns a new config */
LieConfig expmap(const VectorConfig& delta, const Ordering& ordering) const;
LieValues expmap(const VectorConfig& delta, const Ordering& ordering) const;
// /** Add a delta vector to current config and returns a new config, uses iterator order */
// LieConfig expmap(const Vector& delta) const;
// LieValues expmap(const Vector& delta) const;
/** Get a delta config about a linearization point c0 (*this) */
VectorConfig logmap(const LieConfig& cp, const Ordering& ordering) const;
VectorConfig logmap(const LieValues& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */
void logmap(const LieConfig& cp, const Ordering& ordering, VectorConfig& delta) const;
void logmap(const LieValues& cp, const Ordering& ordering, VectorConfig& delta) const;
// imperative methods:
@ -125,10 +123,10 @@ namespace gtsam {
void insert(const J& j, const Value& val);
/** Add a set of variables - does note replace existing values */
void insert(const LieConfig& cfg);
void insert(const LieValues& cfg);
/** update the current available values without adding new ones */
void update(const LieConfig& cfg);
void update(const LieValues& cfg);
/** single element change of existing element */
void update(const J& j, const Value& val);
@ -148,7 +146,7 @@ namespace gtsam {
std::list<J> keys() const;
/** Replace all keys and variables */
LieConfig& operator=(const LieConfig& rhs) {
LieValues& operator=(const LieValues& rhs) {
values_ = rhs.values_;
return (*this);
}

4
nonlinear/Makefile.am
View File

@ -16,8 +16,8 @@ check_PROGRAMS =
#----------------------------------------------------------------------------------------------------
# Lie Groups
headers += Key.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
check_PROGRAMS += tests/testLieConfig
headers += Key.h LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h
check_PROGRAMS += tests/testLieValues
# Nonlinear nonlinear
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h

177
nonlinear/TupleConfig-inl.h
View File

@ -1,177 +0,0 @@
/**
* @file TupleConfig-inl.h
* @author Richard Roberts
* @author Manohar Paluri
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/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>;
#define INSTANTIATE_TUPLE_CONFIG3(Config1, Config2, Config3) \
template class TupleConfig3<Config1, Config2, Config3>;
#define INSTANTIATE_TUPLE_CONFIG4(Config1, Config2, Config3, Config4) \
template class TupleConfig4<Config1, Config2, Config3, Config4>;
#define INSTANTIATE_TUPLE_CONFIG5(Config1, Config2, Config3, Config4, Config5) \
template class TupleConfig5<Config1, Config2, Config3, Config4, Config5>;
#define INSTANTIATE_TUPLE_CONFIG6(Config1, Config2, Config3, Config4, Config5, Config6) \
template class TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>;
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>
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>
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) :
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>
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) {}
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>
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) {}
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>
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) {}
/* ************************************************************************* */
/** 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) {}
}

177
nonlinear/TupleValues-inl.h Normal file
View File

@ -0,0 +1,177 @@
/**
* @file TupleValues-inl.h
* @author Richard Roberts
* @author Manohar Paluri
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/TupleValues.h>
// TupleValues instantiations for N = 1-6
#define INSTANTIATE_TUPLE_CONFIG1(Config1) \
template class TupleValues1<Config1>;
#define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \
template class TupleValues2<Config1, Config2>;
#define INSTANTIATE_TUPLE_CONFIG3(Config1, Config2, Config3) \
template class TupleValues3<Config1, Config2, Config3>;
#define INSTANTIATE_TUPLE_CONFIG4(Config1, Config2, Config3, Config4) \
template class TupleValues4<Config1, Config2, Config3, Config4>;
#define INSTANTIATE_TUPLE_CONFIG5(Config1, Config2, Config3, Config4, Config5) \
template class TupleValues5<Config1, Config2, Config3, Config4, Config5>;
#define INSTANTIATE_TUPLE_CONFIG6(Config1, Config2, Config3, Config4, Config5, Config6) \
template class TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>;
namespace gtsam {
/* ************************************************************************* */
/** TupleValuesN Implementations */
/* ************************************************************************* */
/* ************************************************************************* */
/** TupleValues 1 */
template<class Config1>
TupleValues1<Config1>::TupleValues1(const TupleValues1<Config1>& config) :
TupleValuesEnd<Config1> (config) {}
template<class Config1>
TupleValues1<Config1>::TupleValues1(const Config1& cfg1) :
TupleValuesEnd<Config1> (cfg1) {}
template<class Config1>
TupleValues1<Config1>::TupleValues1(const TupleValuesEnd<Config1>& config) :
TupleValuesEnd<Config1>(config) {}
/* ************************************************************************* */
/** TupleValues 2 */
template<class Config1, class Config2>
TupleValues2<Config1, Config2>::TupleValues2(const TupleValues2<Config1, Config2>& config) :
TupleValues<Config1, TupleValuesEnd<Config2> >(config) {}
template<class Config1, class Config2>
TupleValues2<Config1, Config2>::TupleValues2(const Config1& cfg1, const Config2& cfg2) :
TupleValues<Config1, TupleValuesEnd<Config2> >(
cfg1, TupleValuesEnd<Config2>(cfg2)) {}
template<class Config1, class Config2>
TupleValues2<Config1, Config2>::TupleValues2(const TupleValues<Config1, TupleValuesEnd<Config2> >& config) :
TupleValues<Config1, TupleValuesEnd<Config2> >(config) {}
/* ************************************************************************* */
/** TupleValues 3 */
template<class Config1, class Config2, class Config3>
TupleValues3<Config1, Config2, Config3>::TupleValues3(
const TupleValues3<Config1, Config2, Config3>& config) :
TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >(config) {}
template<class Config1, class Config2, class Config3>
TupleValues3<Config1, Config2, Config3>::TupleValues3(
const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) :
TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >(
cfg1, TupleValues<Config2, TupleValuesEnd<Config3> >(
cfg2, TupleValuesEnd<Config3>(cfg3))) {}
template<class Config1, class Config2, class Config3>
TupleValues3<Config1, Config2, Config3>::TupleValues3(
const TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >& config) :
TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >(config) {}
/* ************************************************************************* */
/** TupleValues 4 */
template<class Config1, class Config2, class Config3, class Config4>
TupleValues4<Config1, Config2, Config3, Config4>::TupleValues4(
const TupleValues4<Config1, Config2, Config3, Config4>& config) :
TupleValues<Config1, TupleValues<Config2,
TupleValues<Config3, TupleValuesEnd<Config4> > > >(config) {}
template<class Config1, class Config2, class Config3, class Config4>
TupleValues4<Config1, Config2, Config3, Config4>::TupleValues4(
const Config1& cfg1, const Config2& cfg2,
const Config3& cfg3,const Config4& cfg4) :
TupleValues<Config1, TupleValues<Config2,
TupleValues<Config3, TupleValuesEnd<Config4> > > >(
cfg1, TupleValues<Config2, TupleValues<Config3, TupleValuesEnd<Config4> > >(
cfg2, TupleValues<Config3, TupleValuesEnd<Config4> >(
cfg3, TupleValuesEnd<Config4>(cfg4)))) {}
template<class Config1, class Config2, class Config3, class Config4>
TupleValues4<Config1, Config2, Config3, Config4>::TupleValues4(
const TupleValues<Config1, TupleValues<Config2,
TupleValues<Config3, TupleValuesEnd<Config4> > > >& config) :
TupleValues<Config1, TupleValues<Config2,TupleValues<Config3,
TupleValuesEnd<Config4> > > >(config) {}
/* ************************************************************************* */
/** TupleValues 5 */
template<class Config1, class Config2, class Config3, class Config4, class Config5>
TupleValues5<Config1, Config2, Config3, Config4, Config5>::TupleValues5(
const TupleValues5<Config1, Config2, Config3, Config4, Config5>& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValuesEnd<Config5> > > > >(config) {}
template<class Config1, class Config2, class Config3, class Config4, class Config5>
TupleValues5<Config1, Config2, Config3, Config4, Config5>::TupleValues5(
const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5) :
TupleValues<Config1, TupleValues<Config2,
TupleValues<Config3, TupleValues<Config4,
TupleValuesEnd<Config5> > > > >(
cfg1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValuesEnd<Config5> > > >(
cfg2, TupleValues<Config3, TupleValues<Config4, TupleValuesEnd<Config5> > >(
cfg3, TupleValues<Config4, TupleValuesEnd<Config5> >(
cfg4, TupleValuesEnd<Config5>(cfg5))))) {}
template<class Config1, class Config2, class Config3, class Config4, class Config5>
TupleValues5<Config1, Config2, Config3, Config4, Config5>::TupleValues5(
const TupleValues<Config1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValuesEnd<Config5> > > > >& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValuesEnd<Config5> > > > >(config) {}
/* ************************************************************************* */
/** TupleValues 6 */
template<class Config1, class Config2, class Config3,
class Config4, class Config5, class Config6>
TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleValues6(
const TupleValues6<Config1, Config2, Config3,
Config4, Config5, Config6>& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValues<Config5,
TupleValuesEnd<Config6> > > > > >(config) {}
template<class Config1, class Config2, class Config3,
class Config4, class Config5, class Config6>
TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleValues6(
const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValues<Config5, TupleValuesEnd<Config6> > > > > >(
cfg1, TupleValues<Config2, TupleValues<Config3, TupleValues<Config4,
TupleValues<Config5, TupleValuesEnd<Config6> > > > >(
cfg2, TupleValues<Config3, TupleValues<Config4, TupleValues<Config5,
TupleValuesEnd<Config6> > > >(
cfg3, TupleValues<Config4, TupleValues<Config5,
TupleValuesEnd<Config6> > >(
cfg4, TupleValues<Config5, TupleValuesEnd<Config6> >(
cfg5, TupleValuesEnd<Config6>(cfg6)))))) {}
template<class Config1, class Config2, class Config3,
class Config4, class Config5, class Config6>
TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleValues6(
const TupleValues<Config1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValues<Config5,
TupleValuesEnd<Config6> > > > > >& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3,
TupleValues<Config4, TupleValues<Config5,
TupleValuesEnd<Config6> > > > > >(config) {}
}

146
nonlinear/TupleConfig.h → nonlinear/TupleValues.h
View File

@ -1,11 +1,11 @@
/**
* @file TupleConfig.h
* @file TupleValues.h
* @author Richard Roberts
* @author Manohar Paluri
* @author Alex Cunningham
*/
#include <gtsam/nonlinear/LieConfig.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/linear/VectorConfig.h>
#pragma once
@ -13,35 +13,35 @@
namespace gtsam {
/**
* TupleConfigs are a structure to manage heterogenous LieConfigs, so as to
* TupleValuess are a structure to manage heterogenous LieValuess, so as to
* enable different types of variables/keys to be used simultaneously. We
* do this with recursive templates (instead of blind pointer casting) to
* reduce run-time overhead and keep static type checking. The interface
* mimics that of a single LieConfig.
* mimics that of a single LieValues.
*
* This uses a recursive structure of config pairs to form a lisp-like
* list, with a special case (TupleConfigEnd) that contains only one config
* list, with a special case (TupleValuesEnd) that contains only one config
* at the end. Because this recursion is done at compile time, there is no
* runtime performance hit to using this structure.
*
* For an easy interface, there are TupleConfigN classes, which wrap
* the recursive TupleConfigs together as a single class, so you can have
* mixed-type classes from 2-6 types. Note that a TupleConfig2 is equivalent
* For an easy interface, there are TupleValuesN classes, which wrap
* the recursive TupleValuess together as a single class, so you can have
* mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent
* to the previously used PairConfig.
*
* Design and extension note:
* To implement a recursively templated data structure, note that most operations
* have two versions: one with templates and one without. The templated one allows
* for the arguments to be passed to the next config, while the specialized one
* operates on the "first" config. TupleConfigEnd contains only the specialized version.
* operates on the "first" config. TupleValuesEnd contains only the specialized version.
*/
template<class Config1, class Config2>
class TupleConfig : public Testable<TupleConfig<Config1, Config2> > {
class TupleValues : public Testable<TupleValues<Config1, Config2> > {
protected:
// Data for internal configs
Config1 first_; /// Arbitrary config
Config2 second_; /// A TupleConfig or TupleConfigEnd, which wraps an arbitrary config
Config2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary config
public:
// typedefs for config subtypes
@ -49,14 +49,14 @@ namespace gtsam {
typedef class Config1::Value Value1;
/** default constructor */
TupleConfig() {}
TupleValues() {}
/** Copy constructor */
TupleConfig(const TupleConfig<Config1, Config2>& config) :
TupleValues(const TupleValues<Config1, Config2>& config) :
first_(config.first_), second_(config.second_) {}
/** Construct from configs */
TupleConfig(const Config1& cfg1, const Config2& cfg2) :
TupleValues(const Config1& cfg1, const Config2& cfg2) :
first_(cfg1), second_(cfg2) {}
/** Print */
@ -66,7 +66,7 @@ namespace gtsam {
}
/** Equality with tolerance for keys and values */
bool equals(const TupleConfig<Config1, Config2>& c, double tol=1e-9) const {
bool equals(const TupleValues<Config1, Config2>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
}
@ -89,8 +89,8 @@ namespace gtsam {
* @param config is a full config to add
*/
template<class Cfg1, class Cfg2>
void insert(const TupleConfig<Cfg1, Cfg2>& config) { second_.insert(config); }
void insert(const TupleConfig<Config1, Config2>& config) {
void insert(const TupleValues<Cfg1, Cfg2>& config) { second_.insert(config); }
void insert(const TupleValues<Config1, Config2>& config) {
first_.insert(config.first_);
second_.insert(config.second_);
}
@ -100,8 +100,8 @@ namespace gtsam {
* @param config is a config to add
*/
template<class Cfg1, class Cfg2>
void update(const TupleConfig<Cfg1, Cfg2>& config) { second_.update(config); }
void update(const TupleConfig<Config1, Config2>& config) {
void update(const TupleValues<Cfg1, Cfg2>& config) { second_.update(config); }
void update(const TupleValues<Config1, Config2>& config) {
first_.update(config.first_);
second_.update(config.second_);
}
@ -192,19 +192,19 @@ namespace gtsam {
}
/** Expmap */
TupleConfig<Config1, Config2> expmap(const VectorConfig& delta, const Ordering& ordering) const {
return TupleConfig(first_.expmap(delta, ordering), second_.expmap(delta, ordering));
TupleValues<Config1, Config2> expmap(const VectorConfig& delta, const Ordering& ordering) const {
return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering));
}
/** logmap each element */
VectorConfig logmap(const TupleConfig<Config1, Config2>& cp, const Ordering& ordering) const {
VectorConfig logmap(const TupleValues<Config1, Config2>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering));
logmap(cp, ordering, delta);
return delta;
}
/** logmap each element */
void logmap(const TupleConfig<Config1, Config2>& cp, const Ordering& ordering, VectorConfig& delta) const {
void logmap(const TupleValues<Config1, Config2>& cp, const Ordering& ordering, VectorConfig& delta) const {
first_.logmap(cp.first_, ordering, delta);
second_.logmap(cp.second_, ordering, delta);
}
@ -237,13 +237,13 @@ namespace gtsam {
};
/**
* End of a recursive TupleConfig - contains only one config
* End of a recursive TupleValues - contains only one config
*
* Do not use this class directly - it should only be used as a part
* of a recursive structure
*/
template<class Config>
class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > {
class TupleValuesEnd : public Testable<TupleValuesEnd<Config> > {
protected:
// Data for internal configs
@ -254,28 +254,28 @@ namespace gtsam {
typedef class Config::Key Key1;
typedef class Config::Value Value1;
TupleConfigEnd() {}
TupleValuesEnd() {}
TupleConfigEnd(const TupleConfigEnd<Config>& config) :
TupleValuesEnd(const TupleValuesEnd<Config>& config) :
first_(config.first_) {}
TupleConfigEnd(const Config& cfg) :
TupleValuesEnd(const Config& cfg) :
first_(cfg) {}
void print(const std::string& s = "") const {
first_.print();
}
bool equals(const TupleConfigEnd<Config>& c, double tol=1e-9) const {
bool equals(const TupleValuesEnd<Config>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol);
}
void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}
void insert(const TupleConfigEnd<Config>& config) {first_.insert(config.first_); }
void insert(const TupleValuesEnd<Config>& config) {first_.insert(config.first_); }
void update(const TupleConfigEnd<Config>& config) {first_.update(config.first_); }
void update(const TupleValuesEnd<Config>& config) {first_.update(config.first_); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); }
@ -307,17 +307,17 @@ namespace gtsam {
size_t dim() const { return first_.dim(); }
TupleConfigEnd<Config> expmap(const VectorConfig& delta, const Ordering& ordering) const {
return TupleConfigEnd(first_.expmap(delta, ordering));
TupleValuesEnd<Config> expmap(const VectorConfig& delta, const Ordering& ordering) const {
return TupleValuesEnd(first_.expmap(delta, ordering));
}
VectorConfig logmap(const TupleConfigEnd<Config>& cp, const Ordering& ordering) const {
VectorConfig logmap(const TupleValuesEnd<Config>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering));
logmap(cp, ordering, delta);
return delta;
}
void logmap(const TupleConfigEnd<Config>& cp, const Ordering& ordering, VectorConfig& delta) const {
void logmap(const TupleValuesEnd<Config>& cp, const Ordering& ordering, VectorConfig& delta) const {
first_.logmap(cp.first_, ordering, delta);
}
@ -345,44 +345,44 @@ namespace gtsam {
/**
* 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.
* recursively, as they are TupleValuess, and are primarily a short form of the config
* structure to make use of the TupleValuess easier.
*
* The interface is designed to mimic PairConfig, but for 2-6 config types.
*/
template<class C1>
class TupleConfig1 : public TupleConfigEnd<C1> {
class TupleValues1 : public TupleValuesEnd<C1> {
public:
// typedefs
typedef C1 Config1;
typedef TupleConfigEnd<C1> Base;
typedef TupleConfig1<C1> This;
typedef TupleValuesEnd<C1> Base;
typedef TupleValues1<C1> This;
TupleConfig1() {}
TupleConfig1(const This& config);
TupleConfig1(const Base& config);
TupleConfig1(const Config1& cfg1);
TupleValues1() {}
TupleValues1(const This& config);
TupleValues1(const Base& config);
TupleValues1(const Config1& cfg1);
// access functions
inline const Config1& first() const { return this->config(); }
};
template<class C1, class C2>
class TupleConfig2 : public TupleConfig<C1, TupleConfigEnd<C2> > {
class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
public:
// typedefs
typedef C1 Config1;
typedef C2 Config2;
typedef TupleConfig<C1, TupleConfigEnd<C2> > Base;
typedef TupleConfig2<C1, C2> This;
typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
typedef TupleValues2<C1, C2> This;
TupleConfig2() {}
TupleConfig2(const This& config);
TupleConfig2(const Base& config);
TupleConfig2(const Config1& cfg1, const Config2& cfg2);
TupleValues2() {}
TupleValues2(const This& config);
TupleValues2(const Base& config);
TupleValues2(const Config1& cfg1, const Config2& cfg2);
// access functions
inline const Config1& first() const { return this->config(); }
@ -390,17 +390,17 @@ namespace gtsam {
};
template<class C1, class C2, class C3>
class TupleConfig3 : public TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > > {
class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > {
public:
// typedefs
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
TupleConfig3() {}
TupleConfig3(const TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > >& config);
TupleConfig3(const TupleConfig3<C1, C2, C3>& config);
TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3);
TupleValues3() {}
TupleValues3(const TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > >& config);
TupleValues3(const TupleValues3<C1, C2, C3>& config);
TupleValues3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3);
// access functions
inline const Config1& first() const { return this->config(); }
@ -409,7 +409,7 @@ namespace gtsam {
};
template<class C1, class C2, class C3, class C4>
class TupleConfig4 : public TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > {
class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > {
public:
// typedefs
typedef C1 Config1;
@ -417,13 +417,13 @@ namespace gtsam {
typedef C3 Config3;
typedef C4 Config4;
typedef TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > Base;
typedef TupleConfig4<C1, C2, C3, C4> This;
typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
typedef TupleValues4<C1, C2, C3, C4> This;
TupleConfig4() {}
TupleConfig4(const This& config);
TupleConfig4(const Base& config);
TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4);
TupleValues4() {}
TupleValues4(const This& config);
TupleValues4(const Base& config);
TupleValues4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4);
// access functions
inline const Config1& first() const { return this->config(); }
@ -433,7 +433,7 @@ namespace gtsam {
};
template<class C1, class C2, class C3, class C4, class C5>
class TupleConfig5 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > > {
class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
public:
// typedefs
typedef C1 Config1;
@ -442,10 +442,10 @@ namespace gtsam {
typedef C4 Config4;
typedef C5 Config5;
TupleConfig5() {}
TupleConfig5(const TupleConfig5<C1, C2, C3, C4, C5>& config);
TupleConfig5(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > >& config);
TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
TupleValues5() {}
TupleValues5(const TupleValues5<C1, C2, C3, C4, C5>& config);
TupleValues5(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > >& config);
TupleValues5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5);
// access functions
@ -457,7 +457,7 @@ namespace gtsam {
};
template<class C1, class C2, class C3, class C4, class C5, class C6>
class TupleConfig6 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > > {
class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
public:
// typedefs
typedef C1 Config1;
@ -467,10 +467,10 @@ namespace gtsam {
typedef C5 Config5;
typedef C6 Config6;
TupleConfig6() {}
TupleConfig6(const TupleConfig6<C1, C2, C3, C4, C5, C6>& config);
TupleConfig6(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > >& config);
TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
TupleValues6() {}
TupleValues6(const TupleValues6<C1, C2, C3, C4, C5, C6>& config);
TupleValues6(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > >& config);
TupleValues6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6);
// access functions
inline const Config1& first() const { return this->config(); }

40
nonlinear/tests/testLieConfig.cpp → nonlinear/tests/testLieValues.cpp
View File

@ -1,5 +1,5 @@
/**
* @file testLieConfig.cpp
* @file testLieValues.cpp
* @author Richard Roberts
*/
@ -9,7 +9,7 @@
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/base/LieVector.h>
using namespace gtsam;
@ -17,12 +17,12 @@ using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef LieConfig<VecKey> Config;
typedef LieValues<VecKey> Config;
VecKey key1(1), key2(2), key3(3), key4(4);
/* ************************************************************************* */
TEST( LieConfig, equals1 )
TEST( LieValues, equals1 )
{
Config expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
@ -33,7 +33,7 @@ TEST( LieConfig, equals1 )
}
/* ************************************************************************* */
TEST( LieConfig, equals2 )
TEST( LieValues, equals2 )
{
Config cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
@ -45,7 +45,7 @@ TEST( LieConfig, equals2 )
}
/* ************************************************************************* */
TEST( LieConfig, equals_nan )
TEST( LieValues, equals_nan )
{
Config cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
@ -57,7 +57,7 @@ TEST( LieConfig, equals_nan )
}
/* ************************************************************************* */
TEST( LieConfig, insert_config )
TEST( LieValues, insert_config )
{
Config cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
@ -80,7 +80,7 @@ TEST( LieConfig, insert_config )
}
/* ************************************************************************* */
TEST( LieConfig, update_element )
TEST( LieValues, update_element )
{
Config cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
@ -96,7 +96,7 @@ TEST( LieConfig, update_element )
}
///* ************************************************************************* */
//TEST(LieConfig, dim_zero)
//TEST(LieValues, dim_zero)
//{
// Config config0;
// config0.insert(key1, Vector_(2, 2.0, 3.0));
@ -110,7 +110,7 @@ TEST( LieConfig, update_element )
//}
/* ************************************************************************* */
TEST(LieConfig, expmap_a)
TEST(LieValues, expmap_a)
{
Config config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
@ -129,7 +129,7 @@ TEST(LieConfig, expmap_a)
}
///* ************************************************************************* */
//TEST(LieConfig, expmap_b)
//TEST(LieValues, expmap_b)
//{
// Config config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
@ -147,7 +147,7 @@ TEST(LieConfig, expmap_a)
//}
///* ************************************************************************* */
//TEST(LieConfig, expmap_c)
//TEST(LieValues, expmap_c)
//{
// Config config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
@ -165,7 +165,7 @@ TEST(LieConfig, expmap_a)
//}
/* ************************************************************************* */
/*TEST(LieConfig, expmap_d)
/*TEST(LieValues, expmap_d)
{
Config config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
@ -174,7 +174,7 @@ TEST(LieConfig, expmap_a)
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
LieConfig<string,Pose2> poseconfig;
LieValues<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig");
@ -183,10 +183,10 @@ TEST(LieConfig, expmap_a)
}*/
/* ************************************************************************* */
/*TEST(LieConfig, extract_keys)
/*TEST(LieValues, extract_keys)
{
typedef TypedSymbol<Pose2, 'x'> PoseKey;
LieConfig<PoseKey, Pose2> config;
LieValues<PoseKey, Pose2> config;
config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2());
@ -205,7 +205,7 @@ TEST(LieConfig, expmap_a)
}*/
/* ************************************************************************* */
TEST(LieConfig, exists_)
TEST(LieValues, exists_)
{
Config config0;
config0.insert(key1, Vector_(1, 1.));
@ -216,7 +216,7 @@ TEST(LieConfig, exists_)
}
/* ************************************************************************* */
TEST(LieConfig, update)
TEST(LieValues, update)
{
Config config0;
config0.insert(key1, Vector_(1, 1.));
@ -235,10 +235,10 @@ TEST(LieConfig, update)
}
/* ************************************************************************* */
TEST(LieConfig, dummy_initialization)
TEST(LieValues, dummy_initialization)
{
typedef TypedSymbol<LieVector, 'z'> Key;
typedef LieConfig<Key> Config1;
typedef LieValues<Key> Config1;
Config1 init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0));

2
slam/PriorFactor.h
View File

@ -16,7 +16,7 @@ namespace gtsam {
* It takes three template parameters:
* T is the Lie group type for which the prior is define
* Key (typically TypedSymbol) is used to look up T's in a Config
* Config where the T's are stored, typically LieConfig<Key,T> or a TupleConfig<...>
* Config where the T's are stored, typically LieValues<Key,T> or a TupleValues<...>
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work
*/

4
slam/Simulated3D.cpp
View File

@ -5,8 +5,8 @@
**/
#include <gtsam/slam/Simulated3D.h>
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {

8
slam/Simulated3D.h
View File

@ -13,7 +13,7 @@
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/TupleConfig.h>
#include <gtsam/nonlinear/TupleValues.h>
// \namespace
@ -23,9 +23,9 @@ namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<PoseConfig, PointConfig> Config;
/**
* Prior on a single pose

2
slam/planarSLAM.cpp
View File

@ -7,7 +7,7 @@
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
// Use planarSLAM namespace for specific SLAM instance
namespace gtsam {

8
slam/planarSLAM.h
View File

@ -7,7 +7,7 @@
#pragma once
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/TupleConfig.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
@ -23,9 +23,9 @@ namespace gtsam {
// Keys and Config
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<PoseConfig, PointConfig> Config;
// Factors
typedef NonlinearEquality<Config, PoseKey> Constraint;

2
slam/pose2SLAM.cpp
View File

@ -5,7 +5,7 @@
**/
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>

4
slam/pose2SLAM.h
View File

@ -8,7 +8,7 @@
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LieConfig.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -23,7 +23,7 @@ namespace gtsam {
// Keys and Config
typedef TypedSymbol<Pose2, 'x'> Key;
typedef LieConfig<Key> Config;
typedef LieValues<Key> Config;
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)

2
slam/pose3SLAM.cpp
View File

@ -5,7 +5,7 @@
**/
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>

4
slam/pose3SLAM.h
View File

@ -7,7 +7,7 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/LieConfig.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -22,7 +22,7 @@ namespace gtsam {
// Keys and Config
typedef TypedSymbol<Pose3, 'x'> Key;
typedef LieConfig<Key> Config;
typedef LieValues<Key> Config;
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)

2
slam/saveGraph.cpp
View File

@ -11,7 +11,7 @@
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/SymbolicBayesNet.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
using namespace std;

4
slam/saveGraph.h
View File

@ -12,12 +12,12 @@
#include <gtsam/inference/SymbolicBayesNet.h>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/LieConfig.h>
#include <gtsam/nonlinear/LieValues.h>
namespace gtsam {
class Point2;
typedef LieConfig<Symbol, Point2> SymbolicConfig;
typedef LieValues<Symbol, Point2> SymbolicConfig;
// save graph to the graphviz format
void saveGraph(const SymbolicFactorGraph& fg, const SymbolicConfig& config, const std::string& s);

4
slam/simulated2D.cpp
View File

@ -5,8 +5,8 @@
*/
#include <gtsam/slam/simulated2D.h>
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {

8
slam/simulated2D.h
View File

@ -9,7 +9,7 @@
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/TupleConfig.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
// \namespace
@ -21,9 +21,9 @@ namespace gtsam {
// Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<PoseConfig, PointConfig> Config;
/**
* Prior on a single pose, and optional derivative version

2
slam/simulated2DOriented.cpp
View File

@ -5,7 +5,7 @@
*/
#include <gtsam/slam/simulated2DOriented.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {

8
slam/simulated2DOriented.h
View File

@ -9,7 +9,7 @@
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/TupleConfig.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
// \namespace
@ -21,9 +21,9 @@ namespace gtsam {
// The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<PoseConfig, PointConfig> Config;
//TODO:: point prior is not implemented right now

2
slam/smallExample.cpp
View File

@ -22,7 +22,7 @@ using namespace std;
// template definitions
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
namespace gtsam {

2
slam/visualSLAM.cpp
View File

@ -6,7 +6,7 @@
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>

8
slam/visualSLAM.h
View File

@ -14,7 +14,7 @@
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/TupleConfig.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
namespace gtsam { namespace visualSLAM {
@ -24,9 +24,9 @@ namespace gtsam { namespace visualSLAM {
*/
typedef TypedSymbol<Pose3,'x'> PoseKey;
typedef TypedSymbol<Point3,'l'> PointKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<PoseConfig, PointConfig> Config;
typedef boost::shared_ptr<Config> shared_config;
typedef NonlinearEquality<Config, PoseKey> PoseConstraint;

2
tests/Makefile.am
View File

@ -18,7 +18,7 @@ check_PROGRAMS += testGaussianJunctionTree
check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
check_PROGRAMS += testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
check_PROGRAMS += testTupleConfig
check_PROGRAMS += testTupleValues
#check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint
#check_PROGRAMS += testTransformConstraint testLinearApproxFactor

2
tests/testGraph.cpp
View File

@ -17,7 +17,7 @@ using namespace boost::assign;
#define GTSAM_MAGIC_GAUSSIAN 3
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>

4
tests/testNonlinearEquality.cpp
View File

@ -11,13 +11,13 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
using namespace std;
using namespace gtsam;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieValues<PoseKey> PoseConfig;
typedef PriorFactor<PoseConfig, PoseKey> PosePrior;
typedef NonlinearEquality<PoseConfig, PoseKey> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;

12
tests/testTransformConstraint.cpp
View File

@ -18,8 +18,8 @@
#include <gtsam/nonlinear/NonlinearEquality.h>
// implementations
#include <gtsam/nonlinear/LieConfig-inl.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
@ -31,11 +31,11 @@ typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'T'> TransformKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef LieConfig<TransformKey> TransformConfig;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef LieValues<TransformKey> TransformConfig;
typedef TupleConfig3< PoseConfig, PointConfig, TransformConfig > DDFConfig;
typedef TupleValues3< PoseConfig, PointConfig, TransformConfig > DDFConfig;
typedef NonlinearFactorGraph<DDFConfig> DDFGraph;
typedef NonlinearOptimizer<DDFGraph, DDFConfig> Optimizer;

100
tests/testTupleConfig.cpp → tests/testTupleValues.cpp
View File

@ -1,5 +1,5 @@
/**
* @file testTupleConfig.cpp
* @file testTupleValues.cpp
* @author Richard Roberts
* @author Alex Cunningham
*/
@ -18,7 +18,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
using namespace gtsam;
using namespace std;
@ -27,12 +27,12 @@ static const double tol = 1e-5;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<PoseConfig, PointConfig> Config;
/* ************************************************************************* */
TEST( TupleConfig, constructors )
TEST( TupleValues, constructors )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -54,7 +54,7 @@ TEST( TupleConfig, constructors )
}
/* ************************************************************************* */
TEST( TupleConfig, insert_equals1 )
TEST( TupleValues, insert_equals1 )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -75,7 +75,7 @@ TEST( TupleConfig, insert_equals1 )
}
/* ************************************************************************* */
TEST( TupleConfig, insert_equals2 )
TEST( TupleValues, insert_equals2 )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -99,7 +99,7 @@ TEST( TupleConfig, insert_equals2 )
}
/* ************************************************************************* */
TEST( TupleConfig, insert_duplicate )
TEST( TupleValues, insert_duplicate )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -117,7 +117,7 @@ TEST( TupleConfig, insert_duplicate )
}
/* ************************************************************************* */
TEST( TupleConfig, size_dim )
TEST( TupleValues, size_dim )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -133,7 +133,7 @@ TEST( TupleConfig, size_dim )
}
/* ************************************************************************* */
TEST(TupleConfig, at)
TEST(TupleValues, at)
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -154,7 +154,7 @@ TEST(TupleConfig, at)
}
/* ************************************************************************* */
TEST(TupleConfig, zero_expmap_logmap)
TEST(TupleValues, zero_expmap_logmap)
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
@ -204,23 +204,23 @@ typedef TypedSymbol<Point3, 'b'> Point3Key;
typedef TypedSymbol<Point3, 'c'> Point3Key2;
// some config types
typedef LieConfig<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig;
typedef LieConfig<LamKey> LamConfig;
typedef LieConfig<Pose3Key> Pose3Config;
typedef LieConfig<Point3Key> Point3Config;
typedef LieConfig<Point3Key2> Point3Config2;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef LieValues<LamKey> LamConfig;
typedef LieValues<Pose3Key> Pose3Config;
typedef LieValues<Point3Key> Point3Config;
typedef LieValues<Point3Key2> Point3Config2;
// some TupleConfig types
typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA;
typedef TupleConfig<PoseConfig, TupleConfig<PointConfig, TupleConfigEnd<LamConfig> > > ConfigB;
// some TupleValues types
typedef TupleValues<PoseConfig, TupleValuesEnd<PointConfig> > ConfigA;
typedef TupleValues<PoseConfig, TupleValues<PointConfig, TupleValuesEnd<LamConfig> > > ConfigB;
typedef TupleConfig1<PoseConfig> TuplePoseConfig;
typedef TupleConfig1<PointConfig> TuplePointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> SimpleConfig;
typedef TupleValues1<PoseConfig> TuplePoseConfig;
typedef TupleValues1<PointConfig> TuplePointConfig;
typedef TupleValues2<PoseConfig, PointConfig> SimpleConfig;
/* ************************************************************************* */
TEST(TupleConfig, slicing) {
TEST(TupleValues, slicing) {
PointKey l1(1), l2(2);
Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0);
PoseKey x1(1), x2(2);
@ -234,14 +234,14 @@ TEST(TupleConfig, slicing) {
liePointConfig.insert(l1, l1_val);
liePointConfig.insert(l2, l2_val);
// construct TupleConfig1 from the base config
// construct TupleValues1 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
// // construct a TupleValues2 from a TupleValues1
// SimpleConfig pairConfig1(tupPoseConfig1);
// EXPECT(assert_equal(liePoseConfig, pairConfig1.first(), tol));
// EXPECT(pairConfig1.second().empty());
@ -253,7 +253,7 @@ TEST(TupleConfig, slicing) {
}
/* ************************************************************************* */
TEST(TupleConfig, basic_functions) {
TEST(TupleValues, basic_functions) {
// create some tuple configs
ConfigA configA;
ConfigB configB;
@ -325,7 +325,7 @@ TEST(TupleConfig, basic_functions) {
}
/* ************************************************************************* */
TEST(TupleConfig, insert_config) {
TEST(TupleValues, insert_config) {
ConfigB config1, config2, expected;
PoseKey x1(1), x2(2);
@ -356,9 +356,9 @@ TEST(TupleConfig, insert_config) {
}
/* ************************************************************************* */
TEST( TupleConfig, update_element )
TEST( TupleValues, update_element )
{
TupleConfig2<PoseConfig, PointConfig> cfg;
TupleValues2<PoseConfig, PointConfig> cfg;
Pose2 x1(2.0, 1.0, 2.0), x2(3.0, 4.0, 5.0);
Point2 l1(1.0, 2.0), l2(3.0, 4.0);
PoseKey xk(1);
@ -382,7 +382,7 @@ TEST( TupleConfig, update_element )
}
/* ************************************************************************* */
TEST( TupleConfig, equals )
TEST( TupleValues, equals )
{
Pose2 x1(1,2,3), x2(6,7,8), x2_alt(5,6,7);
PoseKey x1k(1), x2k(2);
@ -420,7 +420,7 @@ TEST( TupleConfig, equals )
}
/* ************************************************************************* */
TEST(TupleConfig, expmap)
TEST(TupleValues, expmap)
{
Pose2 x1(1,2,3), x2(6,7,8);
PoseKey x1k(1), x2k(2);
@ -452,7 +452,7 @@ TEST(TupleConfig, expmap)
}
/* ************************************************************************* */
TEST(TupleConfig, expmap_typedefs)
TEST(TupleValues, expmap_typedefs)
{
Pose2 x1(1,2,3), x2(6,7,8);
PoseKey x1k(1), x2k(2);
@ -461,7 +461,7 @@ TEST(TupleConfig, expmap_typedefs)
Ordering o; o += "x1", "x2", "l1", "l2";
TupleConfig2<PoseConfig, PointConfig> config1, expected, actual;
TupleValues2<PoseConfig, PointConfig> config1, expected, actual;
config1.insert(x1k, x1);
config1.insert(x2k, x2);
config1.insert(l1k, l1);
@ -478,22 +478,22 @@ TEST(TupleConfig, expmap_typedefs)
expected.insert(l1k, Point2(5.0, 6.1));
expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, TupleConfig2<PoseConfig, PointConfig>(config1.expmap(delta, o))));
CHECK(assert_equal(expected, TupleValues2<PoseConfig, PointConfig>(config1.expmap(delta, o))));
//CHECK(assert_equal(delta, config1.logmap(expected)));
}
/* ************************************************************************* */
TEST(TupleConfig, typedefs)
TEST(TupleValues, typedefs)
{
TupleConfig2<PoseConfig, PointConfig> config1;
TupleConfig3<PoseConfig, PointConfig, LamConfig> config2;
TupleConfig4<PoseConfig, PointConfig, LamConfig, Point3Config> config3;
TupleConfig5<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config> config4;
TupleConfig6<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config, Point3Config2> config5;
TupleValues2<PoseConfig, PointConfig> config1;
TupleValues3<PoseConfig, PointConfig, LamConfig> config2;
TupleValues4<PoseConfig, PointConfig, LamConfig, Point3Config> config3;
TupleValues5<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config> config4;
TupleValues6<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config, Point3Config2> config5;
}
/* ************************************************************************* */
TEST( TupleConfig, pairconfig_style )
TEST( TupleValues, pairconfig_style )
{
PoseKey x1(1);
PointKey l1(1);
@ -507,7 +507,7 @@ TEST( TupleConfig, pairconfig_style )
LamConfig config3; config3.insert(L1, lam1);
// Constructor
TupleConfig3<PoseConfig, PointConfig, LamConfig> config(config1, config2, config3);
TupleValues3<PoseConfig, PointConfig, LamConfig> config(config1, config2, config3);
// access
CHECK(assert_equal(config1, config.first()));
@ -516,9 +516,9 @@ TEST( TupleConfig, pairconfig_style )
}
/* ************************************************************************* */
TEST(TupleConfig, insert_config_typedef) {
TEST(TupleValues, insert_config_typedef) {
TupleConfig4<PoseConfig, PointConfig, LamConfig, Point3Config> config1, config2, expected;
TupleValues4<PoseConfig, PointConfig, LamConfig, Point3Config> config1, config2, expected;
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
@ -548,8 +548,8 @@ TEST(TupleConfig, insert_config_typedef) {
}
/* ************************************************************************* */
TEST(TupleConfig, partial_insert) {
TupleConfig3<PoseConfig, PointConfig, LamConfig> init, expected;
TEST(TupleValues, partial_insert) {
TupleValues3<PoseConfig, PointConfig, LamConfig> init, expected;
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
@ -576,8 +576,8 @@ TEST(TupleConfig, partial_insert) {
}
/* ************************************************************************* */
TEST(TupleConfig, update) {
TupleConfig3<PoseConfig, PointConfig, LamConfig> init, superset, expected;
TEST(TupleValues, update) {
TupleValues3<PoseConfig, PointConfig, LamConfig> init, superset, expected;
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);