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

Switched Values, Ordering, and factors to int 'Key' instead of 'Symbol', 

still more code changes required to compile
release/4.3a0
Richard Roberts 2012-02-19 01:02:07 +00:00
parent c1468c8b40
commit 0592b71ac4
44 changed files with 233 additions and 254 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/types.h
View File

@ -26,6 +26,9 @@ namespace gtsam {
/// Integer variable index type
typedef size_t Index;
/// Integer nonlinear key type
typedef size_t Key;
/**
* Helper class that uses templates to select between two types based on
* whether TEST_TYPE is const or not.

28
gtsam/inference/Conditional.h
View File

@ -46,7 +46,7 @@ private:
/** Create keys by adding key in front */
template<typename ITERATOR>
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
std::vector<Key> keys((lastParent - firstParent) + 1);
std::vector<KeyType> keys((lastParent - firstParent) + 1);
std::copy(firstParent, lastParent, keys.begin() + 1);
keys[0] = key;
return keys;
@ -62,16 +62,16 @@ protected:
public:
typedef KEY Key;
typedef Conditional<Key> This;
typedef Factor<Key> Base;
typedef KEY KeyType;
typedef Conditional<KeyType> This;
typedef Factor<KeyType> Base;
/**
* Typedef to the factor type that produces this conditional and that this
* conditional can be converted to using a factor constructor. Derived
* classes must redefine this.
*/
typedef gtsam::Factor<Key> FactorType;
typedef gtsam::Factor<KeyType> FactorType;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr<This> shared_ptr;
@ -95,23 +95,23 @@ public:
Conditional() : nrFrontals_(0) { assertInvariants(); }
/** No parents */
Conditional(Key key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
/** Single parent */
Conditional(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
/** Two parents */
Conditional(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent1, KeyType parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
/** Three parents */
Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
Conditional(Key key, const std::vector<Key>& parents) :
Conditional(KeyType key, const std::vector<KeyType>& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
assertInvariants();
}
@ -145,8 +145,8 @@ public:
size_t nrParents() const { return FactorType::size() - nrFrontals_; }
/** Special accessor when there is only one frontal variable. */
Key firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
Key lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
/** return a view of the frontal keys */
Frontals frontals() const {
@ -198,9 +198,9 @@ private:
template<typename KEY>
void Conditional<KEY>::print(const std::string& s) const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals()) std::cout << " " << key;
BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << key;
if (nrParents()>0) std::cout << " |";
BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl;
}

4
gtsam/inference/Factor-inl.h
View File

@ -45,8 +45,8 @@ namespace gtsam {
void Factor<KEY>::assertInvariants() const {
#ifndef NDEBUG
// Check that keys are all unique
std::multiset < Key > nonunique(keys_.begin(), keys_.end());
std::set < Key > unique(keys_.begin(), keys_.end());
std::multiset < KeyType > nonunique(keys_.begin(), keys_.end());
std::set < KeyType > unique(keys_.begin(), keys_.end());
bool correct = (nonunique.size() == unique.size())
&& std::equal(nonunique.begin(), nonunique.end(), unique.begin());
if (!correct) throw std::logic_error(

36
gtsam/inference/Factor.h
View File

@ -55,28 +55,28 @@ class Factor {
public:
typedef KEY Key; ///< The KEY template parameter
typedef Factor<Key> This; ///< This class
typedef KEY KeyType; ///< The KEY template parameter
typedef Factor<KeyType> This; ///< This class
/**
* Typedef to the conditional type obtained by eliminating this factor,
* derived classes must redefine this.
*/
typedef Conditional<Key> ConditionalType;
typedef Conditional<KeyType> ConditionalType;
/// A shared_ptr to this class, derived classes must redefine this.
typedef boost::shared_ptr<Factor> shared_ptr;
/// Iterator over keys
typedef typename std::vector<Key>::iterator iterator;
typedef typename std::vector<KeyType>::iterator iterator;
/// Const iterator over keys
typedef typename std::vector<Key>::const_iterator const_iterator;
typedef typename std::vector<KeyType>::const_iterator const_iterator;
protected:
/// The keys involved in this factor
std::vector<Key> keys_;
std::vector<KeyType> keys_;
friend class JacobianFactor;
friend class HessianFactor;
@ -102,19 +102,19 @@ public:
Factor() {}
/** Construct unary factor */
Factor(Key key) : keys_(1) {
Factor(KeyType key) : keys_(1) {
keys_[0] = key; assertInvariants(); }
/** Construct binary factor */
Factor(Key key1, Key key2) : keys_(2) {
Factor(KeyType key1, KeyType key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */
Factor(Key key1, Key key2, Key key3) : keys_(3) {
Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
/** Construct 4-way factor */
Factor(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/// @}
@ -122,13 +122,13 @@ public:
/// @{
/** Construct n-way factor */
Factor(const std::set<Key>& keys) {
BOOST_FOREACH(const Key& key, keys) keys_.push_back(key);
Factor(const std::set<KeyType>& keys) {
BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants();
}
/** Construct n-way factor */
Factor(const std::vector<Key>& keys) : keys_(keys) {
Factor(const std::vector<KeyType>& keys) : keys_(keys) {
assertInvariants();
}
@ -157,16 +157,16 @@ public:
/// @{
/// First key
Key front() const { return keys_.front(); }
KeyType front() const { return keys_.front(); }
/// Last key
Key back() const { return keys_.back(); }
KeyType back() const { return keys_.back(); }
/// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
///TODO: comment
const std::vector<Key>& keys() const { return keys_; }
const std::vector<KeyType>& keys() const { return keys_; }
/** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
@ -194,7 +194,7 @@ public:
/**
* @return keys involved in this factor
*/
std::vector<Key>& keys() { return keys_; }
std::vector<KeyType>& keys() { return keys_; }
/** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment

10
gtsam/inference/IndexConditional.cpp
View File

@ -43,11 +43,11 @@ namespace gtsam {
/* ************************************************************************* */
bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(Key& parent, parents()) {
Key newParent = inversePermutation[parent];
BOOST_FOREACH(KeyType& parent, parents()) {
KeyType newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
@ -61,8 +61,8 @@ namespace gtsam {
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) {
BOOST_FOREACH(const KeyType frontal, this->frontals()) {
BOOST_FOREACH(const KeyType separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}

24
gtsam/inference/tests/timeSymbolMaps.cpp
View File

@ -37,11 +37,11 @@ private:
Map values_;
public:
typedef pair<Symbol, T> value_type;
typedef pair<Key, T> value_type;
SymbolMapExp() {}
T& at(const Symbol& key) {
T& at(Key key) {
typename Map::iterator it = values_.find(key.chr());
if(it != values_.end())
return it->second.at(key.index());
@ -49,7 +49,7 @@ public:
throw invalid_argument("Key " + (string)key + " not present");
}
void set(const Symbol& key, const T& value) {
void set(Key key, const T& value) {
Vec& vec(values_[key.chr()]);
//vec.reserve(10000);
if(key.index() >= vec.size()) {
@ -62,13 +62,13 @@ public:
};
template<class T>
class SymbolMapBinary : public std::map<Symbol, T> {
class SymbolMapBinary : public std::map<Key, T> {
private:
typedef std::map<Symbol, T> Base;
typedef std::map<Key, T> Base;
public:
SymbolMapBinary() : std::map<Symbol, T>() {}
SymbolMapBinary() : std::map<Key, T>() {}
T& at(const Symbol& key) {
T& at(Key key) {
typename Base::iterator it = Base::find(key);
if (it == Base::end())
throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key));
@ -76,8 +76,8 @@ public:
}
};
struct SymbolHash : public std::unary_function<Symbol, std::size_t> {
std::size_t operator()(Symbol const& x) const {
struct SymbolHash : public std::unary_function<Key, std::size_t> {
std::size_t operator()(Key const& x) const {
std::size_t seed = 0;
boost::hash_combine(seed, x.chr());
boost::hash_combine(seed, x.index());
@ -86,9 +86,9 @@ struct SymbolHash : public std::unary_function<Symbol, std::size_t> {
};
template<class T>
class SymbolMapHash : public boost::unordered_map<Symbol, T, SymbolHash> {
class SymbolMapHash : public boost::unordered_map<Key, T, SymbolHash> {
public:
SymbolMapHash() : boost::unordered_map<Symbol, T, SymbolHash>(60000) {}
SymbolMapHash() : boost::unordered_map<Key, T, SymbolHash>(60000) {}
};
struct Value {
@ -107,7 +107,7 @@ int main(int argc, char *argv[]) {
// pre-allocate
cout << "Generating test data ..." << endl;
vector<pair<Symbol, Value> > values;
vector<pair<Key, Value> > values;
for(size_t i=0; i<ELEMS; i++) {
values.push_back(make_pair(Symbol('a',i), (double)i));
values.push_back(make_pair(Symbol('b',i), (double)i));

6
gtsam/linear/SubgraphSolver-inl.h
View File

@ -67,14 +67,14 @@ void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values&
// make the ordering
list<KEY> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
ordering_ = boost::make_shared<Ordering>(list<Key>(keys.begin(), keys.end()));
// build factor pairs
pairs_.clear();
typedef pair<KEY,KEY> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first),
key2 = Symbol(eg.second) ;
Key key1 = Key(eg.first),
key2 = Key(eg.second) ;
pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
}
}

8
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
const Values& linearizationPoints, const Symbol& lastKey,
const Values& linearizationPoints, Key lastKey,
JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key
@ -91,8 +91,8 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
Symbol x0 = motionFactor.key1();
Symbol x1 = motionFactor.key2();
Key x0 = motionFactor.key1();
Key x1 = motionFactor.key2();
// Create an elimination ordering
Ordering ordering;
@ -131,7 +131,7 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
Symbol x0 = measurementFactor.key();
Key x0 = measurementFactor.key();
// Create an elimination ordering
Ordering ordering;

2
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -55,7 +55,7 @@ namespace gtsam {
T solve_(const GaussianFactorGraph& linearFactorGraph,
const Ordering& ordering, const Values& linearizationPoints,
const Symbol& x, JacobianFactor::shared_ptr& newPrior) const;
Key x, JacobianFactor::shared_ptr& newPrior) const;
public:

2
gtsam/nonlinear/GaussianISAM2-inl.h
View File

@ -121,7 +121,7 @@ namespace gtsam {
///* ************************************************************************* */
//boost::shared_ptr<VectorValues> optimize2(const GaussianISAM2::sharedClique& root) {
// boost::shared_ptr<VectorValues> delta(new VectorValues());
// set<Symbol> changed;
// set<Key> changed;
// // starting from the root, call optimize on each conditional
// optimize2(root, delta);
// return delta;

2
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -155,7 +155,7 @@ template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
BOOST_FOREACH(Key key, factor->keys()) {
indices.insert(ordering[key]);
}
}

6
gtsam/nonlinear/ISAM2-inl.h
View File

@ -105,7 +105,7 @@ ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& aff
tic(3,"check candidates");
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
Index var = ordering_[key];
if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
inside = false;
@ -398,7 +398,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
template<class CONDITIONAL, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
const boost::optional<FastSet<Key> >& constrainedKeys, bool force_relinearize) {
static const bool debug = ISDEBUG("ISAM2 update");
static const bool verbose = ISDEBUG("ISAM2 update verbose");
@ -519,7 +519,7 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
boost::optional<FastSet<Index> > constrainedIndices;
if(constrainedKeys) {
constrainedIndices.reset(FastSet<Index>());
BOOST_FOREACH(const Symbol& key, *constrainedKeys) {
BOOST_FOREACH(Key key, *constrainedKeys) {
constrainedIndices->insert(ordering_[key]);
}
}

2
gtsam/nonlinear/ISAM2.h
View File

@ -369,7 +369,7 @@ public:
*/
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
const boost::optional<FastSet<Key> >& constrainedKeys = boost::none,
bool force_relinearize = false);
/** Access the current linearization point */

10
gtsam/nonlinear/NonlinearEquality.h
View File

@ -89,7 +89,7 @@ namespace gtsam {
/**
* Constructor - forces exact evaluation
*/
NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(0.0),
compare_(_compare) {
@ -98,7 +98,7 @@ namespace gtsam {
/**
* Constructor - allows inexact evaluation
*/
NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain),
compare_(_compare) {
@ -109,7 +109,7 @@ namespace gtsam {
/// @{
virtual void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key()) << "]\n";
std::cout << "Constraint: " << s << " on [" << (std::string)this->key() << "]\n";
gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
}
@ -204,7 +204,7 @@ namespace gtsam {
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
///TODO: comment
NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0)
NonlinearEquality1(const X& value, Key key1, double mu = 1000.0)
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
virtual ~NonlinearEquality1() {}
@ -259,7 +259,7 @@ namespace gtsam {
typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
///TODO: comment
NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0)
NonlinearEquality2(Key key1, Key key2, double mu = 1000.0)
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}

61
gtsam/nonlinear/NonlinearFactor.h
View File

@ -32,6 +32,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
namespace gtsam {
@ -44,12 +45,12 @@ namespace gtsam {
* which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping
*/
class NonlinearFactor: public Factor<Symbol> {
class NonlinearFactor: public Factor<Key> {
protected:
// Some handy typedefs
typedef Factor<Symbol> Base;
typedef Factor<Key> Base;
typedef NonlinearFactor This;
public:
@ -187,7 +188,7 @@ public:
virtual void print(const std::string& s = "") const {
std::cout << s << ": NoiseModelFactor\n";
std::cout << " ";
BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; }
BOOST_FOREACH(Key key, this->keys()) { std::cout << (std::string)key << " "; }
std::cout << "\n";
this->noiseModel_->print(" noise model: ");
}
@ -310,14 +311,14 @@ public:
virtual ~NonlinearFactor1() {}
inline const Symbol& key() const { return keys_[0]; }
inline Key key() const { return keys_[0]; }
/**
* Constructor
* @param z measurement
* @param key by which to look up X value in Values
*/
NonlinearFactor1(const SharedNoiseModel& noiseModel, const Symbol& key1) :
NonlinearFactor1(const SharedNoiseModel& noiseModel, Key key1) :
Base(noiseModel) {
keys_.resize(1);
keys_[0] = key1;
@ -393,7 +394,7 @@ public:
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
NonlinearFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
Base(noiseModel) {
keys_.resize(2);
keys_[0] = j1;
@ -403,8 +404,8 @@ public:
virtual ~NonlinearFactor2() {}
/** methods to retrieve both keys */
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
@ -481,7 +482,7 @@ public:
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
NonlinearFactor3(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3) :
NonlinearFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
Base(noiseModel) {
keys_.resize(3);
keys_[0] = j1;
@ -492,9 +493,9 @@ public:
virtual ~NonlinearFactor3() {}
/** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
/** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
@ -574,7 +575,7 @@ public:
* @param j3 key of the third variable
* @param j4 key of the fourth variable
*/
NonlinearFactor4(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4) :
NonlinearFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
Base(noiseModel) {
keys_.resize(4);
keys_[0] = j1;
@ -586,10 +587,10 @@ public:
virtual ~NonlinearFactor4() {}
/** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
inline Key key4() const { return keys_[3]; }
/** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
@ -672,7 +673,7 @@ public:
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
*/
NonlinearFactor5(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5) :
NonlinearFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
Base(noiseModel) {
keys_.resize(5);
keys_[0] = j1;
@ -685,11 +686,11 @@ public:
virtual ~NonlinearFactor5() {}
/** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; }
inline const Symbol& key5() const { return keys_[4]; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
inline Key key4() const { return keys_[3]; }
inline Key key5() const { return keys_[4]; }
/** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
@ -776,7 +777,7 @@ public:
* @param j5 key of the fifth variable
* @param j6 key of the fifth variable
*/
NonlinearFactor6(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5, const Symbol& j6) :
NonlinearFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
Base(noiseModel) {
keys_.resize(6);
keys_[0] = j1;
@ -790,12 +791,12 @@ public:
virtual ~NonlinearFactor6() {}
/** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; }
inline const Symbol& key5() const { return keys_[4]; }
inline const Symbol& key6() const { return keys_[5]; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
inline Key key4() const { return keys_[3]; }
inline Key key5() const { return keys_[4]; }
inline Key key6() const { return keys_[5]; }
/** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */

4
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -49,8 +49,8 @@ namespace gtsam {
}
/* ************************************************************************* */
std::set<Symbol> NonlinearFactorGraph::keys() const {
std::set<Symbol> keys;
std::set<Key> NonlinearFactorGraph::keys() const {
std::set<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -47,7 +47,7 @@ namespace gtsam {
void print(const std::string& str = "NonlinearFactorGraph: ") const;
/** return keys in some random order */
std::set<Symbol> keys() const;
std::set<Key> keys() const;
/** unnormalized error */
double error(const Values& c) const;

4
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -50,7 +50,7 @@ void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
// Augment ordering
// FIXME: should just loop over new values
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors)
BOOST_FOREACH(const Symbol& key, factor->keys())
BOOST_FOREACH(Key key, factor->keys())
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
@ -99,7 +99,7 @@ Values NonlinearISAM<GRAPH>::estimate() const {
/* ************************************************************************* */
template<class GRAPH>
Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const {
Matrix NonlinearISAM<GRAPH>::marginalCovariance(Key key) const {
return isam_.marginalCovariance(ordering_[key]);
}

4
gtsam/nonlinear/NonlinearISAM.h
View File

@ -70,7 +70,7 @@ public:
Values estimate() const;
/** find the marginal covariance for a single variable */
Matrix marginalCovariance(const Symbol& key) const;
Matrix marginalCovariance(Key key) const;
// access
@ -104,7 +104,7 @@ public:
void reorder_relinearize();
/** manually add a key to the end of the ordering */
void addKey(const Symbol& key) { ordering_.push_back(key); }
void addKey(Key key) { ordering_.push_back(key); }
/** replace the current ordering */
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }

2
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -242,7 +242,7 @@ public:
/**
* Return mean and covariance on a single variable
*/
Matrix marginalCovariance(Symbol j) const {
Matrix marginalCovariance(Key j) const {
return createSolver()->marginalCovariance((*ordering_)[j]);
}

6
gtsam/nonlinear/Ordering.cpp
View File

@ -26,9 +26,9 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Ordering::Ordering(const std::list<Symbol> & L):nVars_(0) {
Ordering::Ordering(const std::list<Key> & L):nVars_(0) {
int i = 0;
BOOST_FOREACH( const Symbol& s, L )
BOOST_FOREACH( Key s, L )
insert(s, i++) ;
}
@ -70,7 +70,7 @@ Ordering::value_type Ordering::pop_back() {
}
/* ************************************************************************* */
Index Ordering::pop_back(const Symbol& key) {
Index Ordering::pop_back(Key key) {
Map::iterator item = order_.find(key);
if(item == order_.end()) {
throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key");

42
gtsam/nonlinear/Ordering.h
View File

@ -17,9 +17,8 @@
#pragma once
#include <map>
#include <set>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/inference.h>
#include <boost/foreach.hpp>
@ -34,8 +33,7 @@ namespace gtsam {
*/
class Ordering {
protected:
typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator;
typedef std::map<Symbol, Index, std::less<Symbol>, Allocator> Map;
typedef FastMap<Key, Index> Map;
Map order_;
Index nVars_;
@ -43,7 +41,7 @@ public:
typedef boost::shared_ptr<Ordering> shared_ptr;
typedef std::pair<const Symbol, Index> value_type;
typedef std::pair<const Key, Index> value_type;
typedef Map::iterator iterator;
typedef Map::const_iterator const_iterator;
@ -54,7 +52,7 @@ public:
Ordering() : nVars_(0) {}
/// Construct from list, assigns order indices sequentially to list items.
Ordering(const std::list<Symbol> & L) ;
Ordering(const std::list<Key> & L) ;
/// @}
/// @name Standard Interface
@ -69,7 +67,7 @@ public:
const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
Index at(const Symbol& key) const { return operator[](key); } ///< Synonym for operator[](const Symbol&) const
Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const
/** Assigns the ordering index of the requested \c key into \c index if the symbol
* is present in the ordering, otherwise does not modify \c index. The
@ -79,7 +77,7 @@ public:
* @param [out] index Reference into which to write the index of the requested key, if the key is present.
* @return true if the key is present and \c index was modified, false otherwise.
*/
bool tryAt(const Symbol& key, Index& index) const {
bool tryAt(Key key, Index& index) const {
const_iterator i = order_.find(key);
if(i != order_.end()) {
index = i->second;
@ -91,7 +89,7 @@ public:
/// Access the index for the requested key, throws std::out_of_range if the
/// key is not present in the ordering (note that this differs from the
/// behavior of std::map)
Index& operator[](const Symbol& key) {
Index& operator[](Key key) {
iterator i=order_.find(key);
if(i == order_.end()) throw std::out_of_range(std::string());
else return i->second; }
@ -99,7 +97,7 @@ public:
/// Access the index for the requested key, throws std::out_of_range if the
/// key is not present in the ordering (note that this differs from the
/// behavior of std::map)
Index operator[](const Symbol& key) const {
Index operator[](Key key) const {
const_iterator i=order_.find(key);
if(i == order_.end()) throw std::out_of_range(std::string());
else return i->second; }
@ -110,7 +108,7 @@ public:
* @return An iterator pointing to the symbol/index pair with the requested,
* or the end iterator if it does not exist.
*/
iterator find(const Symbol& key) { return order_.find(key); }
iterator find(Key key) { return order_.find(key); }
/** Returns an iterator pointing to the symbol/index pair with the requested,
* or the end iterator if it does not exist.
@ -118,7 +116,7 @@ public:
* @return An iterator pointing to the symbol/index pair with the requested,
* or the end iterator if it does not exist.
*/
const_iterator find(const Symbol& key) const { return order_.find(key); }
const_iterator find(Key key) const { return order_.find(key); }
/**
* Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
@ -153,22 +151,22 @@ public:
iterator end() { return order_.end(); }
/// Test if the key exists in the ordering.
bool exists(const Symbol& key) const { return order_.count(key); }
bool exists(Key key) const { return order_.count(key); }
///TODO: comment
std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); }
std::pair<iterator,bool> tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); }
///TODO: comment
iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
/// Adds a new key to the ordering with an index of one greater than the current highest index.
Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
Index push_back(Key key) { return insert(std::make_pair(key, nVars_))->second; }
/** Remove the last (last-ordered, not highest-sorting key) symbol/index pair
* from the ordering (this version is \f$ O(n) \f$, use it when you do not
* know the last-ordered key).
*
* If you already know the last-ordered symbol, call popback(const Symbol&)
* If you already know the last-ordered symbol, call popback(Key)
* that accepts this symbol as an argument.
*
* @return The symbol and index that were removed.
@ -183,15 +181,15 @@ public:
*
* @return The index of the symbol that was removed.
*/
Index pop_back(const Symbol& key);
Index pop_back(Key key);
/**
* += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
* very useful for unit tests. This functionality is courtesy of
* boost::assign.
*/
inline boost::assign::list_inserter<boost::assign_detail::call_push_back<Ordering>, Symbol>
operator+=(const Symbol& key) {
inline boost::assign::list_inserter<boost::assign_detail::call_push_back<Ordering>, Key>
operator+=(Key key) {
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<Ordering>(*this))(key); }
/**
@ -201,8 +199,8 @@ public:
*/
void permuteWithInverse(const Permutation& inversePermutation);
/// Synonym for operator[](const Symbol&)
Index& at(const Symbol& key) { return operator[](key); }
/// Synonym for operator[](Key)
Index& at(Key key) { return operator[](key); }
/// @}
/// @name Testable

24
gtsam/nonlinear/Symbol.h
View File

@ -87,6 +87,30 @@ public:
}
#endif
/** Constructor that decodes an integer Key */
Symbol(Key key) {
const size_t keyBytes = sizeof(Key);
const size_t chrBytes = sizeof(unsigned char);
const size_t indexBytes = keyBytes - chrBytes;
const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
const Key indexMask = ~chrMask;
c_ = key & chrMask;
j_ = key & indexMask;
}
/** Cast to integer */
operator Key() const {
const size_t keyBytes = sizeof(Key);
const size_t chrBytes = sizeof(unsigned char);
const size_t indexBytes = keyBytes - chrBytes;
const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
const Key indexMask = ~chrMask;
if(j_ > indexMask)
throw std::invalid_argument("Symbol index is too large");
Key key = (c_ << indexBytes) | j_;
return key;
}
// Testable Requirements
void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;

32
gtsam/nonlinear/Values-inl.h
View File

@ -43,11 +43,11 @@ namespace gtsam {
#if 0
/* ************************************************************************* */
class ValueAutomaticCasting {
const Symbol& key_;
Key key_;
const Value& value_;
public:
ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {}
ValueAutomaticCasting(Key key, const Value& value) : key_(key), value_(value) {}
template<class ValueType>
class ConvertibleToValue : public ValueType {
@ -67,7 +67,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename ValueType>
const ValueType& Values::at(const Symbol& j) const {
const ValueType& Values::at(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
@ -85,7 +85,7 @@ namespace gtsam {
#if 0
/* ************************************************************************* */
inline ValueAutomaticCasting Values::at(const Symbol& j) const {
inline ValueAutomaticCasting Values::at(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
@ -97,26 +97,16 @@ namespace gtsam {
}
#endif
/* ************************************************************************* */
template<typename TypedKey>
const typename TypedKey::Value& Values::at(const TypedKey& j) const {
// Convert to Symbol
const Symbol symbol(j.symbol());
// Call at with the Value type from the key
return at<typename TypedKey::Value>(symbol);
}
#if 0
/* ************************************************************************* */
inline ValueAutomaticCasting Values::operator[](const Symbol& j) const {
inline ValueAutomaticCasting Values::operator[](Key j) const {
return at(j);
}
#endif
/* ************************************************************************* */
template<typename ValueType>
boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
boost::optional<const ValueType&> Values::exists(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
@ -132,14 +122,4 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class TypedKey>
boost::optional<const typename TypedKey::Value&> Values::exists(const TypedKey& j) const {
// Convert to Symbol
const Symbol symbol(j.symbol());
// Call exists with the Value type from the key
return exists<typename TypedKey::Value>(symbol);
}
}

18
gtsam/nonlinear/Values.cpp
View File

@ -64,7 +64,7 @@ namespace gtsam {
}
/* ************************************************************************* */
bool Values::exists(const Symbol& j) const {
bool Values::exists(Key j) const {
return values_.find(j) != values_.end();
}
@ -79,7 +79,7 @@ namespace gtsam {
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
}
@ -107,8 +107,8 @@ namespace gtsam {
}
/* ************************************************************************* */
void Values::insert(const Symbol& j, const Value& val) {
Symbol key = j; // Non-const duplicate to deal with non-const insert argument
void Values::insert(Key j, const Value& val) {
Key key = j; // Non-const duplicate to deal with non-const insert argument
std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
if(!insertResult.second)
throw ValuesKeyAlreadyExists(j);
@ -117,13 +117,13 @@ namespace gtsam {
/* ************************************************************************* */
void Values::insert(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument
insert(key, key_value->second);
}
}
/* ************************************************************************* */
void Values::update(const Symbol& j, const Value& val) {
void Values::update(Key j, const Value& val) {
// Find the value to update
KeyValueMap::iterator item = values_.find(j);
if(item == values_.end())
@ -144,7 +144,7 @@ namespace gtsam {
}
/* ************************************************************************* */
void Values::erase(const Symbol& j) {
void Values::erase(Key j) {
KeyValueMap::iterator item = values_.find(j);
if(item == values_.end())
throw ValuesKeyDoesNotExist("erase", j);
@ -152,8 +152,8 @@ namespace gtsam {
}
/* ************************************************************************* */
FastList<Symbol> Values::keys() const {
FastList<Symbol> result;
FastList<Key> Values::keys() const {
FastList<Key> result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value)
result.push_back(key_value->first);
return result;

81
gtsam/nonlinear/Values.h
View File

@ -37,7 +37,6 @@
#include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -64,11 +63,11 @@ namespace gtsam {
// fast_pool_allocator to allocate map nodes. In this way, all memory is
// allocated in a boost memory pool.
typedef boost::ptr_map<
Symbol,
Key,
Value,
std::less<Symbol>,
std::less<Key>,
ValueCloneAllocator,
boost::fast_pool_allocator<std::pair<const Symbol, void*> > > KeyValueMap;
boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap;
// The member to store the values, see just above
KeyValueMap values_;
@ -84,16 +83,16 @@ namespace gtsam {
/// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
struct ConstKeyValuePair {
const Symbol& first;
const Key first;
const Value& second;
ConstKeyValuePair(const Symbol& key, const Value& value) : first(key), second(value) {}
ConstKeyValuePair(Key key, const Value& value) : first(key), second(value) {}
};
/// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
struct KeyValuePair {
const Symbol& first;
const Key first;
Value& second;
KeyValuePair(const Symbol& key, Value& value) : first(key), second(value) {}
KeyValuePair(Key key, Value& value) : first(key), second(value) {}
};
/// Mutable forward iterator, with value type KeyValuePair
@ -137,7 +136,7 @@ namespace gtsam {
* @return A const reference to the stored value
*/
template<typename ValueType>
const ValueType& at(const Symbol& j) const;
const ValueType& at(Key j) const;
#if 0
/** Retrieve a variable by key \c j. This non-templated version returns a
@ -147,51 +146,25 @@ namespace gtsam {
* @return A ValueAutomaticCasting object that may be assignmed to a Value
* of the proper type.
*/
ValueAutomaticCasting at(const Symbol& j) const;
ValueAutomaticCasting at(Key j) const;
#endif
/** Retrieve a variable using a special key (typically TypedSymbol), which
* contains the type of the value associated with the key, and which must
* be conversion constructible to a Symbol, e.g.
* <tt>Symbol(const TypedKey&)</tt>. Throws DynamicValuesKeyDoesNotExist
* the key is not found, and DynamicValuesIncorrectType if the value type
* associated with the requested key does not match the stored value type.
*/
template<class TypedKey>
const typename TypedKey::Value& at(const TypedKey& j) const;
/** operator[] syntax for at(const TypedKey& j) */
template<class TypedKey>
const typename TypedKey::Value& operator[](const TypedKey& j) const {
return at(j); }
#if 0
/** operator[] syntax for at(const Symbol& j) */
ValueAutomaticCasting operator[](const Symbol& j) const;
/** operator[] syntax for at(Key j) */
ValueAutomaticCasting operator[](Key j) const;
#endif
/** Check if a value exists with key \c j. See exists<>(const Symbol& j)
/** Check if a value exists with key \c j. See exists<>(Key j)
* and exists(const TypedKey& j) for versions that return the value if it
* exists. */
bool exists(const Symbol& j) const;
bool exists(Key j) const;
/** Check if a value with key \c j exists, returns the value with type
* \c Value if the key does exist, or boost::none if it does not exist.
* Throws DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type. */
template<typename ValueType>
boost::optional<const ValueType&> exists(const Symbol& j) const;
/** Check if a value with key \c j exists, returns the value with type
* \c Value if the key does exist, or boost::none if it does not exist.
* Uses a special key (typically TypedSymbol), which contains the type of
* the value associated with the key, and which must be conversion
* constructible to a Symbol, e.g. <tt>Symbol(const TypedKey&)</tt>. Throws
* DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type.
*/
template<class TypedKey>
boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const;
boost::optional<const ValueType&> exists(Key j) const;
/** The number of variables in this config */
size_t size() const { return values_.size(); }
@ -233,25 +206,25 @@ namespace gtsam {
///@}
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
void insert(const Symbol& j, const Value& val);
void insert(Key j, const Value& val);
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const Values& values);
/** single element change of existing element */
void update(const Symbol& j, const Value& val);
void update(Key j, const Value& val);
/** update the current available values without adding new ones */
void update(const Values& values);
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
void erase(const Symbol& j);
void erase(Key j);
/**
* Returns a set of keys in the config
* Note: by construction, the list is ordered
*/
FastList<Symbol> keys() const;
FastList<Key> keys() const;
/** Replace all keys and variables */
Values& operator=(const Values& rhs);
@ -286,20 +259,20 @@ namespace gtsam {
/* ************************************************************************* */
class ValuesKeyAlreadyExists : public std::exception {
protected:
const Symbol key_; ///< The key that already existed
const Key key_; ///< The key that already existed
private:
mutable std::string message_;
public:
/// Construct with the key-value pair attemped to be added
ValuesKeyAlreadyExists(const Symbol& key) throw() :
ValuesKeyAlreadyExists(Key key) throw() :
key_(key) {}
virtual ~ValuesKeyAlreadyExists() throw() {}
/// The duplicate key that was attemped to be added
const Symbol& key() const throw() { return key_; }
Key key() const throw() { return key_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
@ -309,20 +282,20 @@ namespace gtsam {
class ValuesKeyDoesNotExist : public std::exception {
protected:
const char* operation_; ///< The operation that attempted to access the key
const Symbol key_; ///< The key that does not exist
const Key key_; ///< The key that does not exist
private:
mutable std::string message_;
public:
/// Construct with the key that does not exist in the values
ValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() :
ValuesKeyDoesNotExist(const char* operation, Key key) throw() :
operation_(operation), key_(key) {}
virtual ~ValuesKeyDoesNotExist() throw() {}
/// The key that was attempted to be accessed that does not exist
const Symbol& key() const throw() { return key_; }
Key key() const throw() { return key_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
@ -331,7 +304,7 @@ namespace gtsam {
/* ************************************************************************* */
class ValuesIncorrectType : public std::exception {
protected:
const Symbol key_; ///< The key requested
const Key key_; ///< The key requested
const std::type_info& storedTypeId_;
const std::type_info& requestedTypeId_;
@ -340,14 +313,14 @@ namespace gtsam {
public:
/// Construct with the key that does not exist in the values
ValuesIncorrectType(const Symbol& key,
ValuesIncorrectType(Key key,
const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
virtual ~ValuesIncorrectType() throw() {}
/// The key that was attempted to be accessed that does not exist
const Symbol& key() const throw() { return key_; }
Key key() const throw() { return key_; }
/// The typeid of the value stores in the Values
const std::type_info& storedTypeId() const { return storedTypeId_; }

6
gtsam/nonlinear/tests/testValues.cpp
View File

@ -31,7 +31,7 @@ using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
Symbol key1("v1"), key2("v2"), key3("v3"), key4("v4");
Key key1("v1"), key2("v2"), key3("v3"), key4("v4");
/* ************************************************************************* */
TEST( Values, equals1 )
@ -218,12 +218,12 @@ TEST(Values, extract_keys)
config.insert("x4", Pose2());
config.insert("x5", Pose2());
FastList<Symbol> expected, actual;
FastList<Key> expected, actual;
expected += "x1", "x2", "x4", "x5";
actual = config.keys();
CHECK(actual.size() == expected.size());
FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
FastList<Key>::const_iterator itAct = actual.begin(), itExp = expected.begin();
for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
CHECK(assert_equal(*itExp, *itAct));
}

2
gtsam/slam/BearingFactor.h
View File

@ -48,7 +48,7 @@ namespace gtsam {
BearingFactor() {}
/** primary constructor */
BearingFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measured,
BearingFactor(Key poseKey, Key pointKey, const Rot& measured,
const SharedNoiseModel& model) :
Base(model, poseKey, pointKey), measured_(measured) {
}

2
gtsam/slam/BearingRangeFactor.h
View File

@ -50,7 +50,7 @@ namespace gtsam {
public:
BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measuredBearing, const double measuredRange,
BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double measuredRange,
const SharedNoiseModel& model) :
Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) {
}

4
gtsam/slam/BetweenFactor.h
View File

@ -55,7 +55,7 @@ namespace gtsam {
BetweenFactor() {}
/** Constructor */
BetweenFactor(const Symbol& key1, const Symbol& key2, const VALUE& measured,
BetweenFactor(Key key1, Key key2, const VALUE& measured,
const SharedNoiseModel& model) :
Base(model, key1, key2), measured_(measured) {
}
@ -123,7 +123,7 @@ namespace gtsam {
typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
/** Syntactic sugar for constrained version */
BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) :
BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
private:

4
gtsam/slam/BoundingConstraint.h
View File

@ -37,7 +37,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(const Symbol& key, double threshold,
BoundingConstraint1(Key key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(noiseModel::Constrained::All(1, mu), key),
threshold_(threshold), isGreaterThan_(isGreaterThan) {
@ -106,7 +106,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUE1, VALUE2> {
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint2(const Symbol& key1, const Symbol& key2, double threshold,
BoundingConstraint2(Key key1, Key key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
threshold_(threshold), isGreaterThan_(isGreaterThan) {}

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -52,7 +52,7 @@ namespace gtsam {
* @param i is basically the frame number
* @param j is the index of the landmark
*/
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) :
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor

6
gtsam/slam/PartialPriorFactor.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
* constructor with just minimum requirements for a factor - allows more
* computation in the constructor. This should only be used by subclasses
*/
PartialPriorFactor(const Symbol& key, const SharedNoiseModel& model)
PartialPriorFactor(Key key, const SharedNoiseModel& model)
: Base(model, key) {}
public:
@ -71,14 +71,14 @@ namespace gtsam {
virtual ~PartialPriorFactor() {}
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(const Symbol& key, size_t idx, double prior, const SharedNoiseModel& model) :
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
assert(model->dim() == 1);
this->fillH();
}
/** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(const Symbol& key, const std::vector<size_t>& mask, const Vector& prior,
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
assert((size_t)prior_.size() == mask.size());

2
gtsam/slam/PriorFactor.h
View File

@ -52,7 +52,7 @@ namespace gtsam {
virtual ~PriorFactor() {}
/** Constructor */
PriorFactor(const Symbol& key, const VALUE& prior, const SharedNoiseModel& model) :
PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model) :
Base(model, key), prior_(prior) {
}

2
gtsam/slam/ProjectionFactor.h
View File

@ -63,7 +63,7 @@ namespace gtsam {
* @param K shared pointer to the constant calibration
*/
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
const Symbol poseKey, const Symbol& pointKey, const shared_ptrK& K) :
const Symbol poseKey, Key pointKey, const shared_ptrK& K) :
Base(model, poseKey, pointKey), measured_(measured), K_(K) {
}

2
gtsam/slam/RangeFactor.h
View File

@ -44,7 +44,7 @@ namespace gtsam {
RangeFactor() {} /* Default constructor */
RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured,
RangeFactor(Key poseKey, Key pointKey, double measured,
const SharedNoiseModel& model) :
Base(model, poseKey, pointKey), measured_(measured) {
}

2
gtsam/slam/StereoFactor.h
View File

@ -50,7 +50,7 @@ public:
* @param landmarkKey the landmark variable key
* @param K the constant calibration
*/
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey, const shared_ptrKStereo& K) :
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, const shared_ptrKStereo& K) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {
}

6
gtsam/slam/simulated2D.h
View File

@ -129,7 +129,7 @@ namespace simulated2D {
Pose measured_; ///< prior mean
/// Create generic prior
GenericPrior(const Pose& z, const SharedNoiseModel& model, const Symbol& key) :
GenericPrior(const Pose& z, const SharedNoiseModel& model, Key key) :
Base(model, key), measured_(z) {
}
@ -166,7 +166,7 @@ namespace simulated2D {
Pose measured_; ///< odometry measurement
/// Create odometry
GenericOdometry(const Pose& measured, const SharedNoiseModel& model, const Symbol& key1, const Symbol& key2) :
GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key key1, Key key2) :
Base(model, key1, key2), measured_(measured) {
}
@ -206,7 +206,7 @@ namespace simulated2D {
Landmark measured_; ///< Measurement
/// Create measurement factor
GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) :
GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey) :
Base(model, poseKey, landmarkKey), measured_(measured) {
}

6
gtsam/slam/simulated2DConstraints.h
View File

@ -65,7 +65,7 @@ namespace simulated2D {
* @param isGreaterThan is a flag to set inequality as greater than or less than
* @param mu is the penalty function gain
*/
ScalarCoordConstraint1(const Symbol& key, double c,
ScalarCoordConstraint1(Key key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
}
@ -127,7 +127,7 @@ namespace simulated2D {
* @param range_bound is the maximum range allowed between the variables
* @param mu is the gain for the penalty function
*/
MaxDistanceConstraint(const Symbol& key1, const Symbol& key2, double range_bound, double mu = 1000.0) :
MaxDistanceConstraint(Key key1, Key key2, double range_bound, double mu = 1000.0) :
Base(key1, key2, range_bound, false, mu) {}
/**
@ -170,7 +170,7 @@ namespace simulated2D {
* @param range_bound is the minimum range allowed between the variables
* @param mu is the gain for the penalty function
*/
MinDistanceConstraint(const Symbol& key1, const Symbol& key2,
MinDistanceConstraint(Key key1, Key key2,
double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {}

4
gtsam/slam/simulated2DOriented.h
View File

@ -83,7 +83,7 @@ namespace simulated2DOriented {
Pose2 measured_; ///< measurement
/// Create generic pose prior
GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, const Symbol& key) :
GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) :
NonlinearFactor1<VALUE>(model, key), measured_(measured) {
}
@ -106,7 +106,7 @@ namespace simulated2DOriented {
* Creates an odometry factor between two poses
*/
GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
const Symbol& i1, const Symbol& i2) :
Key i1, Key i2) :
NonlinearFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
}

4
gtsam/slam/simulated3D.h
View File

@ -74,7 +74,7 @@ struct PointPrior3D: public NonlinearFactor1<Point3> {
* @param model is the measurement model for the factor (Dimension: 3)
* @param key is the key for the pose
*/
PointPrior3D(const Point3& measured, const SharedNoiseModel& model, const Symbol& key) :
PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) :
NonlinearFactor1<Point3> (model, key), measured_(measured) {
}
@ -106,7 +106,7 @@ struct Simulated3DMeasurement: public NonlinearFactor2<Point3, Point3> {
* @param pointKey is the point key for the landmark
*/
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
const Symbol& poseKey, const Symbol& pointKey) :
Key poseKey, Key pointKey) :
NonlinearFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
/**

2
gtsam/slam/smallExample.cpp
View File

@ -202,7 +202,7 @@ namespace example {
Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, const Symbol& key) :
UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
gtsam::NonlinearFactor1<Point2>(model, key), z_(z) {
}

4
tests/testNonlinearFactorGraph.cpp
View File

@ -64,9 +64,9 @@ TEST( Graph, error )
TEST( Graph, keys )
{
Graph fg = createNonlinearFactorGraph();
set<Symbol> actual = fg.keys();
set<Key> actual = fg.keys();
LONGS_EQUAL(3, actual.size());
set<Symbol>::const_iterator it = actual.begin();
set<Key>::const_iterator it = actual.begin();
CHECK(assert_equal(Symbol('l', 1), *(it++)));
CHECK(assert_equal(Symbol('x', 1), *(it++)));
CHECK(assert_equal(Symbol('x', 2), *(it++)));

4
tests/testNonlinearISAM.cpp
View File

@ -48,8 +48,8 @@ TEST(testNonlinearISAM, markov_chain ) {
Ordering ordering = isam.getOrdering();
// swap last two elements
Symbol last = ordering.pop_back().first;
Symbol secondLast = ordering.pop_back().first;
Key last = ordering.pop_back().first;
Key secondLast = ordering.pop_back().first;
ordering.push_back(last);
ordering.push_back(secondLast);
isam.setOrdering(ordering);