diff --git a/gtsam/base/types.h b/gtsam/base/types.h index b6c5617a5..f3f1ae465 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -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. diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 2fa5f691d..0b5fca660 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -46,7 +46,7 @@ private: /** Create keys by adding key in front */ template static std::vector MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) { - std::vector keys((lastParent - firstParent) + 1); + std::vector 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 This; - typedef Factor Base; + typedef KEY KeyType; + typedef Conditional This; + typedef Factor 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 FactorType; + typedef gtsam::Factor FactorType; /** A shared_ptr to this class. Derived classes must redefine this. */ typedef boost::shared_ptr 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& parents) : + Conditional(KeyType key, const std::vector& 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 void Conditional::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; } diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index 09336186e..d05cc1de3 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -45,8 +45,8 @@ namespace gtsam { void Factor::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( diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 5a0b1e333..962bf44d1 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -55,28 +55,28 @@ class Factor { public: - typedef KEY Key; ///< The KEY template parameter - typedef Factor This; ///< This class + typedef KEY KeyType; ///< The KEY template parameter + typedef Factor This; ///< This class /** * Typedef to the conditional type obtained by eliminating this factor, * derived classes must redefine this. */ - typedef Conditional ConditionalType; + typedef Conditional ConditionalType; /// A shared_ptr to this class, derived classes must redefine this. typedef boost::shared_ptr shared_ptr; /// Iterator over keys - typedef typename std::vector::iterator iterator; + typedef typename std::vector::iterator iterator; /// Const iterator over keys - typedef typename std::vector::const_iterator const_iterator; + typedef typename std::vector::const_iterator const_iterator; protected: /// The keys involved in this factor - std::vector keys_; + std::vector 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& keys) { - BOOST_FOREACH(const Key& key, keys) keys_.push_back(key); + Factor(const std::set& keys) { + BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key); assertInvariants(); } /** Construct n-way factor */ - Factor(const std::vector& keys) : keys_(keys) { + Factor(const std::vector& 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& keys() const { return keys_; } + const std::vector& 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& keys() { return keys_; } + std::vector& keys() { return keys_; } /** mutable iterators */ iterator begin() { return keys_.begin(); } ///TODO: comment diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index eca7a220d..58d5f93d1 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -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]); } } diff --git a/gtsam/inference/tests/timeSymbolMaps.cpp b/gtsam/inference/tests/timeSymbolMaps.cpp index 61ce1b30c..1912f34d3 100644 --- a/gtsam/inference/tests/timeSymbolMaps.cpp +++ b/gtsam/inference/tests/timeSymbolMaps.cpp @@ -37,11 +37,11 @@ private: Map values_; public: - typedef pair value_type; + typedef pair 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 SymbolMapBinary : public std::map { +class SymbolMapBinary : public std::map { private: - typedef std::map Base; + typedef std::map Base; public: - SymbolMapBinary() : std::map() {} + SymbolMapBinary() : std::map() {} - 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 { - std::size_t operator()(Symbol const& x) const { +struct SymbolHash : public std::unary_function { + 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 { }; template -class SymbolMapHash : public boost::unordered_map { +class SymbolMapHash : public boost::unordered_map { public: - SymbolMapHash() : boost::unordered_map(60000) {} + SymbolMapHash() : boost::unordered_map(60000) {} }; struct Value { @@ -107,7 +107,7 @@ int main(int argc, char *argv[]) { // pre-allocate cout << "Generating test data ..." << endl; - vector > values; + vector > values; for(size_t i=0; i::initialize(const GRAPH& G, const Values& // make the ordering list keys = predecessorMap2Keys(tree_); - ordering_ = boost::make_shared(list(keys.begin(), keys.end())); + ordering_ = boost::make_shared(list(keys.begin(), keys.end())); // build factor pairs pairs_.clear(); typedef pair 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((*ordering_)[key1], (*ordering_)[key2])) ; } } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 7cd67f6d2..06a24d473 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -30,7 +30,7 @@ namespace gtsam { template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::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; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index bdfaa7651..37d7d216b 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -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: diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h index d0d37d886..de3992d28 100644 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ b/gtsam/nonlinear/GaussianISAM2-inl.h @@ -121,7 +121,7 @@ namespace gtsam { ///* ************************************************************************* */ //boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { // boost::shared_ptr delta(new VectorValues()); - // set changed; + // set changed; // // starting from the root, call optimize on each conditional // optimize2(root, delta); // return delta; diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index eae39bf52..0abea8e7f 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -155,7 +155,7 @@ template FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet 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]); } } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 5c87d1dda..a054eb179 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -105,7 +105,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& 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 > ISAM2::recalculate( template ISAM2Result ISAM2::update( const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, - const boost::optional >& constrainedKeys, bool force_relinearize) { + const boost::optional >& 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::update( boost::optional > constrainedIndices; if(constrainedKeys) { constrainedIndices.reset(FastSet()); - BOOST_FOREACH(const Symbol& key, *constrainedKeys) { + BOOST_FOREACH(Key key, *constrainedKeys) { constrainedIndices->insert(ordering_[key]); } } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 00d07a040..505aa2de5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -369,7 +369,7 @@ public: */ ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), - const boost::optional >& constrainedKeys = boost::none, + const boost::optional >& constrainedKeys = boost::none, bool force_relinearize = false); /** Access the current linearization point */ diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 1a1fce839..9c34069f5 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -89,7 +89,7 @@ namespace gtsam { /** * Constructor - forces exact evaluation */ - NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : + NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : 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) : + NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : 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 > 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 > 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() {} diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 5ebe662e5..db1de6740 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -32,6 +32,7 @@ #include #include +#include namespace gtsam { @@ -44,12 +45,12 @@ namespace gtsam { * which are objects in non-linear manifolds (Lie groups). * \nosubgrouping */ -class NonlinearFactor: public Factor { +class NonlinearFactor: public Factor { protected: // Some handy typedefs - typedef Factor Base; + typedef Factor 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. */ diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 11f8e84d9..0c005ce3e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -49,8 +49,8 @@ namespace gtsam { } /* ************************************************************************* */ - std::set NonlinearFactorGraph::keys() const { - std::set keys; + std::set NonlinearFactorGraph::keys() const { + std::set keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index fa87baedf..c2cd844be 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -47,7 +47,7 @@ namespace gtsam { void print(const std::string& str = "NonlinearFactorGraph: ") const; /** return keys in some random order */ - std::set keys() const; + std::set keys() const; /** unnormalized error */ double error(const Values& c) const; diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 03d2832ce..e054ecacb 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -50,7 +50,7 @@ void NonlinearISAM::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 linearizedNewFactors( @@ -99,7 +99,7 @@ Values NonlinearISAM::estimate() const { /* ************************************************************************* */ template -Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { +Matrix NonlinearISAM::marginalCovariance(Key key) const { return isam_.marginalCovariance(ordering_[key]); } diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index f74cff5ea..16547770b 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -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; } diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 5a2caed64..4e41649e6 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -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]); } diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index d8b5eac7e..c35b07f43 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -26,9 +26,9 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Ordering::Ordering(const std::list & L):nVars_(0) { +Ordering::Ordering(const std::list & 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"); diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 47d048f2b..084d69ebd 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -17,9 +17,8 @@ #pragma once -#include #include -#include +#include #include #include @@ -34,8 +33,7 @@ namespace gtsam { */ class Ordering { protected: - typedef boost::fast_pool_allocator > Allocator; - typedef std::map, Allocator> Map; + typedef FastMap Map; Map order_; Index nVars_; @@ -43,7 +41,7 @@ public: typedef boost::shared_ptr shared_ptr; - typedef std::pair value_type; + typedef std::pair 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 & L) ; + Ordering(const std::list & 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 tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); } + std::pair 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, Symbol> - operator+=(const Symbol& key) { + inline boost::assign::list_inserter, Key> + operator+=(Key key) { return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*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 diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index f2963dc32..a59e864a3 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -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::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::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; diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index c79d82a18..dd5b29fbd 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -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 ConvertibleToValue : public ValueType { @@ -67,7 +67,7 @@ namespace gtsam { /* ************************************************************************* */ template - 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 - 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(symbol); - } - #if 0 /* ************************************************************************* */ - inline ValueAutomaticCasting Values::operator[](const Symbol& j) const { + inline ValueAutomaticCasting Values::operator[](Key j) const { return at(j); } #endif /* ************************************************************************* */ template - boost::optional Values::exists(const Symbol& j) const { + boost::optional Values::exists(Key j) const { // Find the item KeyValueMap::const_iterator item = values_.find(j); @@ -132,14 +122,4 @@ namespace gtsam { } } - /* ************************************************************************* */ - template - boost::optional 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(symbol); - } - } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 44245c39c..4c8d03d9d 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -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 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 Values::keys() const { - FastList result; + FastList Values::keys() const { + FastList result; for(const_iterator key_value = begin(); key_value != end(); ++key_value) result.push_back(key_value->first); return result; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 61e11ca4e..5074c2360 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -37,7 +37,6 @@ #include #include #include -#include #include 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, + std::less, ValueCloneAllocator, - boost::fast_pool_allocator > > KeyValueMap; + boost::fast_pool_allocator > > 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 - 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. - * Symbol(const TypedKey&). 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 - const typename TypedKey::Value& at(const TypedKey& j) const; - - /** operator[] syntax for at(const TypedKey& j) */ - template - 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 - boost::optional 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. Symbol(const TypedKey&). Throws - * DynamicValuesIncorrectType if the value type associated with the - * requested key does not match the stored value type. - */ - template - boost::optional exists(const TypedKey& j) const; + boost::optional 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 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 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 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 keys() const; + FastList 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_; } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index bb68ef9e6..77e1155a7 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -31,7 +31,7 @@ using namespace gtsam; using namespace std; static double inf = std::numeric_limits::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 expected, actual; + FastList expected, actual; expected += "x1", "x2", "x4", "x5"; actual = config.keys(); CHECK(actual.size() == expected.size()); - FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); + FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { CHECK(assert_equal(*itExp, *itAct)); } diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 70d19061e..b114d14b8 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -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) { } diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 4e442ad86..785b4c488 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -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) { } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 313241eee..9454984b2 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -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 > 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(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} private: diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 13ae33cfc..0decb3290 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -37,7 +37,7 @@ struct BoundingConstraint1: public NonlinearFactor1 { 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 { 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) {} diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 868b064d7..ac5d07f75 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -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 diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index e29893e77..91c71b1ad 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -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& mask, const Vector& prior, + PartialPriorFactor(Key key, const std::vector& 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()); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 43a33a9fe..1f8436b02 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -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) { } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index f654e5d33..805a991cd 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -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) { } diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 43cc3da57..9edc9693f 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -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) { } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 76359d8c9..a69f4189b 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -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) { } diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index bb9f143c5..cdcc2663a 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -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) { } diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 5a6b39861..b44062933 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -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) {} diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index f38e9c34d..47314f8cf 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -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(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(model, i1, i2), measured_(measured) { } diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index 9cd1ed74d..6a2ce8d26 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -74,7 +74,7 @@ struct PointPrior3D: public NonlinearFactor1 { * @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 (model, key), measured_(measured) { } @@ -106,7 +106,7 @@ struct Simulated3DMeasurement: public NonlinearFactor2 { * @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(model, poseKey, pointKey), measured_(measured) {} /** diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 4c13ab2b3..7c5b333ad 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -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(model, key), z_(z) { } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 938067508..da8d8a114 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -64,9 +64,9 @@ TEST( Graph, error ) TEST( Graph, keys ) { Graph fg = createNonlinearFactorGraph(); - set actual = fg.keys(); + set actual = fg.keys(); LONGS_EQUAL(3, actual.size()); - set::const_iterator it = actual.begin(); + set::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++))); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 09a3d4db0..80ed7b6b6 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -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);