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> #include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last // 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/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// Main typedefs // Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included 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::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included
typedef gtsam::LieConfig<PoseKey> PoseConfig; // config type for poses typedef gtsam::LieValues<PoseKey> PoseConfig; // config type for poses
typedef gtsam::LieConfig<PointKey> PointConfig; // config type for points typedef gtsam::LieValues<PointKey> PointConfig; // config type for points
typedef gtsam::TupleConfig2<PoseConfig, PointConfig> Config; // main config with two variable classes typedef gtsam::TupleValues2<PoseConfig, PointConfig> Config; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Config> Graph; // graph structure typedef gtsam::NonlinearFactorGraph<Config> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Config> Optimizer; // optimization engine for this domain 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/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.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/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
@ -31,7 +31,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef TypedSymbol<Rot2, 'x'> Key; typedef TypedSymbol<Rot2, 'x'> Key;
typedef LieConfig<Key> Config; typedef LieValues<Key> Config;
typedef NonlinearFactorGraph<Config> Graph; typedef NonlinearFactorGraph<Config> Graph;
typedef Factorization<Graph,Config> Solver; typedef Factorization<Graph,Config> Solver;
typedef NonlinearOptimizer<Graph,Config> Optimizer; 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 * Created on: Jan 12, 2010
* @author: Frank Dellaert * @author: Frank Dellaert
@ -11,7 +11,6 @@
#include <list> #include <list>
#include <iostream> #include <iostream>
#include <boost/format.hpp> #include <boost/format.hpp>
//#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#ifdef GTSAM_MAGIC_KEY #ifdef GTSAM_MAGIC_KEY
#include <boost/lexical_cast.hpp> #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 * @author Richard Roberts
*/ */
@ -16,14 +16,14 @@
#include <gtsam/base/Lie-inl.h> #include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/LieConfig.h> #include <gtsam/nonlinear/LieValues.h>
#define INSTANTIATE_LIE_CONFIG(J) \ #define INSTANTIATE_LIE_CONFIG(J) \
/*INSTANTIATE_LIE(T);*/ \ /*INSTANTIATE_LIE(T);*/ \
/*template LieConfig<J> expmap(const LieConfig<J>&, const VectorConfig&);*/ \ /*template LieValues<J> expmap(const LieValues<J>&, const VectorConfig&);*/ \
/*template LieConfig<J> expmap(const LieConfig<J>&, const Vector&);*/ \ /*template LieValues<J> expmap(const LieValues<J>&, const Vector&);*/ \
/*template VectorConfig logmap(const LieConfig<J>&, const LieConfig<J>&);*/ \ /*template VectorConfig logmap(const LieValues<J>&, const LieValues<J>&);*/ \
template class LieConfig<J>; template class LieValues<J>;
using namespace std; using namespace std;
@ -31,8 +31,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieConfig<J>::print(const string &s) const { void LieValues<J>::print(const string &s) const {
cout << "LieConfig " << s << ", size " << values_.size() << "\n"; cout << "LieValues " << s << ", size " << values_.size() << "\n";
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
gtsam::print(v.second, (string)(v.first)); gtsam::print(v.second, (string)(v.first));
} }
@ -40,7 +40,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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; if (values_.size() != expected.values_.size()) return false;
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
if (!expected.exists(v.first)) return false; if (!expected.exists(v.first)) return false;
@ -52,7 +52,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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); const_iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
else return it->second; else return it->second;
@ -60,7 +60,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
size_t LieConfig<J>::dim() const { size_t LieValues<J>::dim() const {
size_t n = 0; size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_) BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim(); n += value.second.dim();
@ -69,7 +69,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
VectorConfig LieConfig<J>::zero(const Ordering& ordering) const { VectorConfig LieValues<J>::zero(const Ordering& ordering) const {
VectorConfig z(this->dims(ordering)); VectorConfig z(this->dims(ordering));
z.makeZero(); z.makeZero();
return z; return z;
@ -77,20 +77,20 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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)); values_.insert(make_pair(name, val));
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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_) BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
insert(v.first, v.second); insert(v.first, v.second);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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_FOREACH(const KeyValuePair& v, values_) {
boost::optional<typename J::Value_t> t = cfg.exists_(v.first); boost::optional<typename J::Value_t> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t; if (t) values_[v.first] = *t;
@ -99,13 +99,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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; values_[j] = val;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
std::list<J> LieConfig<J>::keys() const { std::list<J> LieValues<J>::keys() const {
std::list<J> ret; std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_) BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first); ret.push_back(v.first);
@ -114,14 +114,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieConfig<J>::erase(const J& j) { void LieValues<J>::erase(const J& j) {
size_t dim; // unused size_t dim; // unused
erase(j, dim); erase(j, dim);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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); iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j); if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
dim = it->second.dim(); dim = it->second.dim();
@ -131,8 +131,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
template<class J> template<class J>
LieConfig<J> LieConfig<J>::expmap(const VectorConfig& delta, const Ordering& ordering) const { LieValues<J> LieValues<J>::expmap(const VectorConfig& delta, const Ordering& ordering) const {
LieConfig<J> newConfig; LieValues<J> newConfig;
typedef pair<J,typename J::Value_t> KeyValue; typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) { BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first; const J& j = value.first;
@ -145,7 +145,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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); _ConfigDimensionCollector dimCollector(ordering);
this->apply(dimCollector); this->apply(dimCollector);
return dimCollector.dimensions; return dimCollector.dimensions;
@ -153,7 +153,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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 // Generate an initial key ordering in iterator order
_ConfigKeyOrderer keyOrderer(firstVar); _ConfigKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer); this->apply(keyOrderer);
@ -163,12 +163,12 @@ namespace gtsam {
// /* ************************************************************************* */ // /* ************************************************************************* */
// // todo: insert for every element is inefficient // // todo: insert for every element is inefficient
// template<class J> // 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()) { // 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."); // throw invalid_argument("Delta vector length does not match config dimensionality.");
// } // }
// LieConfig<J> newConfig; // LieValues<J> newConfig;
// int delta_offset = 0; // int delta_offset = 0;
// typedef pair<J,typename J::Value_t> KeyValue; // typedef pair<J,typename J::Value_t> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) { // BOOST_FOREACH(const KeyValue& value, this->values_) {
@ -185,7 +185,7 @@ namespace gtsam {
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs // todo: currently only logmaps elements in both configs
template<class J> 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)); VectorConfig delta(this->dims(ordering));
logmap(cp, ordering, delta); logmap(cp, ordering, delta);
return delta; return delta;
@ -193,7 +193,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> 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; typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first)); 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 * @Author: Richard Roberts
* *
* @brief A templated config for Lie-group elements * @brief A templated config for Lie-group elements
* *
* Detailed story: * Detailed story:
* A configuration is a map from keys to values. It is used to specify the value of a bunch * 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 * 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. * 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/pool/pool_alloc.hpp>
//#include <boost/serialization/map.hpp>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
@ -40,7 +38,7 @@ namespace gtsam {
* labels (example: Pose2, Point2, etc) * labels (example: Pose2, Point2, etc)
*/ */
template<class J> template<class J>
class LieConfig : public Testable<LieConfig<J> > { class LieValues : public Testable<LieValues<J> > {
public: public:
@ -60,18 +58,18 @@ namespace gtsam {
public: public:
LieConfig() {} LieValues() {}
LieConfig(const LieConfig& config) : LieValues(const LieValues& config) :
values_(config.values_) {} values_(config.values_) {}
template<class J_alt> template<class J_alt>
LieConfig(const LieConfig<J_alt>& other) {} // do nothing when initializing with wrong type LieValues(const LieValues<J_alt>& other) {} // do nothing when initializing with wrong type
virtual ~LieConfig() {} virtual ~LieValues() {}
/** print */ /** print */
void print(const std::string &s="") const; void print(const std::string &s="") const;
/** Test whether configs are identical in keys and values */ /** 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 */ /** Retrieve a variable by j, throws std::invalid_argument if not found */
const Value& at(const J& j) const; const Value& at(const J& j) const;
@ -108,16 +106,16 @@ namespace gtsam {
// Lie operations // Lie operations
/** Add a delta config to current config and returns a new config */ /** 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 */ // /** 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) */ /** 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) */ /** 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: // imperative methods:
@ -125,10 +123,10 @@ namespace gtsam {
void insert(const J& j, const Value& val); void insert(const J& j, const Value& val);
/** Add a set of variables - does note replace existing values */ /** 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 */ /** 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 */ /** single element change of existing element */
void update(const J& j, const Value& val); void update(const J& j, const Value& val);
@ -148,7 +146,7 @@ namespace gtsam {
std::list<J> keys() const; std::list<J> keys() const;
/** Replace all keys and variables */ /** Replace all keys and variables */
LieConfig& operator=(const LieConfig& rhs) { LieValues& operator=(const LieValues& rhs) {
values_ = rhs.values_; values_ = rhs.values_;
return (*this); return (*this);
} }

4
nonlinear/Makefile.am
View File

@ -16,8 +16,8 @@ check_PROGRAMS =
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# Lie Groups # Lie Groups
headers += Key.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h headers += Key.h LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h
check_PROGRAMS += tests/testLieConfig check_PROGRAMS += tests/testLieValues
# Nonlinear nonlinear # Nonlinear nonlinear
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h 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 Richard Roberts
* @author Manohar Paluri * @author Manohar Paluri
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <gtsam/nonlinear/LieConfig.h> #include <gtsam/nonlinear/LieValues.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorConfig.h>
#pragma once #pragma once
@ -13,35 +13,35 @@
namespace gtsam { 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 * enable different types of variables/keys to be used simultaneously. We
* do this with recursive templates (instead of blind pointer casting) to * do this with recursive templates (instead of blind pointer casting) to
* reduce run-time overhead and keep static type checking. The interface * 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 * 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 * at the end. Because this recursion is done at compile time, there is no
* runtime performance hit to using this structure. * runtime performance hit to using this structure.
* *
* For an easy interface, there are TupleConfigN classes, which wrap * For an easy interface, there are TupleValuesN classes, which wrap
* the recursive TupleConfigs together as a single class, so you can have * the recursive TupleValuess together as a single class, so you can have
* mixed-type classes from 2-6 types. Note that a TupleConfig2 is equivalent * mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent
* to the previously used PairConfig. * to the previously used PairConfig.
* *
* Design and extension note: * Design and extension note:
* To implement a recursively templated data structure, note that most operations * To implement a recursively templated data structure, note that most operations
* have two versions: one with templates and one without. The templated one allows * 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 * 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> template<class Config1, class Config2>
class TupleConfig : public Testable<TupleConfig<Config1, Config2> > { class TupleValues : public Testable<TupleValues<Config1, Config2> > {
protected: protected:
// Data for internal configs // Data for internal configs
Config1 first_; /// Arbitrary config 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: public:
// typedefs for config subtypes // typedefs for config subtypes
@ -49,14 +49,14 @@ namespace gtsam {
typedef class Config1::Value Value1; typedef class Config1::Value Value1;
/** default constructor */ /** default constructor */
TupleConfig() {} TupleValues() {}
/** Copy constructor */ /** Copy constructor */
TupleConfig(const TupleConfig<Config1, Config2>& config) : TupleValues(const TupleValues<Config1, Config2>& config) :
first_(config.first_), second_(config.second_) {} first_(config.first_), second_(config.second_) {}
/** Construct from configs */ /** Construct from configs */
TupleConfig(const Config1& cfg1, const Config2& cfg2) : TupleValues(const Config1& cfg1, const Config2& cfg2) :
first_(cfg1), second_(cfg2) {} first_(cfg1), second_(cfg2) {}
/** Print */ /** Print */
@ -66,7 +66,7 @@ namespace gtsam {
} }
/** Equality with tolerance for keys and values */ /** 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); 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 * @param config is a full config to add
*/ */
template<class Cfg1, class Cfg2> template<class Cfg1, class Cfg2>
void insert(const TupleConfig<Cfg1, Cfg2>& config) { second_.insert(config); } void insert(const TupleValues<Cfg1, Cfg2>& config) { second_.insert(config); }
void insert(const TupleConfig<Config1, Config2>& config) { void insert(const TupleValues<Config1, Config2>& config) {
first_.insert(config.first_); first_.insert(config.first_);
second_.insert(config.second_); second_.insert(config.second_);
} }
@ -100,8 +100,8 @@ namespace gtsam {
* @param config is a config to add * @param config is a config to add
*/ */
template<class Cfg1, class Cfg2> template<class Cfg1, class Cfg2>
void update(const TupleConfig<Cfg1, Cfg2>& config) { second_.update(config); } void update(const TupleValues<Cfg1, Cfg2>& config) { second_.update(config); }
void update(const TupleConfig<Config1, Config2>& config) { void update(const TupleValues<Config1, Config2>& config) {
first_.update(config.first_); first_.update(config.first_);
second_.update(config.second_); second_.update(config.second_);
} }
@ -192,19 +192,19 @@ namespace gtsam {
} }
/** Expmap */ /** Expmap */
TupleConfig<Config1, Config2> expmap(const VectorConfig& delta, const Ordering& ordering) const { TupleValues<Config1, Config2> expmap(const VectorConfig& delta, const Ordering& ordering) const {
return TupleConfig(first_.expmap(delta, ordering), second_.expmap(delta, ordering)); return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering));
} }
/** logmap each element */ /** 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)); VectorConfig delta(this->dims(ordering));
logmap(cp, ordering, delta); logmap(cp, ordering, delta);
return delta; return delta;
} }
/** logmap each element */ /** 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); first_.logmap(cp.first_, ordering, delta);
second_.logmap(cp.second_, 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 * Do not use this class directly - it should only be used as a part
* of a recursive structure * of a recursive structure
*/ */
template<class Config> template<class Config>
class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > { class TupleValuesEnd : public Testable<TupleValuesEnd<Config> > {
protected: protected:
// Data for internal configs // Data for internal configs
@ -254,28 +254,28 @@ namespace gtsam {
typedef class Config::Key Key1; typedef class Config::Key Key1;
typedef class Config::Value Value1; typedef class Config::Value Value1;
TupleConfigEnd() {} TupleValuesEnd() {}
TupleConfigEnd(const TupleConfigEnd<Config>& config) : TupleValuesEnd(const TupleValuesEnd<Config>& config) :
first_(config.first_) {} first_(config.first_) {}
TupleConfigEnd(const Config& cfg) : TupleValuesEnd(const Config& cfg) :
first_(cfg) {} first_(cfg) {}
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
first_.print(); 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); return first_.equals(c.first_, tol);
} }
void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } 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(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); } 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(); } size_t dim() const { return first_.dim(); }
TupleConfigEnd<Config> expmap(const VectorConfig& delta, const Ordering& ordering) const { TupleValuesEnd<Config> expmap(const VectorConfig& delta, const Ordering& ordering) const {
return TupleConfigEnd(first_.expmap(delta, ordering)); 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)); VectorConfig delta(this->dims(ordering));
logmap(cp, ordering, delta); logmap(cp, ordering, delta);
return 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); 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 * 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 * recursively, as they are TupleValuess, and are primarily a short form of the config
* structure to make use of the TupleConfigs easier. * structure to make use of the TupleValuess easier.
* *
* The interface is designed to mimic PairConfig, but for 2-6 config types. * The interface is designed to mimic PairConfig, but for 2-6 config types.
*/ */
template<class C1> template<class C1>
class TupleConfig1 : public TupleConfigEnd<C1> { class TupleValues1 : public TupleValuesEnd<C1> {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Config1;
typedef TupleConfigEnd<C1> Base; typedef TupleValuesEnd<C1> Base;
typedef TupleConfig1<C1> This; typedef TupleValues1<C1> This;
TupleConfig1() {} TupleValues1() {}
TupleConfig1(const This& config); TupleValues1(const This& config);
TupleConfig1(const Base& config); TupleValues1(const Base& config);
TupleConfig1(const Config1& cfg1); TupleValues1(const Config1& cfg1);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Config1& first() const { return this->config(); }
}; };
template<class C1, class C2> template<class C1, class C2>
class TupleConfig2 : public TupleConfig<C1, TupleConfigEnd<C2> > { class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Config1;
typedef C2 Config2; typedef C2 Config2;
typedef TupleConfig<C1, TupleConfigEnd<C2> > Base; typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
typedef TupleConfig2<C1, C2> This; typedef TupleValues2<C1, C2> This;
TupleConfig2() {} TupleValues2() {}
TupleConfig2(const This& config); TupleValues2(const This& config);
TupleConfig2(const Base& config); TupleValues2(const Base& config);
TupleConfig2(const Config1& cfg1, const Config2& cfg2); TupleValues2(const Config1& cfg1, const Config2& cfg2);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Config1& first() const { return this->config(); }
@ -390,17 +390,17 @@ namespace gtsam {
}; };
template<class C1, class C2, class C3> 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: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Config1;
typedef C2 Config2; typedef C2 Config2;
typedef C3 Config3; typedef C3 Config3;
TupleConfig3() {} TupleValues3() {}
TupleConfig3(const TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > >& config); TupleValues3(const TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > >& config);
TupleConfig3(const TupleConfig3<C1, C2, C3>& config); TupleValues3(const TupleValues3<C1, C2, C3>& config);
TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3); TupleValues3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Config1& first() const { return this->config(); }
@ -409,7 +409,7 @@ namespace gtsam {
}; };
template<class C1, class C2, class C3, class C4> 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: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Config1;
@ -417,13 +417,13 @@ namespace gtsam {
typedef C3 Config3; typedef C3 Config3;
typedef C4 Config4; typedef C4 Config4;
typedef TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > Base; typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
typedef TupleConfig4<C1, C2, C3, C4> This; typedef TupleValues4<C1, C2, C3, C4> This;
TupleConfig4() {} TupleValues4() {}
TupleConfig4(const This& config); TupleValues4(const This& config);
TupleConfig4(const Base& config); TupleValues4(const Base& config);
TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4); TupleValues4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4);
// access functions // access functions
inline const Config1& first() const { return this->config(); } 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> 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: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Config1;
@ -442,10 +442,10 @@ namespace gtsam {
typedef C4 Config4; typedef C4 Config4;
typedef C5 Config5; typedef C5 Config5;
TupleConfig5() {} TupleValues5() {}
TupleConfig5(const TupleConfig5<C1, C2, C3, C4, C5>& config); TupleValues5(const TupleValues5<C1, C2, C3, C4, C5>& config);
TupleConfig5(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > >& config); TupleValues5(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > >& config);
TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, TupleValues5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5); const Config4& cfg4, const Config5& cfg5);
// access functions // access functions
@ -457,7 +457,7 @@ namespace gtsam {
}; };
template<class C1, class C2, class C3, class C4, class C5, class C6> 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: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Config1;
@ -467,10 +467,10 @@ namespace gtsam {
typedef C5 Config5; typedef C5 Config5;
typedef C6 Config6; typedef C6 Config6;
TupleConfig6() {} TupleValues6() {}
TupleConfig6(const TupleConfig6<C1, C2, C3, C4, C5, C6>& config); TupleValues6(const TupleValues6<C1, C2, C3, C4, C5, C6>& config);
TupleConfig6(const TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > >& config); TupleValues6(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > >& config);
TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, TupleValues6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6); const Config4& cfg4, const Config5& cfg5, const Config6& cfg6);
// access functions // access functions
inline const Config1& first() const { return this->config(); } 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 * @author Richard Roberts
*/ */
@ -9,7 +9,7 @@
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
#include <gtsam/nonlinear/LieConfig-inl.h> #include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
using namespace gtsam; using namespace gtsam;
@ -17,12 +17,12 @@ using namespace std;
static double inf = std::numeric_limits<double>::infinity(); static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey; typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef LieConfig<VecKey> Config; typedef LieValues<VecKey> Config;
VecKey key1(1), key2(2), key3(3), key4(4); VecKey key1(1), key2(2), key3(3), key4(4);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieConfig, equals1 ) TEST( LieValues, equals1 )
{ {
Config expected; Config expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0); 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; Config cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); 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; Config cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); 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; Config cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); 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; Config cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); 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; // Config config0;
// config0.insert(key1, Vector_(2, 2.0, 3.0)); // 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; Config config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); 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; // Config config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // 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; // Config config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // 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; Config config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
@ -174,7 +174,7 @@ TEST(LieConfig, expmap_a)
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
LieConfig<string,Pose2> poseconfig; LieValues<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3)); poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig"); //poseconfig.print("poseconfig");
@ -183,10 +183,10 @@ TEST(LieConfig, expmap_a)
}*/ }*/
/* ************************************************************************* */ /* ************************************************************************* */
/*TEST(LieConfig, extract_keys) /*TEST(LieValues, extract_keys)
{ {
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
LieConfig<PoseKey, Pose2> config; LieValues<PoseKey, Pose2> config;
config.insert(PoseKey(1), Pose2()); config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2()); config.insert(PoseKey(2), Pose2());
@ -205,7 +205,7 @@ TEST(LieConfig, expmap_a)
}*/ }*/
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, exists_) TEST(LieValues, exists_)
{ {
Config config0; Config config0;
config0.insert(key1, Vector_(1, 1.)); config0.insert(key1, Vector_(1, 1.));
@ -216,7 +216,7 @@ TEST(LieConfig, exists_)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieConfig, update) TEST(LieValues, update)
{ {
Config config0; Config config0;
config0.insert(key1, Vector_(1, 1.)); 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 TypedSymbol<LieVector, 'z'> Key;
typedef LieConfig<Key> Config1; typedef LieValues<Key> Config1;
Config1 init1; Config1 init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0)); 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: * It takes three template parameters:
* T is the Lie group type for which the prior is define * 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 * 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 * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work * 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/slam/Simulated3D.h>
#include <gtsam/nonlinear/LieConfig-inl.h> #include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/TupleConfig-inl.h> #include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam { namespace gtsam {

8
slam/Simulated3D.h
View File

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

2
slam/planarSLAM.cpp
View File

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

8
slam/planarSLAM.h
View File

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

2
slam/pose2SLAM.cpp
View File

@ -5,7 +5,7 @@
**/ **/
#include <gtsam/slam/pose2SLAM.h> #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/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-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/base/LieVector.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LieConfig.h> #include <gtsam/nonlinear/LieValues.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
@ -23,7 +23,7 @@ namespace gtsam {
// Keys and Config // Keys and Config
typedef TypedSymbol<Pose2, 'x'> Key; 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) * 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/slam/pose3SLAM.h>
#include <gtsam/nonlinear/LieConfig-inl.h> #include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>

4
slam/pose3SLAM.h
View File

@ -7,7 +7,7 @@
#pragma once #pragma once
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/LieConfig.h> #include <gtsam/nonlinear/LieValues.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
@ -22,7 +22,7 @@ namespace gtsam {
// Keys and Config // Keys and Config
typedef TypedSymbol<Pose3, 'x'> Key; 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) * 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/SymbolicFactorGraph.h>
#include <gtsam/inference/SymbolicBayesNet.h> #include <gtsam/inference/SymbolicBayesNet.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
#include <gtsam/nonlinear/LieConfig-inl.h> #include <gtsam/nonlinear/LieValues-inl.h>
using namespace std; using namespace std;

4
slam/saveGraph.h
View File

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

8
slam/simulated2D.h
View File

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

8
slam/simulated2DOriented.h
View File

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

2
slam/smallExample.cpp
View File

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

2
slam/visualSLAM.cpp
View File

@ -6,7 +6,7 @@
*/ */
#include <gtsam/slam/visualSLAM.h> #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/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-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/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/TupleConfig.h> #include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
namespace gtsam { namespace visualSLAM { namespace gtsam { namespace visualSLAM {
@ -24,9 +24,9 @@ namespace gtsam { namespace visualSLAM {
*/ */
typedef TypedSymbol<Pose3,'x'> PoseKey; typedef TypedSymbol<Pose3,'x'> PoseKey;
typedef TypedSymbol<Point3,'l'> PointKey; typedef TypedSymbol<Point3,'l'> PointKey;
typedef LieConfig<PoseKey> PoseConfig; typedef LieValues<PoseKey> PoseConfig;
typedef LieConfig<PointKey> PointConfig; typedef LieValues<PointKey> PointConfig;
typedef TupleConfig2<PoseConfig, PointConfig> Config; typedef TupleValues2<PoseConfig, PointConfig> Config;
typedef boost::shared_ptr<Config> shared_config; typedef boost::shared_ptr<Config> shared_config;
typedef NonlinearEquality<Config, PoseKey> PoseConstraint; 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 += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
check_PROGRAMS += testNonlinearOptimizer check_PROGRAMS += testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
check_PROGRAMS += testTupleConfig check_PROGRAMS += testTupleValues
#check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint #check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint
#check_PROGRAMS += testTransformConstraint testLinearApproxFactor #check_PROGRAMS += testTransformConstraint testLinearApproxFactor

2
tests/testGraph.cpp
View File

@ -17,7 +17,7 @@ using namespace boost::assign;
#define GTSAM_MAGIC_GAUSSIAN 3 #define GTSAM_MAGIC_GAUSSIAN 3
#include <gtsam/slam/pose2SLAM.h> #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/graph-inl.h>
#include <gtsam/inference/FactorGraph-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/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/LieConfig-inl.h> #include <gtsam/nonlinear/LieValues-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieConfig<PoseKey> PoseConfig; typedef LieValues<PoseKey> PoseConfig;
typedef PriorFactor<PoseConfig, PoseKey> PosePrior; typedef PriorFactor<PoseConfig, PoseKey> PosePrior;
typedef NonlinearEquality<PoseConfig, PoseKey> PoseNLE; typedef NonlinearEquality<PoseConfig, PoseKey> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE; typedef boost::shared_ptr<PoseNLE> shared_poseNLE;

12
tests/testTransformConstraint.cpp
View File

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