Deprecated boost iterators in Values

examples/Pose3Localization.cpp

@@ -43,9 +43,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
@@ -74,10 +74,8 @@ int main(const int argc, const char* argv[]) {
 
   // Calculate and print marginal covariances for all variables
   Marginals marginals(*graph, result);
-  for (const auto key_value : result) {
+  for (const auto& key_pose : result.extract<Pose3>()) {
-    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+    std::cout << marginals.marginalCovariance(key_pose.first) << endl;
-    if (!p) continue;
-    std::cout << marginals.marginalCovariance(key_value.key) << endl;
   }
   return 0;
 }

examples/Pose3SLAMExample_changeKeys.cpp

@@ -55,14 +55,14 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
     // Additional: rewrite input with simplified keys 0,1,...
     Values simpleInitial;
-    for(const auto key_value: *initial) {
+    for (const auto k : initial->keys()) {
       Key key;
-      if(add)
+      if (add)
-        key = key_value.key + firstKey;
+        key = k + firstKey;
       else
-        key = key_value.key - firstKey;
+        key = k - firstKey;
 
-      simpleInitial.insert(key, initial->at(key_value.key));
+      simpleInitial.insert(key, initial->at(k));
     }
     NonlinearFactorGraph simpleGraph;
     for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {

examples/Pose3SLAMExample_g2o.cpp

@@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }

examples/SolverComparer.cpp

@@ -559,12 +559,12 @@ void runPerturb()
 
   // Perturb values
   VectorValues noise;
-  for(const Values::KeyValuePair key_val: initial)
+  for(const auto& key_dim: initial.dims())
   {
-    Vector noisev(key_val.value.dim());
+    Vector noisev(key_dim.second);
     for(Vector::Index i = 0; i < noisev.size(); ++i)
       noisev(i) = normal(rng);
-    noise.insert(key_val.key, noisev);
+    noise.insert(key_dim.first, noisev);
   }
   Values perturbed = initial.retract(noise);
 

gtsam/hybrid/HybridValues.h

@@ -102,7 +102,7 @@ class GTSAM_EXPORT HybridValues {
 
   /// Check whether a variable with key \c j exists in values.
   bool existsNonlinear(Key j) {
-    return (nonlinear_.find(j) != nonlinear_.end());
+    return nonlinear_.exists(j);
   };
 
   /// Check whether a variable with key \c j exists.

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -129,10 +129,7 @@ TEST(HybridBayesNet, evaluateHybrid) {
 TEST(HybridBayesNet, Choose) {
   Switching s(4);
 
-  Ordering ordering;
+  const Ordering ordering(s.linearizationPoint.keys());
-  for (auto&& kvp : s.linearizationPoint) {
-    ordering += kvp.key;
-  }
 
   HybridBayesNet::shared_ptr hybridBayesNet;
   HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
@@ -163,10 +160,7 @@ TEST(HybridBayesNet, Choose) {
 TEST(HybridBayesNet, OptimizeAssignment) {
   Switching s(4);
 
-  Ordering ordering;
+  const Ordering ordering(s.linearizationPoint.keys());
-  for (auto&& kvp : s.linearizationPoint) {
-    ordering += kvp.key;
-  }
 
   HybridBayesNet::shared_ptr hybridBayesNet;
   HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;

gtsam/nonlinear/ISAM2-impl.h

@@ -436,36 +436,6 @@ struct GTSAM_EXPORT UpdateImpl {
     }
   }
 
-  /**
-   * Apply expmap to the given values, but only for indices appearing in
-   * \c mask.  Values are expmapped in-place.
-   * \param mask Mask on linear indices, only \c true entries are expmapped
-   */
-  static void ExpmapMasked(const VectorValues& delta, const KeySet& mask,
-                           Values* theta) {
-    gttic(ExpmapMasked);
-    assert(theta->size() == delta.size());
-    Values::iterator key_value;
-    VectorValues::const_iterator key_delta;
-#ifdef GTSAM_USE_TBB
-    for (key_value = theta->begin(); key_value != theta->end(); ++key_value) {
-      key_delta = delta.find(key_value->key);
-#else
-    for (key_value = theta->begin(), key_delta = delta.begin();
-         key_value != theta->end(); ++key_value, ++key_delta) {
-      assert(key_value->key == key_delta->first);
-#endif
-      Key var = key_value->key;
-      assert(static_cast<size_t>(delta[var].size()) == key_value->value.dim());
-      assert(delta[var].allFinite());
-      if (mask.exists(var)) {
-        Value* retracted = key_value->value.retract_(delta[var]);
-        key_value->value = *retracted;
-        retracted->deallocate_();
-      }
-    }
-  }
-
   // Linearize new factors
   void linearizeNewFactors(const NonlinearFactorGraph& newFactors,
                            const Values& theta, size_t numNonlinearFactors,

gtsam/nonlinear/ISAM2.cpp

@@ -454,7 +454,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
       update.findFluid(roots_, relinKeys, &result.markedKeys, result.details());
       // 6. Update linearization point for marked variables:
       // \Theta_{J}:=\Theta_{J}+\Delta_{J}.
-      UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_);
+      theta_.retractMasked(delta_, relinKeys);
     }
     result.variablesRelinearized = result.markedKeys.size();
   }

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -285,8 +285,8 @@ static Scatter scatterFromValues(const Values& values) {
   scatter.reserve(values.size());
 
   // use "natural" ordering with keys taken from the initial values
-  for (const auto key_value : values) {
+  for (const auto& key_dim : values.dims()) {
-    scatter.add(key_value.key, key_value.value.dim());
+    scatter.add(key_dim.first, key_dim.second);
   }
 
   return scatter;

gtsam/nonlinear/Values-inl.h

@@ -24,12 +24,10 @@
 
 #pragma once
 
+#include <gtsam/nonlinear/Values.h>
+
 #include <utility>
 
-#include <boost/bind/bind.hpp>
-
-#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
-
 namespace gtsam {
 
 
@@ -95,13 +93,13 @@ namespace gtsam {
   std::map<Key, ValueType>
   Values::extract(const std::function<bool(Key)>& filterFcn) const {
     std::map<Key, ValueType> result;
-    for (const auto& key_value : *this) {
+    for (const auto& key_value : values_) {
       // Check if key matches
-      if (filterFcn(key_value.key)) {
+      if (filterFcn(key_value.first)) {
         // Check if type matches (typically does as symbols matched with types)
         if (auto t =
-                dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
+                dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
-          result[key_value.key] = t->value();
+          result[key_value.first] = t->value();
       }
     }
     return result;
@@ -109,6 +107,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+#include <boost/bind/bind.hpp>
+
   template<class ValueType>
   class Values::Filtered {
   public:

gtsam/nonlinear/Values.cpp

@@ -25,8 +25,6 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/VectorValues.h>
 
-#include <boost/iterator/transform_iterator.hpp>
-
 #include <list>
 #include <memory>
 #include <sstream>
@@ -52,15 +50,15 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Values::Values(const Values& other, const VectorValues& delta) {
-    for (const_iterator key_value = other.begin(); key_value != other.end(); ++key_value) {
+    for (const auto& key_value : other.values_) {
-      VectorValues::const_iterator it = delta.find(key_value->key);
+      VectorValues::const_iterator it = delta.find(key_value.first);
-      Key key = key_value->key;  // 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
       if (it != delta.end()) {
         const Vector& v = it->second;
-        Value* retractedValue(key_value->value.retract_(v));  // Retract
+        Value* retractedValue(key_value.second->retract_(v));  // Retract
         values_.insert(key, retractedValue);  // Add retracted result directly to result values
       } else {
-        values_.insert(key, key_value->value.clone_());  // Add original version to result values
+        values_.insert(key, key_value.second->clone_());  // Add original version to result values
       }
     }
   }
@@ -69,9 +67,9 @@ namespace gtsam {
   void Values::print(const string& str, const KeyFormatter& keyFormatter) const {
     cout << str << (str.empty() ? "" : "\n");
     cout << "Values with " << size() << " values:\n";
-    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
+    for (const auto& key_value : values_) {
-      cout << "Value " << keyFormatter(key_value->key) << ": ";
+      cout << "Value " << keyFormatter(key_value.first) << ": ";
-      key_value->value.print("");
+      key_value.second->print("");
       cout << "\n";
     }
   }
@@ -80,12 +78,12 @@ namespace gtsam {
   bool Values::equals(const Values& other, double tol) const {
     if (this->size() != other.size())
       return false;
-    for (const_iterator it1 = this->begin(), it2 = other.begin();
+    for (auto it1 = values_.begin(), it2 = other.values_.begin();
-        it1 != this->end(); ++it1, ++it2) {
+         it1 != values_.end(); ++it1, ++it2) {
-      const Value& value1 = it1->value;
+      const Value* value1 = it1->second;
-      const Value& value2 = it2->value;
+      const Value* value2 = it2->second;
-      if (typeid(value1) != typeid(value2) || it1->key != it2->key
+      if (typeid(*value1) != typeid(*value2) || it1->first != it2->first
-          || !value1.equals_(value2, tol)) {
+          || !value1->equals_(*value2, tol)) {
         return false;
       }
     }
@@ -102,17 +100,44 @@ namespace gtsam {
     return Values(*this, delta);
   }
 
+  /* ************************************************************************* */
+  void Values::retractMasked(const VectorValues& delta, const KeySet& mask) {
+    gttic(retractMasked);
+    assert(theta->size() == delta.size());
+    auto key_value = values_.begin();
+    VectorValues::const_iterator key_delta;
+#ifdef GTSAM_USE_TBB
+    for (; key_value != values_.end(); ++key_value) {
+      key_delta = delta.find(key_value.first);
+#else
+    for (key_delta = delta.begin(); key_value != values_.end();
+         ++key_value, ++key_delta) {
+      assert(key_value.first == key_delta->first);
+#endif
+      Key var = key_value->first;
+      assert(static_cast<size_t>(delta[var].size()) == key_value->second->dim());
+      assert(delta[var].allFinite());
+      if (mask.exists(var)) {
+        Value* retracted = key_value->second->retract_(delta[var]);
+        // TODO(dellaert): can we use std::move here?
+        *(key_value->second) = *retracted;
+        retracted->deallocate_();
+      }
+    }
+  }
+
   /* ************************************************************************* */
   VectorValues Values::localCoordinates(const Values& cp) const {
     if(this->size() != cp.size())
       throw DynamicValuesMismatched();
     VectorValues result;
-    for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
+    for (auto it1 = values_.begin(), it2 = cp.values_.begin();
-      if(it1->key != it2->key)
+         it1 != values_.end(); ++it1, ++it2) {
+      if(it1->first != it2->first)
         throw DynamicValuesMismatched(); // If keys do not match
       // Will throw a dynamic_cast exception if types do not match
       // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert
-      result.insert(it1->key, it1->value.localCoordinates_(it2->value));
+      result.insert(it1->first, it1->second->localCoordinates_(*it2->second));
     }
     return result;
   }
@@ -130,24 +155,26 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void Values::insert(Key j, const Value& val) {
-    std::pair<iterator,bool> insertResult = tryInsert(j, val);
+    auto insertResult = values_.insert(j, val.clone_());
     if(!insertResult.second)
       throw ValuesKeyAlreadyExists(j);
   }
 
   /* ************************************************************************* */
-  void Values::insert(const Values& values) {
+  void Values::insert(const Values& other) {
-    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
+    for (auto key_value = other.values_.begin();
-      Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
+         key_value != other.values_.end(); ++key_value) {
-      insert(key, key_value->value);
+      insert(key_value->first, *(key_value->second));
     }
   }
 
   /* ************************************************************************* */
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
   std::pair<Values::iterator, bool> Values::tryInsert(Key j, const Value& value) {
     std::pair<KeyValueMap::iterator, bool> result = values_.insert(j, value.clone_());
     return std::make_pair(boost::make_transform_iterator(result.first, &make_deref_pair), result.second);
   }
+#endif
 
   /* ************************************************************************* */
   void Values::update(Key j, const Value& val) {
@@ -165,9 +192,10 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void Values::update(const Values& values) {
+  void Values::update(const Values& other) {
-    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
+    for (auto key_value = other.values_.begin();
-      this->update(key_value->key, key_value->value);
+         key_value != other.values_.end(); ++key_value) {
+      this->update(key_value->first, *(key_value->second));
     }
   }
 
@@ -183,10 +211,10 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  void Values::insert_or_assign(const Values& values) {
+  void Values::insert_or_assign(const Values& other) {
-    for (const_iterator key_value = values.begin(); key_value != values.end();
+    for (auto key_value = other.values_.begin();
-         ++key_value) {
+         key_value != other.values_.end(); ++key_value) {
-      this->insert_or_assign(key_value->key, key_value->value);
+      this->insert_or_assign(key_value->first, *(key_value->second));
     }
   }
 
@@ -202,8 +230,16 @@ namespace gtsam {
   KeyVector Values::keys() const {
     KeyVector result;
     result.reserve(size());
-    for(const_iterator key_value = begin(); key_value != end(); ++key_value)
+    for(const auto& key_value: values_)
-      result.push_back(key_value->key);
+      result.push_back(key_value.first);
+    return result;
+  }
+
+  /* ************************************************************************* */
+  KeySet Values::keySet() const {
+    KeySet result;
+    for(const auto& key_value: values_)
+      result.insert(key_value.first);
     return result;
   }
 
@@ -217,8 +253,17 @@ namespace gtsam {
   /* ************************************************************************* */
   size_t Values::dim() const {
     size_t result = 0;
-    for(const auto key_value: *this) {
+    for (const auto key_value : values_) {
-      result += key_value.value.dim();
+      result += key_value->second->dim();
+    }
+    return result;
+  }
+
+  /* ************************************************************************* */
+  std::map<Key,size_t> Values::dims() const {
+    std::map<Key,size_t> result;
+    for (const auto key_value : values_) {
+      result.emplace(key_value->first, key_value->second->dim());
     }
     return result;
   }
@@ -226,8 +271,8 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues Values::zeroVectors() const {
     VectorValues result;
-    for(const auto key_value: *this)
+    for (const auto key_value : values_)
-      result.insert(key_value.key, Vector::Zero(key_value.value.dim()));
+      result.insert(key_value->first, Vector::Zero(key_value->second->dim()));
     return result;
   }
 

gtsam/nonlinear/Values.h

@@ -28,10 +28,10 @@
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/VectorSpace.h>
 #include <gtsam/inference/Key.h>
-#include <boost/iterator/transform_iterator.hpp>
 #include <boost/ptr_container/serialize_ptr_map.hpp>
 #include <boost/shared_ptr.hpp>
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+#include <boost/iterator/transform_iterator.hpp>
 #include <boost/iterator/filter_iterator.hpp>
 #endif
 
@@ -81,10 +81,6 @@ namespace gtsam {
     // The member to store the values, see just above
     KeyValueMap values_;
 
-    // Types obtained by iterating
-    typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
-    typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
-
   public:
 
     /// A shared_ptr to this class
@@ -110,22 +106,6 @@ namespace gtsam {
       ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
     };
 
-    /// Mutable forward iterator, with value type KeyValuePair
-    typedef boost::transform_iterator<
-        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
-
-    /// Const forward iterator, with value type ConstKeyValuePair
-    typedef boost::transform_iterator<
-        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
-
-    /// Mutable reverse iterator, with value type KeyValuePair
-    typedef boost::transform_iterator<
-        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
-
-    /// Const reverse iterator, with value type ConstKeyValuePair
-    typedef boost::transform_iterator<
-        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
-
     typedef KeyValuePair value_type;
 
     /** Default constructor creates an empty Values class */
@@ -191,47 +171,24 @@ namespace gtsam {
     template<typename ValueType>
     boost::optional<const ValueType&> exists(Key j) const;
 
-    /** Find an element by key, returning an iterator, or end() if the key was
-     * not found. */
-    iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
-
-    /** Find an element by key, returning an iterator, or end() if the key was
-     * not found. */
-    const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); }
-
-    /** Find the element greater than or equal to the specified key. */
-    iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
-
-    /** Find the element greater than or equal to the specified key. */
-    const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); }
-
-    /** Find the lowest-ordered element greater than the specified key. */
-    iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
-
-    /** Find the lowest-ordered element greater than the specified key. */
-    const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); }
-
     /** The number of variables in this config */
     size_t size() const { return values_.size(); }
 
     /** whether the config is empty */
     bool empty() const { return values_.empty(); }
 
-    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
     /// @{
 
     /** Add a delta config to current config and returns a new config */
     Values retract(const VectorValues& delta) const;
 
+    /**
+     * Retract, but only for Keys appearing in \c mask. In-place.
+     * \param mask Mask on Keys where to apply retract.
+     */
+    void retractMasked(const VectorValues& delta, const KeySet& mask);
+
     /** Get a delta config about a linearization point c0 (*this) */
     VectorValues localCoordinates(const Values& cp) const;
 
@@ -252,12 +209,6 @@ namespace gtsam {
     /// version for double
     void insertDouble(Key j, double c) { insert<double>(j,c); }
 
-    /** insert that mimics the STL map insert - if the value already exists, the map is not modified
-     *  and an iterator to the existing value is returned, along with 'false'.  If the value did not
-     *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
-     *  returned. */
-    std::pair<iterator, bool> tryInsert(Key j, const Value& value);
-
     /** single element change of existing element */
     void update(Key j, const Value& val);
 
@@ -288,11 +239,16 @@ namespace gtsam {
     void erase(Key j);
 
     /**
-     * Returns a set of keys in the config
+     * Returns a vector of keys in the config.
      * Note: by construction, the list is ordered
      */
     KeyVector keys() const;
 
+    /**
+     * Returns a set of keys in the config.
+     */
+    KeySet keySet() const;
+
     /** Replace all keys and variables */
     Values& operator=(const Values& rhs);
 
@@ -305,6 +261,9 @@ namespace gtsam {
     /** Compute the total dimensionality of all values (\f$ O(n) \f$) */
     size_t dim() const;
 
+    /** Return all dimensions in a map (\f$ O(n log n) \f$) */
+    std::map<Key,size_t> dims() const;
+
     /** Return a VectorValues of zero vectors for each variable in this Values */
     VectorValues zeroVectors() const;
 
@@ -312,8 +271,8 @@ namespace gtsam {
     template<class ValueType>
     size_t count() const {
       size_t i = 0;
-      for (const auto key_value : *this) {
+      for (const auto key_value : values_) {
-        if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
+        if (dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
           ++i;
       }
       return i;
@@ -343,6 +302,67 @@ namespace gtsam {
     extract(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+    // Types obtained by iterating
+    typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
+    typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
+
+    /** insert that mimics the STL map insert - if the value already exists, the map is not modified
+     *  and an iterator to the existing value is returned, along with 'false'.  If the value did not
+     *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
+     *  returned. */
+    std::pair<iterator, bool> tryInsert(Key j, const Value& value);
+
+    /// Mutable forward iterator, with value type KeyValuePair
+    typedef boost::transform_iterator<
+        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
+
+    /// Const forward iterator, with value type ConstKeyValuePair
+    typedef boost::transform_iterator<
+        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
+
+    /// Mutable reverse iterator, with value type KeyValuePair
+    typedef boost::transform_iterator<
+        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
+
+    /// Const reverse iterator, with value type ConstKeyValuePair
+    typedef boost::transform_iterator<
+        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
+
+    static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
+      return ConstKeyValuePair(key_value.first, *key_value.second); }
+
+    static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
+      return KeyValuePair(key_value.first, *key_value.second); }
+
+    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); }
+
+    /** Find an element by key, returning an iterator, or end() if the key was
+     * not found. */
+    iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
+
+    /** Find an element by key, returning an iterator, or end() if the key was
+     * not found. */
+    const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); }
+
+    /** Find the element greater than or equal to the specified key. */
+    iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
+
+    /** Find the element greater than or equal to the specified key. */
+    const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); }
+
+    /** Find the lowest-ordered element greater than the specified key. */
+    iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
+
+    /** Find the lowest-ordered element greater than the specified key. */
+    const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); }
+
     /** A filtered view of a Values, returned from Values::filter. */
     template <class ValueType = Value>
     class Filtered;
@@ -395,12 +415,6 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(values_);
     }
 
-    static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
-      return ConstKeyValuePair(key_value.first, *key_value.second); }
-
-    static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
-      return KeyValuePair(key_value.first, *key_value.second); }
-
   };
 
   /* ************************************************************************* */

gtsam/nonlinear/internal/LevenbergMarquardtState.h

@@ -128,9 +128,10 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
     noiseModelCache.resize(0);
     // for each of the variables, add a prior
     damped.reserve(damped.size() + values.size());
-    for (const auto key_value : values) {
+    std::map<Key,size_t> dims = values.dims();
-      const Key key = key_value.key;
+    for (const auto& key_dim : dims) {
-      const size_t dim = key_value.value.dim();
+      const Key& key = key_dim.first;
+      const size_t& dim = key_dim.second;
       const CachedModel* item = getCachedModel(dim);
       damped += boost::make_shared<JacobianFactor>(key, item->A, item->b, item->model);
     }

gtsam/nonlinear/tests/testLinearContainerFactor.cpp

@@ -350,8 +350,8 @@ TEST(TestLinearContainerFactor, Rekey) {
 
   // For extra fun lets try linearizing this LCF
   gtsam::Values linearization_pt_rekeyed;
-  for (auto key_val : linearization_pt) {
+  for (auto key : linearization_pt.keys()) {
-    linearization_pt_rekeyed.insert(key_map.at(key_val.key), key_val.value);
+    linearization_pt_rekeyed.insert(key_map.at(key), linearization_pt.at(key));
   }
 
   // Check independent values since we don't want to unnecessarily sort

gtsam/nonlinear/tests/testValues.cpp

@@ -195,6 +195,7 @@ TEST(Values, basic_functions)
   values.insert(6, M1);
   values.insert(8, M2);
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
   // find
   EXPECT_LONGS_EQUAL(4, values.find(4)->key);
   EXPECT_LONGS_EQUAL(4, values_c.find(4)->key);
@@ -210,7 +211,7 @@ TEST(Values, basic_functions)
   EXPECT_LONGS_EQUAL(6, values_c.upper_bound(4)->key);
   EXPECT_LONGS_EQUAL(4, values.upper_bound(3)->key);
   EXPECT_LONGS_EQUAL(4, values_c.upper_bound(3)->key);
-
+#endif
 }
 
 /* ************************************************************************* */
@@ -220,8 +221,8 @@ TEST(Values, retract_full)
   config0.insert(key1, Vector3(1.0, 2.0, 3.0));
   config0.insert(key2, Vector3(5.0, 6.0, 7.0));
 
-  VectorValues delta {{key1, Vector3(1.0, 1.1, 1.2)},
+  const VectorValues delta{{key1, Vector3(1.0, 1.1, 1.2)},
-                      {key2, Vector3(1.3, 1.4, 1.5)}};
+                           {key2, Vector3(1.3, 1.4, 1.5)}};
 
   Values expected;
   expected.insert(key1, Vector3(2.0, 3.1, 4.2));
@@ -238,7 +239,7 @@ TEST(Values, retract_partial)
   config0.insert(key1, Vector3(1.0, 2.0, 3.0));
   config0.insert(key2, Vector3(5.0, 6.0, 7.0));
 
-  VectorValues delta {{key2, Vector3(1.3, 1.4, 1.5)}};
+  const VectorValues delta{{key2, Vector3(1.3, 1.4, 1.5)}};
 
   Values expected;
   expected.insert(key1, Vector3(1.0, 2.0, 3.0));
@@ -248,6 +249,24 @@ TEST(Values, retract_partial)
   CHECK(assert_equal(expected, Values(config0, delta)));
 }
 
+/* ************************************************************************* */
+TEST(Values, retract_masked)
+{
+  Values config0;
+  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
+  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
+
+  const VectorValues delta{{key1, Vector3(1.0, 1.1, 1.2)},
+                           {key2, Vector3(1.3, 1.4, 1.5)}};
+
+  Values expected;
+  expected.insert(key1, Vector3(1.0, 2.0, 3.0));
+  expected.insert(key2, Vector3(6.3, 7.4, 8.5));
+
+  config0.retractMasked(delta, {key2});
+  CHECK(assert_equal(expected, config0));
+}
+
 /* ************************************************************************* */
 TEST(Values, equals)
 {

gtsam/slam/InitializePose.h

@@ -62,10 +62,10 @@ static Values computePoses(const Values& initialRot,
 
   // Upgrade rotations to full poses
   Values initialPose;
-  for (const auto key_value : initialRot) {
+  for (const auto& key_rot : initialRot.extract<typename Pose::Rotation>()) {
-    Key key = key_value.key;
+    const Key& key = key_rot.first;
-    const auto& rot = initialRot.at<typename Pose::Rotation>(key);
+    const auto& rot = key_rot.second;
-    Pose initializedPose = Pose(rot, origin);
+    const Pose initializedPose(rot, origin);
     initialPose.insert(key, initializedPose);
   }
 
@@ -82,14 +82,14 @@ static Values computePoses(const Values& initialRot,
     params.setVerbosity("TERMINATION");
   }
   GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
-  Values GNresult = optimizer.optimize();
+  const Values GNresult = optimizer.optimize();
 
   // put into Values structure
   Values estimate;
-  for (const auto key_value : GNresult) {
+  for (const auto& key_pose : GNresult.extract<Pose>()) {
-    Key key = key_value.key;
+    const Key& key = key_pose.first;
     if (key != kAnchorKey) {
-      const Pose& pose = GNresult.at<Pose>(key);
+      const Pose& pose = key_pose.second;
       estimate.insert(key, pose);
     }
   }

gtsam/slam/InitializePose3.cpp

@@ -122,12 +122,12 @@ Values InitializePose3::computeOrientationsGradient(
   gttic(InitializePose3_computeOrientationsGradient);
 
   // this works on the inverse rotations, according to Tron&Vidal,2011
-  Values inverseRot;
+  std::map<Key,Rot3> inverseRot;
-  inverseRot.insert(initialize::kAnchorKey, Rot3());
+  inverseRot.emplace(initialize::kAnchorKey, Rot3());
-  for(const auto key_value: givenGuess) {
+  for(const auto& key_pose: givenGuess.extract<Pose3>()) {
-    Key key = key_value.key;
+    const Key& key = key_pose.first;
-    const Pose3& pose = givenGuess.at<Pose3>(key);
+    const Pose3& pose = key_pose.second;
-    inverseRot.insert(key, pose.rotation().inverse());
+    inverseRot.emplace(key, pose.rotation().inverse());
   }
 
   // Create the map of edges incident on each node
@@ -138,10 +138,8 @@ Values InitializePose3::computeOrientationsGradient(
 
   // calculate max node degree & allocate gradient
   size_t maxNodeDeg = 0;
-  VectorValues grad;
+  for (const auto& key_R : inverseRot) {
-  for(const auto key_value: inverseRot) {
+    const Key& key = key_R.first;
-    Key key = key_value.key;
-    grad.insert(key,Z_3x1);
     size_t currNodeDeg = (adjEdgesMap.at(key)).size();
     if(currNodeDeg > maxNodeDeg)
       maxNodeDeg = currNodeDeg;
@@ -162,28 +160,29 @@ Values InitializePose3::computeOrientationsGradient(
     //////////////////////////////////////////////////////////////////////////
     // compute the gradient at each node
     maxGrad = 0;
-    for (const auto key_value : inverseRot) {
+    VectorValues grad;
-      Key key = key_value.key;
+    for (const auto& key_R : inverseRot) {
+      const Key& key = key_R.first;
+      const Rot3& Ri = key_R.second;
       Vector gradKey = Z_3x1;
       // collect the gradient for each edge incident on key
       for (const size_t& factorId : adjEdgesMap.at(key)) {
-        Rot3 Rij = factorId2RotMap.at(factorId);
+        const Rot3& Rij = factorId2RotMap.at(factorId);
-        Rot3 Ri = inverseRot.at<Rot3>(key);
         auto factor = pose3Graph.at(factorId);
         const auto& keys = factor->keys();
         if (key == keys[0]) {
           Key key1 = keys[1];
-          Rot3 Rj = inverseRot.at<Rot3>(key1);
+          const Rot3& Rj = inverseRot.at(key1);
           gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
         } else if (key == keys[1]) {
           Key key0 = keys[0];
-          Rot3 Rj = inverseRot.at<Rot3>(key0);
+          const Rot3& Rj = inverseRot.at(key0);
           gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
         } else {
           cout << "Error in gradient computation" << endl;
         }
       }  // end of i-th gradient computation
-      grad.at(key) = stepsize * gradKey;
+      grad.insert(key, stepsize * gradKey);
 
       double normGradKey = (gradKey).norm();
       if(normGradKey>maxGrad)
@@ -192,8 +191,12 @@ Values InitializePose3::computeOrientationsGradient(
 
     //////////////////////////////////////////////////////////////////////////
     // update estimates
-    inverseRot = inverseRot.retract(grad);
+    for (auto& key_R : inverseRot) {
-
+      const Key& key = key_R.first;
+      Rot3& Ri = key_R.second;
+      Ri = Ri.retract(grad.at(key));
+    }
+    
     //////////////////////////////////////////////////////////////////////////
     // check stopping condition
     if (it>20 && maxGrad < 5e-3)
@@ -201,12 +204,13 @@ Values InitializePose3::computeOrientationsGradient(
   } // enf of gradient iterations
 
   // Return correct rotations
-  const Rot3& Rref = inverseRot.at<Rot3>(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
+  const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
   Values estimateRot;
-  for(const auto key_value: inverseRot) {
+    for (const auto key_R : inverseRot) {
-    Key key = key_value.key;
+    const Key& key = key_R.first;
+    const Rot3& Ri = key_R.second;
     if (key != initialize::kAnchorKey) {
-      const Rot3& R = inverseRot.at<Rot3>(key);
+      const Rot3& R = inverseRot.at(key);
       if(setRefFrame)
         estimateRot.insert(key, Rref.compose(R.inverse()));
       else

gtsam/slam/dataset.cpp

@@ -586,9 +586,9 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config,
   fstream stream(filename.c_str(), fstream::out);
 
   // save poses
-  for (const auto key_value : config) {
+  for (const auto &key_pose : config.extract<Pose2>()) {
-    const Pose2 &pose = key_value.value.cast<Pose2>();
+    const Pose2 &pose = key_pose.second;
-    stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
+    stream << "VERTEX2 " << key_pose.first << " " << pose.x() << " " << pose.y()
            << " " << pose.theta() << endl;
   }
 
@@ -635,45 +635,33 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
   auto index = [](gtsam::Key key) { return Symbol(key).index(); };
 
   // save 2D poses
-  for (const auto key_value : estimate) {
+  for (const auto &pair : estimate.extract<Pose2>()) {
-    auto p = dynamic_cast<const GenericValue<Pose2> *>(&key_value.value);
+    const Pose2 &pose = pair.second;
-    if (!p)
+    stream << "VERTEX_SE2 " << index(pair.first) << " " << pose.x() << " "
-      continue;
-    const Pose2 &pose = p->value();
-    stream << "VERTEX_SE2 " << index(key_value.key) << " " << pose.x() << " "
            << pose.y() << " " << pose.theta() << endl;
   }
 
   // save 3D poses
-  for (const auto key_value : estimate) {
+  for (const auto &pair : estimate.extract<Pose3>()) {
-    auto p = dynamic_cast<const GenericValue<Pose3> *>(&key_value.value);
+    const Pose3 &pose = pair.second;
-    if (!p)
-      continue;
-    const Pose3 &pose = p->value();
     const Point3 t = pose.translation();
     const auto q = pose.rotation().toQuaternion();
-    stream << "VERTEX_SE3:QUAT " << index(key_value.key) << " " << t.x() << " "
+    stream << "VERTEX_SE3:QUAT " << index(pair.first) << " " << t.x() << " "
            << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
            << q.z() << " " << q.w() << endl;
   }
 
   // save 2D landmarks
-  for (const auto key_value : estimate) {
+  for (const auto &pair : estimate.extract<Point2>()) {
-    auto p = dynamic_cast<const GenericValue<Point2> *>(&key_value.value);
+    const Point2 &point = pair.second;
-    if (!p)
+    stream << "VERTEX_XY " << index(pair.first) << " " << point.x() << " "
-      continue;
-    const Point2 &point = p->value();
-    stream << "VERTEX_XY " << index(key_value.key) << " " << point.x() << " "
            << point.y() << endl;
   }
 
   // save 3D landmarks
-  for (const auto key_value : estimate) {
+  for (const auto &pair : estimate.extract<Point3>()) {
-    auto p = dynamic_cast<const GenericValue<Point3> *>(&key_value.value);
+    const Point3 &point = pair.second;
-    if (!p)
+    stream << "VERTEX_TRACKXYZ " << index(pair.first) << " " << point.x()
-      continue;
-    const Point3 &point = p->value();
-    stream << "VERTEX_TRACKXYZ " << index(key_value.key) << " " << point.x()
            << " " << point.y() << " " << point.z() << endl;
   }
 

gtsam/slam/tests/testLago.cpp

@@ -283,9 +283,10 @@ TEST( Lago, largeGraphNoisy_orientations ) {
   Values::shared_ptr expected;
   boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  for(const auto key_val: *expected){
+  for(const auto key_pose: expected->extract<Pose2>()){
-    Key k = key_val.key;
+    const Key& k = key_pose.first;
-    EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
+    const Pose2& pose = key_pose.second;
+    EXPECT(assert_equal(pose, actual.at<Pose2>(k), 1e-5));
   }
 }
 
@@ -309,9 +310,10 @@ TEST( Lago, largeGraphNoisy ) {
   Values::shared_ptr expected;
   boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  for(const auto key_val: *expected){
+  for(const auto key_pose: expected->extract<Pose2>()){
-    Key k = key_val.key;
+    const Key& k = key_pose.first;
-    EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
+    const Pose2& pose = key_pose.second;
+    EXPECT(assert_equal(pose, actual.at<Pose2>(k), 1e-2));
   }
 }
 

gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp

@@ -308,12 +308,12 @@ int main(int argc, char** argv) {
   // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
   cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
   cout << "  Concurrent Filter Keys: " << endl;
-  for(const auto key_value: concurrentFilter.getLinearizationPoint()) {
+  for(const auto key: concurrentFilter.getLinearizationPoint().keys()) {
-    cout << setprecision(5) << "    Key: " << key_value.key << endl;
+    cout << setprecision(5) << "    Key: " << key << endl;
   }
   cout << "  Concurrent Smoother Keys: " << endl;
-  for(const auto key_value: concurrentSmoother.getLinearizationPoint()) {
+  for(const auto key: concurrentSmoother.getLinearizationPoint().keys()) {
-    cout << setprecision(5) << "    Key: " << key_value.key << endl;
+    cout << setprecision(5) << "    Key: " << key << endl;
   }
   cout << "  Fixed-Lag Smoother Keys: " << endl;
   for(const auto& key_timestamp: fixedlagSmoother.timestamps()) {

gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp

@@ -59,8 +59,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
   // Add the new variables to theta
   theta_.insert(newTheta);
   // Add new variables to the end of the ordering
-  for (const auto key_value : newTheta) {
+  for (const auto key : newTheta.keys()) {
-    ordering_.push_back(key_value.key);
+    ordering_.push_back(key);
   }
   // Augment Delta
   delta_.insert(newTheta.zeroVectors());
@@ -161,8 +161,8 @@ void BatchFixedLagSmoother::eraseKeys(const KeyVector& keys) {
     factorIndex_.erase(key);
 
     // Erase the key from the set of linearized keys
-    if (linearKeys_.exists(key)) {
+    if (linearValues_.exists(key)) {
-      linearKeys_.erase(key);
+      linearValues_.erase(key);
     }
   }
 
@@ -186,8 +186,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
 
   // Create output result structure
   Result result;
-  result.nonlinearVariables = theta_.size() - linearKeys_.size();
+  result.nonlinearVariables = theta_.size() - linearValues_.size();
-  result.linearVariables = linearKeys_.size();
+  result.linearVariables = linearValues_.size();
 
   // Set optimization parameters
   double lambda = parameters_.lambdaInitial;
@@ -265,10 +265,10 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
           // Reset the deltas to zeros
           delta_.setZero();
           // Put the linearization points and deltas back for specific variables
-          if (enforceConsistency_ && (linearKeys_.size() > 0)) {
+          if (enforceConsistency_ && (linearValues_.size() > 0)) {
-            theta_.update(linearKeys_);
+            theta_.update(linearValues_);
-            for(const auto key_value: linearKeys_) {
+            for(const auto key: linearValues_.keys()) {
-              delta_.at(key_value.key) = newDelta.at(key_value.key);
+              delta_.at(key) = newDelta.at(key);
             }
           }
           // Decrease lambda for next time

gtsam_unstable/nonlinear/BatchFixedLagSmoother.h

@@ -136,8 +136,8 @@ protected:
   /** The current linearization point **/
   Values theta_;
 
-  /** The set of keys involved in current linear factors. These keys should not be relinearized. **/
+  /** The set of values involved in current linear factors. **/
-  Values linearKeys_;
+  Values linearValues_;
 
   /** The current ordering */
   Ordering ordering_;

gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp

@@ -139,8 +139,8 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
   // Add the new variables to theta
   theta_.insert(newTheta);
   // Add new variables to the end of the ordering
-  for(const auto key_value: newTheta) {
+  for (const auto key : newTheta.keys()) {
-    ordering_.push_back(key_value.key);
+    ordering_.push_back(key);
   }
   // Augment Delta
   delta_.insert(newTheta.zeroVectors());
@@ -221,10 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
   if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize  ", "Updated Smoother Summarization:", DefaultKeyFormatter); }
 
   // Find the set of new separator keys
-  KeySet newSeparatorKeys;
+  const KeySet newSeparatorKeys = separatorValues_.keySet();
-  for(const auto key_value: separatorValues_) {
-    newSeparatorKeys.insert(key_value.key);
-  }
 
   if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize  ", "Current Separator Keys:"); }
 
@@ -236,9 +233,9 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
     graph.push_back(smootherShortcut_);
     Values values;
     values.insert(smootherSummarizationValues);
-    for(const auto key_value: separatorValues_) {
+    for(const auto key: newSeparatorKeys) {
-      if(!values.exists(key_value.key)) {
+      if(!values.exists(key)) {
-        values.insert(key_value.key, key_value.value);
+        values.insert(key, separatorValues_.at(key));
       }
     }
     // Calculate the summarized factor on just the new separator keys
@@ -471,8 +468,8 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values
           // Put the linearization points and deltas back for specific variables
           if(linearValues.size() > 0) {
             theta.update(linearValues);
-            for(const auto key_value: linearValues) {
+            for(const auto key: linearValues.keys()) {
-              delta.at(key_value.key) = newDelta.at(key_value.key);
+              delta.at(key) = newDelta.at(key);
             }
           }
 
@@ -574,9 +571,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
 
   // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
   KeySet newSeparatorKeys = removedFactors.keys();
-  for(const auto key_value: separatorValues_) {
+  newSeparatorKeys.merge(separatorValues_.keySet());
-    newSeparatorKeys.insert(key_value.key);
-  }
   for(Key key: keysToMove) {
     newSeparatorKeys.erase(key);
   }

gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp

@@ -61,8 +61,8 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
     theta_.insert(newTheta);
 
     // Add new variables to the end of the ordering
-    for(const auto key_value: newTheta) {
+    for(const auto key: newTheta.keys()) {
-      ordering_.push_back(key_value.key);
+      ordering_.push_back(key);
     }
 
     // Augment Delta
@@ -135,26 +135,30 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
   removeFactors(filterSummarizationSlots_);
 
   // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta
-  for(const auto key_value: smootherValues) {
+  for(const auto key: smootherValues.keys()) {
-    std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
+    if(!theta_.exists(key)) {
-    if(iter_inserted.second) {
+      // If this a new key for theta_, also add to ordering and delta.
-      // If the insert succeeded
+      const auto& value = smootherValues.at(key);
-      ordering_.push_back(key_value.key);
+      delta_.insert(key, Vector::Zero(value.dim()));
-      delta_.insert(key_value.key, Vector::Zero(key_value.value.dim()));
+      theta_.insert(key, value);
+      ordering_.push_back(key);
     } else {
-      // If the element already existed in theta_
+      // If the key already existed in theta_, just update.
-      iter_inserted.first->value = key_value.value;
+      const auto& value = smootherValues.at(key);
+      theta_.update(key, value);
     }
   }
-  for(const auto key_value: separatorValues) {
+  for(const auto key: separatorValues.keys()) {
-    std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
+    if(!theta_.exists(key)) {
-    if(iter_inserted.second) {
+      // If this a new key for theta_, also add to ordering and delta.
-      // If the insert succeeded
+      const auto& value = separatorValues.at(key);
-      ordering_.push_back(key_value.key);
+      delta_.insert(key, Vector::Zero(value.dim()));
-      delta_.insert(key_value.key, Vector::Zero(key_value.value.dim()));
+      theta_.insert(key, value);
+      ordering_.push_back(key);
     } else {
-      // If the element already existed in theta_
+      // If the key already existed in theta_, just update.
-      iter_inserted.first->value = key_value.value;
+      const auto& value = separatorValues.at(key);
+      theta_.update(key, value);
     }
   }
 
@@ -322,8 +326,8 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
           // Put the linearization points and deltas back for specific variables
           if(separatorValues_.size() > 0) {
             theta_.update(separatorValues_);
-            for(const auto key_value: separatorValues_) {
+            for(const auto key: separatorValues_.keys()) {
-              delta_.at(key_value.key) = newDelta.at(key_value.key);
+              delta_.at(key) = newDelta.at(key);
             }
           }
 
@@ -371,10 +375,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
   }
 
   // Get the set of separator keys
-  gtsam::KeySet separatorKeys;
+  const KeySet separatorKeys = separatorValues_.keySet();
-  for(const auto key_value: separatorValues_) {
-    separatorKeys.insert(key_value.key);
-  }
 
   // Calculate the marginal factors on the separator
   smootherSummarization_ = internal::calculateMarginalFactors(graph, theta_, separatorKeys, parameters_.getEliminationFunction());

gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp

@@ -69,14 +69,14 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
     int group = 1;
     // Set all existing variables to Group1
     if(isam2_.getLinearizationPoint().size() > 0) {
-      for(const auto key_value: isam2_.getLinearizationPoint()) {
+      for(const auto key: isam2_.getLinearizationPoint().keys()) {
-        orderingConstraints->operator[](key_value.key) = group;
+        orderingConstraints->operator[](key) = group;
       }
       ++group;
     }
     // Assign new variables to the root
-    for(const auto key_value: newTheta) {
+    for(const auto key: newTheta.keys()) {
-      orderingConstraints->operator[](key_value.key) = group;
+      orderingConstraints->operator[](key) = group;
     }
     // Set marginalizable variables to Group0
     for(Key key: *keysToMove){
@@ -201,8 +201,8 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
 
   // Force iSAM2 not to relinearize anything during this iteration
   FastList<Key> noRelinKeys;
-  for(const auto key_value: isam2_.getLinearizationPoint()) {
+  for(const auto key: isam2_.getLinearizationPoint().keys()) {
-    noRelinKeys.push_back(key_value.key);
+    noRelinKeys.push_back(key);
   }
 
   // Calculate the summarized factor on just the new separator keys

gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp

@@ -66,9 +66,9 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
   // Also, mark the separator keys as fixed linearization points
   FastMap<Key,int> constrainedKeys;
   FastList<Key> noRelinKeys;
-  for(const auto key_value: separatorValues_) {
+  for (const auto key : separatorValues_.keys()) {
-    constrainedKeys[key_value.key] = 1;
+    constrainedKeys[key] = 1;
-    noRelinKeys.push_back(key_value.key);
+    noRelinKeys.push_back(key);
   }
 
   // Use iSAM2 to perform an update
@@ -82,14 +82,14 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
       Values values(newTheta);
       // Unfortunately, we must be careful here, as some of the smoother values
       // and/or separator values may have been added previously
-      for(const auto key_value: smootherValues_) {
+      for(const auto key: smootherValues_.keys()) {
-        if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
+        if(!isam2_.getLinearizationPoint().exists(key)) {
-          values.insert(key_value.key, smootherValues_.at(key_value.key));
+          values.insert(key, smootherValues_.at(key));
         }
       }
-      for(const auto key_value: separatorValues_) {
+      for(const auto key: separatorValues_.keys()) {
-        if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
+        if(!isam2_.getLinearizationPoint().exists(key)) {
-          values.insert(key_value.key, separatorValues_.at(key_value.key));
+          values.insert(key, separatorValues_.at(key));
         }
       }
 
@@ -245,10 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
   }
 
   // Get the set of separator keys
-  KeySet separatorKeys;
+  const KeySet separatorKeys = separatorValues_.keySet();
-  for(const auto key_value: separatorValues_) {
-    separatorKeys.insert(key_value.key);
-  }
 
   // Calculate the marginal factors on the separator
   smootherSummarization_ = internal::calculateMarginalFactors(graph, isam2_.getLinearizationPoint(), separatorKeys,

gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp

@@ -64,8 +64,8 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
 
   std::set<Key> KeysToKeep;
-  for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph
+  for(const auto key: linPoint.keys()) { // we cycle over all the keys of factorGraph
-    KeysToKeep.insert(key_value.key);
+    KeysToKeep.insert(key);
   } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
   for(Key key: keysToMarginalize) {
     KeysToKeep.erase(key);

gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp

@@ -560,8 +560,8 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
   GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
 
   KeySet eliminateKeys = linearFactors->keys();
-  for(const auto key_value: filterSeparatorValues) {
+  for(const auto key: filterSeparatorValues.keys()) {
-    eliminateKeys.erase(key_value.key);
+    eliminateKeys.erase(key);
   }
   KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());
   GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second;

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp

@@ -80,8 +80,8 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
 
   std::set<Key> KeysToKeep;
-  for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph
+  for(const auto key: linPoint.keys()) { // we cycle over all the keys of factorGraph
-    KeysToKeep.insert(key_value.key);
+    KeysToKeep.insert(key);
   } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
   for(Key key: keysToMarginalize) {
     KeysToKeep.erase(key);

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp

@@ -512,8 +512,8 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
 //  Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
   Values expectedLinearizationPoint = filterSeparatorValues;
   Values actualLinearizationPoint;
-  for(const auto key_value: filterSeparatorValues) {
+  for(const auto key: filterSeparatorValues.keys()) {
-    actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
+    actualLinearizationPoint.insert(key, smoother.getLinearizationPoint().at(key));
   }
   CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
 }
@@ -580,8 +580,8 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
 //  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
 
   KeySet allkeys = LinFactorGraph->keys();
-  for(const auto key_value: filterSeparatorValues) {
+  for(const auto key: filterSeparatorValues.keys()) {
-    allkeys.erase(key_value.key);
+    allkeys.erase(key);
   }
   KeyVector variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);

gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp

@@ -513,8 +513,8 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
 //  Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
   Values expectedLinearizationPoint = filterSeparatorValues;
   Values actualLinearizationPoint;
-  for(const auto key_value: filterSeparatorValues) {
+  for(const auto key: filterSeparatorValues.keys()) {
-    actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
+    actualLinearizationPoint.insert(key, smoother.getLinearizationPoint().at(key));
   }
   CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
 }
@@ -582,8 +582,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
 //  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
 
   KeySet allkeys = LinFactorGraph->keys();
-  for(const auto key_value: filterSeparatorValues)
+  for (const auto key : filterSeparatorValues.keys()) allkeys.erase(key);
-    allkeys.erase(key_value.key);
   KeyVector variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);