From 366595bd0524e36cfd2398788ece21e61c2588d2 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 25 Jan 2023 22:59:22 -0800
Subject: [PATCH] Added a const_iterator back in

---
 gtsam/nonlinear/Values.h             | 44 +++++++++++++++++++++++++++-
 gtsam/nonlinear/tests/testValues.cpp | 20 +++++++++++++
 2 files changed, 63 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index ba0252c98..be2df9056 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -105,8 +105,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);
@@ -124,6 +127,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
     /// @{
 
@@ -134,6 +138,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.
@@ -174,6 +180,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)}; }
+      std::unique_ptr<ConstKeyValuePair> operator->() {
+        return std::make_unique<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
     /// @{
 
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 02a3374e4..1ad7277dc 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -194,11 +194,31 @@ TEST(Values, basic_functions)
   values.insert(6, M1);
   values.insert(8, M2);
 
+
   EXPECT(!values.exists(1));
   EXPECT(values.exists(2));
   EXPECT(values.exists(4));
   EXPECT(values.exists(6));
   EXPECT(values.exists(8));
+
+  size_t count = 0;
+  for (const auto& [key, value] : values) {
+    count += 1;
+    if (key == 2 || key == 4) EXPECT_LONGS_EQUAL(3, value.dim());
+    if (key == 6 || key == 8) EXPECT_LONGS_EQUAL(6, value.dim());
+  }
+  EXPECT_LONGS_EQUAL(4, count);
+
+  // find
+  EXPECT_LONGS_EQUAL(4, values.find(4)->key);
+
+  // lower_bound
+  EXPECT_LONGS_EQUAL(4, values.lower_bound(4)->key);
+  EXPECT_LONGS_EQUAL(4, values.lower_bound(3)->key);
+
+  // upper_bound
+  EXPECT_LONGS_EQUAL(6, values.upper_bound(4)->key);
+  EXPECT_LONGS_EQUAL(4, values.upper_bound(3)->key);
 }
 
 /* ************************************************************************* */