Merge pull request #1417 from borglab/feature/iterator

borglab · Jan 26, 2023 · 88b4082 · 88b4082
2 parents e9e794b + d00971d
commit 88b4082
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 19 deletions.
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
@@ -163,14 +163,14 @@ namespace gtsam {
                 &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), constBegin_(
             boost::make_transform_iterator(
                 boost::make_filter_iterator(filter,
-                    ((const Values&) values).begin(),
-                    ((const Values&) values).end()),
+                    values._begin(),
+                    values._end()),
                 &ValuesCastHelper<ValueType, ConstKeyValuePair,
                     Values::ConstKeyValuePair>::cast)), constEnd_(
             boost::make_transform_iterator(
                 boost::make_filter_iterator(filter,
-                    ((const Values&) values).end(),
-                    ((const Values&) values).end()),
+                    values._end(),
+                    values._end()),
                 &ValuesCastHelper<ValueType, ConstKeyValuePair,
                     Values::ConstKeyValuePair>::cast)) {
     }

diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
@@ -108,8 +108,11 @@ namespace gtsam {
 
     typedef KeyValuePair value_type;
 
+    /// @name Constructors
+    /// @{
+
     /** Default constructor creates an empty Values class */
-    Values() {}
+    Values() = default;
 
     /** Copy constructor duplicates all keys and values */
     Values(const Values& other);
@@ -127,6 +130,7 @@ namespace gtsam {
     /** Construct from a Values and an update vector: identical to other.retract(delta) */
     Values(const Values& other, const VectorValues& delta);
 
+    /// @}
     /// @name Testable
     /// @{
 
@@ -137,6 +141,8 @@ namespace gtsam {
     bool equals(const Values& other, double tol=1e-9) const;
 
     /// @}
+    /// @name Standard Interface
+    /// @{
 
     /** Retrieve a variable by key \c j.  The type of the value associated with
      * this key is supplied as a template argument to this function.
@@ -177,6 +183,42 @@ namespace gtsam {
     /** whether the config is empty */
     bool empty() const { return values_.empty(); }
 
+    /// @}
+    /// @name Iterator
+    /// @{
+
+    struct deref_iterator {
+      using const_iterator_type = typename KeyValueMap::const_iterator;
+      const_iterator_type it_;
+      deref_iterator(const_iterator_type it) : it_(it) {}
+      ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; }
+      boost::shared_ptr<ConstKeyValuePair> operator->() {
+        return boost::make_shared<ConstKeyValuePair>(it_->first, *(it_->second));
+      }
+      bool operator==(const deref_iterator& other) const {
+        return it_ == other.it_;
+      }
+      bool operator!=(const deref_iterator& other) const { return it_ != other.it_; }
+      deref_iterator& operator++() {
+        ++it_;
+        return *this;
+      }
+    };
+
+    deref_iterator begin() const { return deref_iterator(values_.begin()); }
+    deref_iterator end() const { return deref_iterator(values_.end()); }
+
+    /** Find an element by key, returning an iterator, or end() if the key was
+     * not found. */
+    deref_iterator find(Key j) const { return deref_iterator(values_.find(j)); }
+
+    /** Find the element greater than or equal to the specified key. */
+    deref_iterator lower_bound(Key j) const { return deref_iterator(values_.lower_bound(j)); }
+
+    /** Find the lowest-ordered element greater than the specified key. */
+    deref_iterator upper_bound(Key j) const { return deref_iterator(values_.upper_bound(j)); }
+
+    /// @}
     /// @name Manifold Operations
     /// @{
 
@@ -334,8 +376,8 @@ namespace gtsam {
     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); }
+    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); }
@@ -347,22 +389,12 @@ namespace gtsam {
      * 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;

diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
@@ -195,7 +195,14 @@ TEST(Values, basic_functions)
   values.insert(6, M1);
   values.insert(8, M2);
 
-#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+  size_t count = 0;
+  for (const auto& it : values) {
+    count += 1;
+    if (it.key == 2 || it.key == 4) EXPECT_LONGS_EQUAL(3, it.value.dim());
+    if (it.key == 6 || it.key == 8) EXPECT_LONGS_EQUAL(6, it.value.dim());
+  }
+  EXPECT_LONGS_EQUAL(4, count);
+
   // find
   EXPECT_LONGS_EQUAL(4, values.find(4)->key);
   EXPECT_LONGS_EQUAL(4, values_c.find(4)->key);
@@ -211,7 +218,6 @@ 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
 }
 
 /* ************************************************************************* */