From 4b2b9d815865e2bcf8fcdec5bf4507d82fc1d78e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 2 Feb 2012 22:06:33 +0000 Subject: [PATCH] Fixed Values iterators and removed apply function --- gtsam/nonlinear/ISAM2-impl-inl.h | 85 ++++++++++------------------- gtsam/nonlinear/Values-inl.h | 4 +- gtsam/nonlinear/Values.cpp | 46 +++++++--------- gtsam/nonlinear/Values.h | 94 ++++++++++++++------------------ 4 files changed, 94 insertions(+), 135 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index c37892654..eae39bf52 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -117,22 +117,6 @@ struct ISAM2::Impl { }; -/* ************************************************************************* */ -struct _VariableAdder { - Ordering& ordering; - Permuted& vconfig; - Index nextVar; - _VariableAdder(Ordering& _ordering, Permuted& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){} - template - void operator()(I xIt) { - const bool debug = ISDEBUG("ISAM2 AddVariables"); - vconfig.permutation()[nextVar] = nextVar; - ordering.insert(xIt->first, nextVar); - if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl; - ++ nextVar; - } -}; - /* ************************************************************************* */ template void ISAM2::Impl::AddVariables( @@ -151,8 +135,13 @@ void ISAM2::Impl::AddVariables( delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); delta.permutation().resize(originalnVars + newTheta.size()); { - _VariableAdder vadder(ordering, delta, originalnVars); - newTheta.apply(vadder); + Index nextVar = originalnVars; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + delta.permutation()[nextVar] = nextVar; + ordering.insert(key_value.first, nextVar); + if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl; + ++ nextVar; + } assert(delta.permutation().size() == delta.container().size()); assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); @@ -222,41 +211,6 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique:: } } -/* ************************************************************************* */ -// Internal class used in ExpmapMasked() -// This debug version sets delta entries that are applied to "Inf". The -// idea is that if a delta is applied, the variable is being relinearized, -// so the same delta should not be re-applied because it will be recalc- -// ulated. This is a debug check to prevent against a mix-up of indices -// or not keeping track of recalculated variables. -struct _SelectiveExpmapAndClear { - const Permuted& delta; - const Ordering& ordering; - const vector& mask; - const boost::optional&> invalidate; - _SelectiveExpmapAndClear(const Permuted& _delta, const Ordering& _ordering, const vector& _mask, boost::optional&> _invalidate) : - delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {} - template - void operator()(I it_x) { - Index var = ordering[it_x->first]; - if(ISDEBUG("ISAM2 update verbose")) { - if(mask[var]) - cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - else - cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - } - assert(delta[var].size() == (int)it_x->second->dim()); - assert(delta[var].unaryExpr(&isfinite).all()); - if(mask[var]) { - Value* retracted = it_x->second->retract_(delta[var]); - *it_x->second = *retracted; - retracted->deallocate_(); - if(invalidate) - (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) - } - } -}; - /* ************************************************************************* */ template void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, @@ -268,8 +222,29 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permute invalidateIfDebug = boost::optional&>(); #endif - _SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug); - values.apply(selectiveExpmapper); + assert(values.size() == ordering.size()); + Values::iterator key_value; + Ordering::const_iterator key_index; + for(key_value = values.begin(), key_index = ordering.begin(); + key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) { + assert(key_value->first == key_index->first); + const Index var = key_index->second; + if(ISDEBUG("ISAM2 update verbose")) { + if(mask[var]) + cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + else + cout << " " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + } + assert(delta[var].size() == (int)key_value->second.dim()); + assert(delta[var].unaryExpr(&isfinite).all()); + if(mask[var]) { + Value* retracted = key_value->second.retract_(delta[var]); + key_value->second = *retracted; + retracted->deallocate_(); + if(invalidateIfDebug) + (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) + } + } } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index b49049a02..76a1ce035 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -41,7 +41,7 @@ namespace gtsam { template const ValueType& Values::at(const Symbol& j) const { // Find the item - const_iterator item = values_.find(j); + KeyValueMap::const_iterator item = values_.find(j); // Throw exception if it does not exist if(item == values_.end()) @@ -69,7 +69,7 @@ namespace gtsam { template boost::optional Values::exists(const Symbol& j) const { // Find the item - const_iterator item = values_.find(j); + KeyValueMap::const_iterator item = values_.find(j); if(item != values_.end()) { // Check the type and throw exception if incorrect diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index c5190cbc8..2d8b72120 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -42,9 +42,9 @@ namespace gtsam { /* ************************************************************************* */ void Values::print(const string& str) const { cout << str << "Values with " << size() << " values:\n" << endl; - for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { cout << " " << (string)key_value->first << ": "; - key_value->second->print(""); + key_value->second.print(""); } } @@ -53,11 +53,11 @@ namespace gtsam { if(this->size() != other.size()) return false; for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { - if(typeid(*it1->second) != typeid(*it2->second)) + if(typeid(it1->second) != typeid(it2->second)) return false; if(it1->first != it2->first) return false; - if(!it1->second->equals_(*it2->second, tol)) + if(!it1->second.equals_(it2->second, tol)) return false; } return true; // We return false earlier if we find anything that does not match @@ -77,10 +77,10 @@ namespace gtsam { Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { Values result; - for(KeyValueMap::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 Symbol 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 } @@ -102,7 +102,7 @@ namespace gtsam { if(it1->first != it2->first) throw DynamicValuesMismatched(); // If keys do not match // Will throw a dynamic_cast exception if types do not match - result.insert(ordering[it1->first], it1->second->localCoordinates_(*it2->second)); + result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second)); } } @@ -116,16 +116,16 @@ namespace gtsam { /* ************************************************************************* */ void Values::insert(const Values& values) { - for(KeyValueMap::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 - insert(key, *key_value->second); + insert(key, key_value->second); } } /* ************************************************************************* */ void Values::update(const Symbol& j, const Value& val) { // Find the value to update - iterator item = values_.find(j); + KeyValueMap::iterator item = values_.find(j); if(item == values_.end()) throw ValuesKeyDoesNotExist("update", j); @@ -138,14 +138,14 @@ namespace gtsam { /* ************************************************************************* */ void Values::update(const Values& values) { - for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { - this->update(key_value->first, *key_value->second); + for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { + this->update(key_value->first, key_value->second); } } /* ************************************************************************* */ void Values::erase(const Symbol& j) { - iterator item = values_.find(j); + KeyValueMap::iterator item = values_.find(j); if(item == values_.end()) throw ValuesKeyDoesNotExist("erase", j); values_.erase(item); @@ -154,7 +154,7 @@ namespace gtsam { /* ************************************************************************* */ FastList Values::keys() const { FastList result; - for(KeyValueMap::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); return result; } @@ -168,23 +168,17 @@ namespace gtsam { /* ************************************************************************* */ vector Values::dims(const Ordering& ordering) const { -// vector result(values_.size()); -// // Transform with Value::dim(auto_ptr::get(KeyValuePair::second)) -// result.assign( -// boost::make_transform_iterator(values_.begin(), -// boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))), -// boost::make_transform_iterator(values_.end(), -// boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1)))); -// return result; - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; + vector result(values_.size()); + BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { + result[ordering[key_value.first]] = key_value.second.dim(); + } + return result; } /* ************************************************************************* */ Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { Ordering::shared_ptr ordering(new Ordering); - for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { ordering->insert(key_value->first, firstVar++); } return ordering; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 27da4d7e4..ae06daa74 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,6 +29,9 @@ #include #include +#include +#include +#include #include #include @@ -41,19 +44,6 @@ namespace gtsam { // Forward declarations class ValueCloneAllocator; - struct _ValuesDimensionCollector { - const Ordering& ordering; - std::vector dimensions; - _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {} - template void operator()(const I& key_value) { - Index var; - if(ordering.tryAt(key_value->first, var)) { - assert(var < dimensions.size()); - dimensions[var] = key_value->second->dim(); - } - } - }; - /** * A non-templated config holding any types of Manifold-group elements. A * values structure is a map from keys to values. It is used to specify the @@ -81,15 +71,33 @@ namespace gtsam { // The member to store the values, see just above KeyValueMap values_; - // Type obtained by iterating - typedef KeyValueMap::const_iterator::value_type KeyValuePair; + // Types obtained by iterating + typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; + typedef KeyValueMap::iterator::value_type KeyValuePtrPair; public: - typedef KeyValueMap::iterator iterator; - typedef KeyValueMap::const_iterator const_iterator; - typedef KeyValueMap::reverse_iterator reverse_iterator; - typedef KeyValueMap::const_reverse_iterator const_reverse_iterator; + /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator + typedef std::pair ConstKeyValuePair; + + /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator + typedef std::pair KeyValuePair; + + /// Mutable forward iterator, with value type KeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::iterator> iterator; + + /// Const forward iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::const_iterator> const_iterator; + + /// Mutable reverse iterator, with value type KeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::reverse_iterator> reverse_iterator; + + /// Const reverse iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; /** Default constructor creates an empty Values class */ Values() {} @@ -165,14 +173,21 @@ namespace gtsam { /** Get a zero VectorValues of the correct structure */ VectorValues zeroVectors(const Ordering& ordering) const; - const_iterator begin() const { return values_.begin(); } - const_iterator end() const { return values_.end(); } - iterator begin() { return values_.begin(); } - iterator end() { return values_.end(); } - const_reverse_iterator rbegin() const { return values_.rbegin(); } - const_reverse_iterator rend() const { return values_.rend(); } - reverse_iterator rbegin() { return values_.rbegin(); } - reverse_iterator rend() { return values_.rend(); } + private: + static std::pair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { + return std::make_pair(key_value.first, *key_value.second); } + static std::pair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { + return std::make_pair(key_value.first, *key_value.second); } + + public: + const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } + const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } + iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } + iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); } + const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } + const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); } + reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); } + reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); } /// @name Manifold Operations /// @{ @@ -188,8 +203,6 @@ namespace gtsam { ///@} - // imperative methods: - /** Add a variable with the given j, throws KeyAlreadyExists if j is already present */ void insert(const Symbol& j, const Value& val); @@ -205,12 +218,6 @@ namespace gtsam { /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ void erase(const Symbol& j); - /** Remove a variable from the config while returning the dimensionality of - * the removed element (normally not needed by user code), throws - * KeyDoesNotExist if j is not present. - */ - //void erase(const J& j, size_t& dim); - /** * Returns a set of keys in the config * Note: by construction, the list is ordered @@ -226,23 +233,6 @@ namespace gtsam { /** Create an array of variable dimensions using the given ordering */ std::vector dims(const Ordering& ordering) const; - /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - for(iterator it = begin(); it != end(); ++it) - operation(it); - } - - template - void apply(A& operation) const { - for(const_iterator it = begin(); it != end(); ++it) - operation(it); - } - /** * Generate a default ordering, simply in key sort order. To instead * create a fill-reducing ordering, use