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

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

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

36
gtsam/inference/Factor.h
View File

@ -55,28 +55,28 @@ class Factor {
public: public:
typedef KEY Key; ///< The KEY template parameter typedef KEY KeyType; ///< The KEY template parameter
typedef Factor<Key> This; ///< This class typedef Factor<KeyType> This; ///< This class
/** /**
* Typedef to the conditional type obtained by eliminating this factor, * Typedef to the conditional type obtained by eliminating this factor,
* derived classes must redefine this. * derived classes must redefine this.
*/ */
typedef Conditional<Key> ConditionalType; typedef Conditional<KeyType> ConditionalType;
/// A shared_ptr to this class, derived classes must redefine this. /// A shared_ptr to this class, derived classes must redefine this.
typedef boost::shared_ptr<Factor> shared_ptr; typedef boost::shared_ptr<Factor> shared_ptr;
/// Iterator over keys /// Iterator over keys
typedef typename std::vector<Key>::iterator iterator; typedef typename std::vector<KeyType>::iterator iterator;
/// Const iterator over keys /// Const iterator over keys
typedef typename std::vector<Key>::const_iterator const_iterator; typedef typename std::vector<KeyType>::const_iterator const_iterator;
protected: protected:
/// The keys involved in this factor /// The keys involved in this factor
std::vector<Key> keys_; std::vector<KeyType> keys_;
friend class JacobianFactor; friend class JacobianFactor;
friend class HessianFactor; friend class HessianFactor;
@ -102,19 +102,19 @@ public:
Factor() {} Factor() {}
/** Construct unary factor */ /** Construct unary factor */
Factor(Key key) : keys_(1) { Factor(KeyType key) : keys_(1) {
keys_[0] = key; assertInvariants(); } keys_[0] = key; assertInvariants(); }
/** Construct binary factor */ /** Construct binary factor */
Factor(Key key1, Key key2) : keys_(2) { Factor(KeyType key1, KeyType key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); } keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */ /** 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(); } keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
/** Construct 4-way factor */ /** 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(); } keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/// @} /// @}
@ -122,13 +122,13 @@ public:
/// @{ /// @{
/** Construct n-way factor */ /** Construct n-way factor */
Factor(const std::set<Key>& keys) { Factor(const std::set<KeyType>& keys) {
BOOST_FOREACH(const Key& key, keys) keys_.push_back(key); BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants(); assertInvariants();
} }
/** Construct n-way factor */ /** Construct n-way factor */
Factor(const std::vector<Key>& keys) : keys_(keys) { Factor(const std::vector<KeyType>& keys) : keys_(keys) {
assertInvariants(); assertInvariants();
} }
@ -157,16 +157,16 @@ public:
/// @{ /// @{
/// First key /// First key
Key front() const { return keys_.front(); } KeyType front() const { return keys_.front(); }
/// Last key /// Last key
Key back() const { return keys_.back(); } KeyType back() const { return keys_.back(); }
/// find /// 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 ///TODO: comment
const std::vector<Key>& keys() const { return keys_; } const std::vector<KeyType>& keys() const { return keys_; }
/** iterators */ /** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment const_iterator begin() const { return keys_.begin(); } ///TODO: comment
@ -194,7 +194,7 @@ public:
/** /**
* @return keys involved in this factor * @return keys involved in this factor
*/ */
std::vector<Key>& keys() { return keys_; } std::vector<KeyType>& keys() { return keys_; }
/** mutable iterators */ /** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment 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) { bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG #ifndef NDEBUG
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); } BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
#endif #endif
bool parentChanged = false; bool parentChanged = false;
BOOST_FOREACH(Key& parent, parents()) { BOOST_FOREACH(KeyType& parent, parents()) {
Key newParent = inversePermutation[parent]; KeyType newParent = inversePermutation[parent];
if(parent != newParent) { if(parent != newParent) {
parentChanged = true; parentChanged = true;
parent = newParent; parent = newParent;
@ -61,8 +61,8 @@ namespace gtsam {
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) { void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals // The permutation may not move the separators into the frontals
#ifndef NDEBUG #ifndef NDEBUG
BOOST_FOREACH(const Key frontal, this->frontals()) { BOOST_FOREACH(const KeyType frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) { BOOST_FOREACH(const KeyType separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]); assert(inversePermutation[frontal] < inversePermutation[separator]);
} }
} }

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

@ -37,11 +37,11 @@ private:
Map values_; Map values_;
public: public:
typedef pair<Symbol, T> value_type; typedef pair<Key, T> value_type;
SymbolMapExp() {} SymbolMapExp() {}
T& at(const Symbol& key) { T& at(Key key) {
typename Map::iterator it = values_.find(key.chr()); typename Map::iterator it = values_.find(key.chr());
if(it != values_.end()) if(it != values_.end())
return it->second.at(key.index()); return it->second.at(key.index());
@ -49,7 +49,7 @@ public:
throw invalid_argument("Key " + (string)key + " not present"); 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& vec(values_[key.chr()]);
//vec.reserve(10000); //vec.reserve(10000);
if(key.index() >= vec.size()) { if(key.index() >= vec.size()) {
@ -62,13 +62,13 @@ public:
}; };
template<class T> template<class T>
class SymbolMapBinary : public std::map<Symbol, T> { class SymbolMapBinary : public std::map<Key, T> {
private: private:
typedef std::map<Symbol, T> Base; typedef std::map<Key, T> Base;
public: 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); typename Base::iterator it = Base::find(key);
if (it == Base::end()) if (it == Base::end())
throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key)); 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> { struct SymbolHash : public std::unary_function<Key, std::size_t> {
std::size_t operator()(Symbol const& x) const { std::size_t operator()(Key const& x) const {
std::size_t seed = 0; std::size_t seed = 0;
boost::hash_combine(seed, x.chr()); boost::hash_combine(seed, x.chr());
boost::hash_combine(seed, x.index()); boost::hash_combine(seed, x.index());
@ -86,9 +86,9 @@ struct SymbolHash : public std::unary_function<Symbol, std::size_t> {
}; };
template<class T> template<class T>
class SymbolMapHash : public boost::unordered_map<Symbol, T, SymbolHash> { class SymbolMapHash : public boost::unordered_map<Key, T, SymbolHash> {
public: public:
SymbolMapHash() : boost::unordered_map<Symbol, T, SymbolHash>(60000) {} SymbolMapHash() : boost::unordered_map<Key, T, SymbolHash>(60000) {}
}; };
struct Value { struct Value {
@ -107,7 +107,7 @@ int main(int argc, char *argv[]) {
// pre-allocate // pre-allocate
cout << "Generating test data ..." << endl; cout << "Generating test data ..." << endl;
vector<pair<Symbol, Value> > values; vector<pair<Key, Value> > values;
for(size_t i=0; i<ELEMS; i++) { for(size_t i=0; i<ELEMS; i++) {
values.push_back(make_pair(Symbol('a',i), (double)i)); values.push_back(make_pair(Symbol('a',i), (double)i));
values.push_back(make_pair(Symbol('b',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 // make the ordering
list<KEY> keys = predecessorMap2Keys(tree_); 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 // build factor pairs
pairs_.clear(); pairs_.clear();
typedef pair<KEY,KEY> EG ; typedef pair<KEY,KEY> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) { BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first), Key key1 = Key(eg.first),
key2 = Symbol(eg.second) ; key2 = Key(eg.second) ;
pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ; 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> template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_( typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
const Values& linearizationPoints, const Symbol& lastKey, const Values& linearizationPoints, Key lastKey,
JacobianFactor::shared_ptr& newPrior) const { JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key // 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 // different keys will still compute as if a common key-set was used
// Create Keys // Create Keys
Symbol x0 = motionFactor.key1(); Key x0 = motionFactor.key1();
Symbol x1 = motionFactor.key2(); Key x1 = motionFactor.key2();
// Create an elimination ordering // Create an elimination ordering
Ordering ordering; Ordering ordering;
@ -131,7 +131,7 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used // different keys will still compute as if a common key-set was used
// Create Keys // Create Keys
Symbol x0 = measurementFactor.key(); Key x0 = measurementFactor.key();
// Create an elimination ordering // Create an elimination ordering
Ordering ordering; Ordering ordering;

2
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -55,7 +55,7 @@ namespace gtsam {
T solve_(const GaussianFactorGraph& linearFactorGraph, T solve_(const GaussianFactorGraph& linearFactorGraph,
const Ordering& ordering, const Values& linearizationPoints, const Ordering& ordering, const Values& linearizationPoints,
const Symbol& x, JacobianFactor::shared_ptr& newPrior) const; Key x, JacobianFactor::shared_ptr& newPrior) const;
public: 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> optimize2(const GaussianISAM2::sharedClique& root) {
// boost::shared_ptr<VectorValues> delta(new VectorValues()); // boost::shared_ptr<VectorValues> delta(new VectorValues());
// set<Symbol> changed; // set<Key> changed;
// // starting from the root, call optimize on each conditional // // starting from the root, call optimize on each conditional
// optimize2(root, delta); // optimize2(root, delta);
// return 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> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> indices; FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { 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]); 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"); tic(3,"check candidates");
BOOST_FOREACH(size_t idx, candidates) { BOOST_FOREACH(size_t idx, candidates) {
bool inside = true; bool inside = true;
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) { BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
Index var = ordering_[key]; Index var = ordering_[key];
if (affectedKeysSet.find(var) == affectedKeysSet.end()) { if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
inside = false; inside = false;
@ -398,7 +398,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
template<class CONDITIONAL, class GRAPH> template<class CONDITIONAL, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update( ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices, 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 debug = ISDEBUG("ISAM2 update");
static const bool verbose = ISDEBUG("ISAM2 update verbose"); static const bool verbose = ISDEBUG("ISAM2 update verbose");
@ -519,7 +519,7 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
boost::optional<FastSet<Index> > constrainedIndices; boost::optional<FastSet<Index> > constrainedIndices;
if(constrainedKeys) { if(constrainedKeys) {
constrainedIndices.reset(FastSet<Index>()); constrainedIndices.reset(FastSet<Index>());
BOOST_FOREACH(const Symbol& key, *constrainedKeys) { BOOST_FOREACH(Key key, *constrainedKeys) {
constrainedIndices->insert(ordering_[key]); 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(), ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(), 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); bool force_relinearize = false);
/** Access the current linearization point */ /** Access the current linearization point */

10
gtsam/nonlinear/NonlinearEquality.h
View File

@ -89,7 +89,7 @@ namespace gtsam {
/** /**
* Constructor - forces exact evaluation * 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), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(0.0), allow_error_(false), error_gain_(0.0),
compare_(_compare) { compare_(_compare) {
@ -98,7 +98,7 @@ namespace gtsam {
/** /**
* Constructor - allows inexact evaluation * 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), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain), allow_error_(true), error_gain_(error_gain),
compare_(_compare) { compare_(_compare) {
@ -109,7 +109,7 @@ namespace gtsam {
/// @{ /// @{
virtual void print(const std::string& s = "") const { 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"); gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
} }
@ -204,7 +204,7 @@ namespace gtsam {
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
///TODO: comment ///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) {} : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
virtual ~NonlinearEquality1() {} virtual ~NonlinearEquality1() {}
@ -259,7 +259,7 @@ namespace gtsam {
typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
///TODO: comment ///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) {} : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {} virtual ~NonlinearEquality2() {}

61
gtsam/nonlinear/NonlinearFactor.h
View File

@ -32,6 +32,7 @@
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
namespace gtsam { namespace gtsam {
@ -44,12 +45,12 @@ namespace gtsam {
* which are objects in non-linear manifolds (Lie groups). * which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping * \nosubgrouping
*/ */
class NonlinearFactor: public Factor<Symbol> { class NonlinearFactor: public Factor<Key> {
protected: protected:
// Some handy typedefs // Some handy typedefs
typedef Factor<Symbol> Base; typedef Factor<Key> Base;
typedef NonlinearFactor This; typedef NonlinearFactor This;
public: public:
@ -187,7 +188,7 @@ public:
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": NoiseModelFactor\n"; std::cout << s << ": NoiseModelFactor\n";
std::cout << " "; 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"; std::cout << "\n";
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -310,14 +311,14 @@ public:
virtual ~NonlinearFactor1() {} virtual ~NonlinearFactor1() {}
inline const Symbol& key() const { return keys_[0]; } inline Key key() const { return keys_[0]; }
/** /**
* Constructor * Constructor
* @param z measurement * @param z measurement
* @param key by which to look up X value in Values * @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) { Base(noiseModel) {
keys_.resize(1); keys_.resize(1);
keys_[0] = key1; keys_[0] = key1;
@ -393,7 +394,7 @@ public:
* @param j1 key of the first variable * @param j1 key of the first variable
* @param j2 key of the second 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) { Base(noiseModel) {
keys_.resize(2); keys_.resize(2);
keys_[0] = j1; keys_[0] = j1;
@ -403,8 +404,8 @@ public:
virtual ~NonlinearFactor2() {} virtual ~NonlinearFactor2() {}
/** methods to retrieve both keys */ /** methods to retrieve both keys */
inline const Symbol& key1() const { return keys_[0]; } inline Key key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; } inline Key key2() const { return keys_[1]; }
/** Calls the 2-key specific version of evaluateError, which is pure virtual /** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
@ -481,7 +482,7 @@ public:
* @param j2 key of the second variable * @param j2 key of the second variable
* @param j3 key of the third 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) { Base(noiseModel) {
keys_.resize(3); keys_.resize(3);
keys_[0] = j1; keys_[0] = j1;
@ -492,9 +493,9 @@ public:
virtual ~NonlinearFactor3() {} virtual ~NonlinearFactor3() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; } inline Key key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; } inline Key key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; } inline Key key3() const { return keys_[2]; }
/** Calls the 3-key specific version of evaluateError, which is pure virtual /** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
@ -574,7 +575,7 @@ public:
* @param j3 key of the third variable * @param j3 key of the third variable
* @param j4 key of the fourth 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) { Base(noiseModel) {
keys_.resize(4); keys_.resize(4);
keys_[0] = j1; keys_[0] = j1;
@ -586,10 +587,10 @@ public:
virtual ~NonlinearFactor4() {} virtual ~NonlinearFactor4() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; } inline Key key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; } inline Key key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; } inline Key key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; } inline Key key4() const { return keys_[3]; }
/** Calls the 4-key specific version of evaluateError, which is pure virtual /** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
@ -672,7 +673,7 @@ public:
* @param j4 key of the fourth variable * @param j4 key of the fourth variable
* @param j5 key of the fifth 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) { Base(noiseModel) {
keys_.resize(5); keys_.resize(5);
keys_[0] = j1; keys_[0] = j1;
@ -685,11 +686,11 @@ public:
virtual ~NonlinearFactor5() {} virtual ~NonlinearFactor5() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; } inline Key key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; } inline Key key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; } inline Key key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; } inline Key key4() const { return keys_[3]; }
inline const Symbol& key5() const { return keys_[4]; } inline Key key5() const { return keys_[4]; }
/** Calls the 5-key specific version of evaluateError, which is pure virtual /** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
@ -776,7 +777,7 @@ public:
* @param j5 key of the fifth variable * @param j5 key of the fifth variable
* @param j6 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) { Base(noiseModel) {
keys_.resize(6); keys_.resize(6);
keys_[0] = j1; keys_[0] = j1;
@ -790,12 +791,12 @@ public:
virtual ~NonlinearFactor6() {} virtual ~NonlinearFactor6() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const Symbol& key1() const { return keys_[0]; } inline Key key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; } inline Key key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; } inline Key key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; } inline Key key4() const { return keys_[3]; }
inline const Symbol& key5() const { return keys_[4]; } inline Key key5() const { return keys_[4]; }
inline const Symbol& key6() const { return keys_[5]; } inline Key key6() const { return keys_[5]; }
/** Calls the 6-key specific version of evaluateError, which is pure virtual /** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * 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<Key> NonlinearFactorGraph::keys() const {
std::set<Symbol> keys; std::set<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor) if(factor)
keys.insert(factor->begin(), factor->end()); 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; void print(const std::string& str = "NonlinearFactorGraph: ") const;
/** return keys in some random order */ /** return keys in some random order */
std::set<Symbol> keys() const; std::set<Key> keys() const;
/** unnormalized error */ /** unnormalized error */
double error(const Values& c) const; 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 // Augment ordering
// FIXME: should just loop over new values // FIXME: should just loop over new values
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) 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 ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors( boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
@ -99,7 +99,7 @@ Values NonlinearISAM<GRAPH>::estimate() const {
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> template<class GRAPH>
Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const { Matrix NonlinearISAM<GRAPH>::marginalCovariance(Key key) const {
return isam_.marginalCovariance(ordering_[key]); return isam_.marginalCovariance(ordering_[key]);
} }

4
gtsam/nonlinear/NonlinearISAM.h
View File

@ -70,7 +70,7 @@ public:
Values estimate() const; Values estimate() const;
/** find the marginal covariance for a single variable */ /** find the marginal covariance for a single variable */
Matrix marginalCovariance(const Symbol& key) const; Matrix marginalCovariance(Key key) const;
// access // access
@ -104,7 +104,7 @@ public:
void reorder_relinearize(); void reorder_relinearize();
/** manually add a key to the end of the ordering */ /** 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 */ /** replace the current ordering */
void setOrdering(const Ordering& new_ordering) { ordering_ = new_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 * Return mean and covariance on a single variable
*/ */
Matrix marginalCovariance(Symbol j) const { Matrix marginalCovariance(Key j) const {
return createSolver()->marginalCovariance((*ordering_)[j]); return createSolver()->marginalCovariance((*ordering_)[j]);
} }

6
gtsam/nonlinear/Ordering.cpp
View File

@ -26,9 +26,9 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Ordering::Ordering(const std::list<Symbol> & L):nVars_(0) { Ordering::Ordering(const std::list<Key> & L):nVars_(0) {
int i = 0; int i = 0;
BOOST_FOREACH( const Symbol& s, L ) BOOST_FOREACH( Key s, L )
insert(s, i++) ; 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); Map::iterator item = order_.find(key);
if(item == order_.end()) { if(item == order_.end()) {
throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key"); 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 #pragma once
#include <map>
#include <set> #include <set>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/base/FastMap.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -34,8 +33,7 @@ namespace gtsam {
*/ */
class Ordering { class Ordering {
protected: protected:
typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator; typedef FastMap<Key, Index> Map;
typedef std::map<Symbol, Index, std::less<Symbol>, Allocator> Map;
Map order_; Map order_;
Index nVars_; Index nVars_;
@ -43,7 +41,7 @@ public:
typedef boost::shared_ptr<Ordering> shared_ptr; 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::iterator iterator;
typedef Map::const_iterator const_iterator; typedef Map::const_iterator const_iterator;
@ -54,7 +52,7 @@ public:
Ordering() : nVars_(0) {} Ordering() : nVars_(0) {}
/// Construct from list, assigns order indices sequentially to list items. /// 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 /// @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 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! */ 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 /** 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 * 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. * @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. * @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); const_iterator i = order_.find(key);
if(i != order_.end()) { if(i != order_.end()) {
index = i->second; index = i->second;
@ -91,7 +89,7 @@ public:
/// Access the index for the requested key, throws std::out_of_range if the /// 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 /// key is not present in the ordering (note that this differs from the
/// behavior of std::map) /// behavior of std::map)
Index& operator[](const Symbol& key) { Index& operator[](Key key) {
iterator i=order_.find(key); iterator i=order_.find(key);
if(i == order_.end()) throw std::out_of_range(std::string()); if(i == order_.end()) throw std::out_of_range(std::string());
else return i->second; } else return i->second; }
@ -99,7 +97,7 @@ public:
/// Access the index for the requested key, throws std::out_of_range if the /// 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 /// key is not present in the ordering (note that this differs from the
/// behavior of std::map) /// behavior of std::map)
Index operator[](const Symbol& key) const { Index operator[](Key key) const {
const_iterator i=order_.find(key); const_iterator i=order_.find(key);
if(i == order_.end()) throw std::out_of_range(std::string()); if(i == order_.end()) throw std::out_of_range(std::string());
else return i->second; } else return i->second; }
@ -110,7 +108,7 @@ public:
* @return An iterator pointing to the symbol/index pair with the requested, * @return An iterator pointing to the symbol/index pair with the requested,
* or the end iterator if it does not exist. * 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, /** Returns an iterator pointing to the symbol/index pair with the requested,
* or the end iterator if it does not exist. * 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, * @return An iterator pointing to the symbol/index pair with the requested,
* or the end iterator if it does not exist. * 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(), * Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
@ -153,22 +151,22 @@ public:
iterator end() { return order_.end(); } iterator end() { return order_.end(); }
/// Test if the key exists in the ordering. /// 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 ///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 ///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. /// 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 /** 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 * from the ordering (this version is \f$ O(n) \f$, use it when you do not
* know the last-ordered key). * 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. * that accepts this symbol as an argument.
* *
* @return The symbol and index that were removed. * @return The symbol and index that were removed.
@ -183,15 +181,15 @@ public:
* *
* @return The index of the symbol that was removed. * @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 * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
* very useful for unit tests. This functionality is courtesy of * very useful for unit tests. This functionality is courtesy of
* boost::assign. * boost::assign.
*/ */
inline boost::assign::list_inserter<boost::assign_detail::call_push_back<Ordering>, Symbol> inline boost::assign::list_inserter<boost::assign_detail::call_push_back<Ordering>, Key>
operator+=(const Symbol& key) { operator+=(Key key) {
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<Ordering>(*this))(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); void permuteWithInverse(const Permutation& inversePermutation);
/// Synonym for operator[](const Symbol&) /// Synonym for operator[](Key)
Index& at(const Symbol& key) { return operator[](key); } Index& at(Key key) { return operator[](key); }
/// @} /// @}
/// @name Testable /// @name Testable

24
gtsam/nonlinear/Symbol.h
View File

@ -87,6 +87,30 @@ public:
} }
#endif #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 // Testable Requirements
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl; std::cout << s << ": " << (std::string) (*this) << std::endl;

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

@ -43,11 +43,11 @@ namespace gtsam {
#if 0 #if 0
/* ************************************************************************* */ /* ************************************************************************* */
class ValueAutomaticCasting { class ValueAutomaticCasting {
const Symbol& key_; Key key_;
const Value& value_; const Value& value_;
public: 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> template<class ValueType>
class ConvertibleToValue : public ValueType { class ConvertibleToValue : public ValueType {
@ -67,7 +67,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ValueType> template<typename ValueType>
const ValueType& Values::at(const Symbol& j) const { const ValueType& Values::at(Key j) const {
// Find the item // Find the item
KeyValueMap::const_iterator item = values_.find(j); KeyValueMap::const_iterator item = values_.find(j);
@ -85,7 +85,7 @@ namespace gtsam {
#if 0 #if 0
/* ************************************************************************* */ /* ************************************************************************* */
inline ValueAutomaticCasting Values::at(const Symbol& j) const { inline ValueAutomaticCasting Values::at(Key j) const {
// Find the item // Find the item
KeyValueMap::const_iterator item = values_.find(j); KeyValueMap::const_iterator item = values_.find(j);
@ -97,26 +97,16 @@ namespace gtsam {
} }
#endif #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 #if 0
/* ************************************************************************* */ /* ************************************************************************* */
inline ValueAutomaticCasting Values::operator[](const Symbol& j) const { inline ValueAutomaticCasting Values::operator[](Key j) const {
return at(j); return at(j);
} }
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ValueType> 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 // Find the item
KeyValueMap::const_iterator item = values_.find(j); 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(); return values_.find(j) != values_.end();
} }
@ -79,7 +79,7 @@ namespace gtsam {
for(const_iterator key_value = begin(); key_value != end(); ++key_value) { for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this 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 Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values 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) { void Values::insert(Key j, const Value& val) {
Symbol key = j; // Non-const duplicate to deal with non-const insert argument Key key = j; // Non-const duplicate to deal with non-const insert argument
std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_()); std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
if(!insertResult.second) if(!insertResult.second)
throw ValuesKeyAlreadyExists(j); throw ValuesKeyAlreadyExists(j);
@ -117,13 +117,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void Values::insert(const Values& values) { void Values::insert(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { 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); 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 // Find the value to update
KeyValueMap::iterator item = values_.find(j); KeyValueMap::iterator item = values_.find(j);
if(item == values_.end()) 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); KeyValueMap::iterator item = values_.find(j);
if(item == values_.end()) if(item == values_.end())
throw ValuesKeyDoesNotExist("erase", j); throw ValuesKeyDoesNotExist("erase", j);
@ -152,8 +152,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastList<Symbol> Values::keys() const { FastList<Key> Values::keys() const {
FastList<Symbol> result; FastList<Key> result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) for(const_iterator key_value = begin(); key_value != end(); ++key_value)
result.push_back(key_value->first); result.push_back(key_value->first);
return result; return result;

81
gtsam/nonlinear/Values.h
View File

@ -37,7 +37,6 @@
#include <gtsam/base/Value.h> #include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
namespace gtsam { namespace gtsam {
@ -64,11 +63,11 @@ namespace gtsam {
// fast_pool_allocator to allocate map nodes. In this way, all memory is // fast_pool_allocator to allocate map nodes. In this way, all memory is
// allocated in a boost memory pool. // allocated in a boost memory pool.
typedef boost::ptr_map< typedef boost::ptr_map<
Symbol, Key,
Value, Value,
std::less<Symbol>, std::less<Key>,
ValueCloneAllocator, 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 // The member to store the values, see just above
KeyValueMap values_; 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 /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
struct ConstKeyValuePair { struct ConstKeyValuePair {
const Symbol& first; const Key first;
const Value& second; 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 /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
struct KeyValuePair { struct KeyValuePair {
const Symbol& first; const Key first;
Value& second; 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 /// Mutable forward iterator, with value type KeyValuePair
@ -137,7 +136,7 @@ namespace gtsam {
* @return A const reference to the stored value * @return A const reference to the stored value
*/ */
template<typename ValueType> template<typename ValueType>
const ValueType& at(const Symbol& j) const; const ValueType& at(Key j) const;
#if 0 #if 0
/** Retrieve a variable by key \c j. This non-templated version returns a /** 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 * @return A ValueAutomaticCasting object that may be assignmed to a Value
* of the proper type. * of the proper type.
*/ */
ValueAutomaticCasting at(const Symbol& j) const; ValueAutomaticCasting at(Key j) const;
#endif #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 #if 0
/** operator[] syntax for at(const Symbol& j) */ /** operator[] syntax for at(Key j) */
ValueAutomaticCasting operator[](const Symbol& j) const; ValueAutomaticCasting operator[](Key j) const;
#endif #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 * and exists(const TypedKey& j) for versions that return the value if it
* exists. */ * 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 /** 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. * \c Value if the key does exist, or boost::none if it does not exist.
* Throws DynamicValuesIncorrectType if the value type associated with the * Throws DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type. */ * requested key does not match the stored value type. */
template<typename ValueType> template<typename ValueType>
boost::optional<const ValueType&> exists(const Symbol& j) const; boost::optional<const ValueType&> 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.
* 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;
/** The number of variables in this config */ /** The number of variables in this config */
size_t size() const { return values_.size(); } 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 */ /** 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 */ /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const Values& values); void insert(const Values& values);
/** single element change of existing element */ /** 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 */ /** update the current available values without adding new ones */
void update(const Values& values); void update(const Values& values);
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */ /** 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 * Returns a set of keys in the config
* Note: by construction, the list is ordered * Note: by construction, the list is ordered
*/ */
FastList<Symbol> keys() const; FastList<Key> keys() const;
/** Replace all keys and variables */ /** Replace all keys and variables */
Values& operator=(const Values& rhs); Values& operator=(const Values& rhs);
@ -286,20 +259,20 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
class ValuesKeyAlreadyExists : public std::exception { class ValuesKeyAlreadyExists : public std::exception {
protected: protected:
const Symbol key_; ///< The key that already existed const Key key_; ///< The key that already existed
private: private:
mutable std::string message_; mutable std::string message_;
public: public:
/// Construct with the key-value pair attemped to be added /// Construct with the key-value pair attemped to be added
ValuesKeyAlreadyExists(const Symbol& key) throw() : ValuesKeyAlreadyExists(Key key) throw() :
key_(key) {} key_(key) {}
virtual ~ValuesKeyAlreadyExists() throw() {} virtual ~ValuesKeyAlreadyExists() throw() {}
/// The duplicate key that was attemped to be added /// 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 /// The message to be displayed to the user
virtual const char* what() const throw(); virtual const char* what() const throw();
@ -309,20 +282,20 @@ namespace gtsam {
class ValuesKeyDoesNotExist : public std::exception { class ValuesKeyDoesNotExist : public std::exception {
protected: protected:
const char* operation_; ///< The operation that attempted to access the key 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: private:
mutable std::string message_; mutable std::string message_;
public: public:
/// Construct with the key that does not exist in the values /// 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) {} operation_(operation), key_(key) {}
virtual ~ValuesKeyDoesNotExist() throw() {} virtual ~ValuesKeyDoesNotExist() throw() {}
/// The key that was attempted to be accessed that does not exist /// 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 /// The message to be displayed to the user
virtual const char* what() const throw(); virtual const char* what() const throw();
@ -331,7 +304,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
class ValuesIncorrectType : public std::exception { class ValuesIncorrectType : public std::exception {
protected: protected:
const Symbol key_; ///< The key requested const Key key_; ///< The key requested
const std::type_info& storedTypeId_; const std::type_info& storedTypeId_;
const std::type_info& requestedTypeId_; const std::type_info& requestedTypeId_;
@ -340,14 +313,14 @@ namespace gtsam {
public: public:
/// Construct with the key that does not exist in the values /// 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() : const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {} key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
virtual ~ValuesIncorrectType() throw() {} virtual ~ValuesIncorrectType() throw() {}
/// The key that was attempted to be accessed that does not exist /// 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 /// The typeid of the value stores in the Values
const std::type_info& storedTypeId() const { return storedTypeId_; } 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; using namespace std;
static double inf = std::numeric_limits<double>::infinity(); 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 ) TEST( Values, equals1 )
@ -218,12 +218,12 @@ TEST(Values, extract_keys)
config.insert("x4", Pose2()); config.insert("x4", Pose2());
config.insert("x5", Pose2()); config.insert("x5", Pose2());
FastList<Symbol> expected, actual; FastList<Key> expected, actual;
expected += "x1", "x2", "x4", "x5"; expected += "x1", "x2", "x4", "x5";
actual = config.keys(); actual = config.keys();
CHECK(actual.size() == expected.size()); 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) { for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
CHECK(assert_equal(*itExp, *itAct)); CHECK(assert_equal(*itExp, *itAct));
} }

2
gtsam/slam/BearingFactor.h
View File

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

2
gtsam/slam/BearingRangeFactor.h
View File

@ -50,7 +50,7 @@ namespace gtsam {
public: public:
BearingRangeFactor() {} /* Default constructor */ 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) : const SharedNoiseModel& model) :
Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) { Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) {
} }

4
gtsam/slam/BetweenFactor.h
View File

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

4
gtsam/slam/BoundingConstraint.h
View File

@ -37,7 +37,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
double threshold_; double threshold_;
bool isGreaterThan_; /// flag for greater/less than bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(const Symbol& key, double threshold, BoundingConstraint1(Key key, double threshold,
bool isGreaterThan, double mu = 1000.0) : bool isGreaterThan, double mu = 1000.0) :
Base(noiseModel::Constrained::All(1, mu), key), Base(noiseModel::Constrained::All(1, mu), key),
threshold_(threshold), isGreaterThan_(isGreaterThan) { threshold_(threshold), isGreaterThan_(isGreaterThan) {
@ -106,7 +106,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUE1, VALUE2> {
double threshold_; double threshold_;
bool isGreaterThan_; /// flag for greater/less than 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) bool isGreaterThan, double mu = 1000.0)
: Base(noiseModel::Constrained::All(1, mu), key1, key2), : Base(noiseModel::Constrained::All(1, mu), key1, key2),
threshold_(threshold), isGreaterThan_(isGreaterThan) {} 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 i is basically the frame number
* @param j is the index of the landmark * @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) {} Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor 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 * constructor with just minimum requirements for a factor - allows more
* computation in the constructor. This should only be used by subclasses * 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) {} : Base(model, key) {}
public: public:
@ -71,14 +71,14 @@ namespace gtsam {
virtual ~PartialPriorFactor() {} virtual ~PartialPriorFactor() {}
/** Single Element Constructor: acts on a single parameter specified by idx */ /** 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())) { Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
assert(model->dim() == 1); assert(model->dim() == 1);
this->fillH(); this->fillH();
} }
/** Indices Constructor: specify the mask with a set of indices */ /** 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) : const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
assert((size_t)prior_.size() == mask.size()); assert((size_t)prior_.size() == mask.size());

2
gtsam/slam/PriorFactor.h
View File

@ -52,7 +52,7 @@ namespace gtsam {
virtual ~PriorFactor() {} virtual ~PriorFactor() {}
/** Constructor */ /** 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) { 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 * @param K shared pointer to the constant calibration
*/ */
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model, 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) { 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() {} /* Default constructor */
RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured, RangeFactor(Key poseKey, Key pointKey, double measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, poseKey, pointKey), measured_(measured) { 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 landmarkKey the landmark variable key
* @param K the constant calibration * @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) { 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 Pose measured_; ///< prior mean
/// Create generic prior /// 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) { Base(model, key), measured_(z) {
} }
@ -166,7 +166,7 @@ namespace simulated2D {
Pose measured_; ///< odometry measurement Pose measured_; ///< odometry measurement
/// Create odometry /// 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) { Base(model, key1, key2), measured_(measured) {
} }
@ -206,7 +206,7 @@ namespace simulated2D {
Landmark measured_; ///< Measurement Landmark measured_; ///< Measurement
/// Create measurement factor /// 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) { 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 isGreaterThan is a flag to set inequality as greater than or less than
* @param mu is the penalty function gain * @param mu is the penalty function gain
*/ */
ScalarCoordConstraint1(const Symbol& key, double c, ScalarCoordConstraint1(Key key, double c,
bool isGreaterThan, double mu = 1000.0) : bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) { Base(key, c, isGreaterThan, mu) {
} }
@ -127,7 +127,7 @@ namespace simulated2D {
* @param range_bound is the maximum range allowed between the variables * @param range_bound is the maximum range allowed between the variables
* @param mu is the gain for the penalty function * @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) {} 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 range_bound is the minimum range allowed between the variables
* @param mu is the gain for the penalty function * @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) double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {} : Base(key1, key2, range_bound, true, mu) {}

4
gtsam/slam/simulated2DOriented.h
View File

@ -83,7 +83,7 @@ namespace simulated2DOriented {
Pose2 measured_; ///< measurement Pose2 measured_; ///< measurement
/// Create generic pose prior /// 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) { NonlinearFactor1<VALUE>(model, key), measured_(measured) {
} }
@ -106,7 +106,7 @@ namespace simulated2DOriented {
* Creates an odometry factor between two poses * Creates an odometry factor between two poses
*/ */
GenericOdometry(const Pose2& measured, const SharedNoiseModel& model, 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) { 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 model is the measurement model for the factor (Dimension: 3)
* @param key is the key for the pose * @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) { 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 * @param pointKey is the point key for the landmark
*/ */
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, 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) {} NonlinearFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
/** /**

2
gtsam/slam/smallExample.cpp
View File

@ -202,7 +202,7 @@ namespace example {
Point2 z_; 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) { gtsam::NonlinearFactor1<Point2>(model, key), z_(z) {
} }

4
tests/testNonlinearFactorGraph.cpp
View File

@ -64,9 +64,9 @@ TEST( Graph, error )
TEST( Graph, keys ) TEST( Graph, keys )
{ {
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
set<Symbol> actual = fg.keys(); set<Key> actual = fg.keys();
LONGS_EQUAL(3, actual.size()); 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('l', 1), *(it++)));
CHECK(assert_equal(Symbol('x', 1), *(it++))); CHECK(assert_equal(Symbol('x', 1), *(it++)));
CHECK(assert_equal(Symbol('x', 2), *(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(); Ordering ordering = isam.getOrdering();
// swap last two elements // swap last two elements
Symbol last = ordering.pop_back().first; Key last = ordering.pop_back().first;
Symbol secondLast = ordering.pop_back().first; Key secondLast = ordering.pop_back().first;
ordering.push_back(last); ordering.push_back(last);
ordering.push_back(secondLast); ordering.push_back(secondLast);
isam.setOrdering(ordering); isam.setOrdering(ordering);