From dbf1d9fd1a44373e742539ae32e7017878d9c976 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 31 Dec 2022 01:11:48 -0500
Subject: [PATCH] Initializer list and insert/update chaining

---
 gtsam/linear/VectorValues.cpp           | 26 ++++++++++++++-----------
 gtsam/linear/VectorValues.h             | 15 +++++++++-----
 gtsam/linear/tests/testVectorValues.cpp | 16 +++++++++++++++
 3 files changed, 41 insertions(+), 16 deletions(-)

diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index 62996af27..04792b6ba 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -98,30 +98,34 @@ namespace gtsam {
   }
 
   /* ************************************************************************ */
-  void VectorValues::update(const VectorValues& values)
-  {
+  VectorValues& VectorValues::update(const VectorValues& values) {
     iterator hint = begin();
-    for(const KeyValuePair& key_value: values)
-    {
-      // Use this trick to find the value using a hint, since we are inserting from another sorted map
+    for (const KeyValuePair& key_value : values) {
+      // Use this trick to find the value using a hint, since we are inserting
+      // from another sorted map
       size_t oldSize = values_.size();
       hint = values_.insert(hint, key_value);
-      if(values_.size() > oldSize) {
+      if (values_.size() > oldSize) {
         values_.unsafe_erase(hint);
-        throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first.");
+        throw out_of_range(
+            "Requested to update a VectorValues with another VectorValues that "
+            "contains keys not present in the first.");
       } else {
         hint->second = key_value.second;
       }
     }
+    return *this;
   }
 
   /* ************************************************************************ */
-  void VectorValues::insert(const VectorValues& values)
-  {
+  VectorValues& VectorValues::insert(const VectorValues& values) {
     size_t originalSize = size();
     values_.insert(values.begin(), values.end());
-    if(size() != originalSize + values.size())
-      throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
+    if (size() != originalSize + values.size())
+      throw invalid_argument(
+          "Requested to insert a VectorValues into another VectorValues that "
+          "already contains one or more of its keys.");
+    return *this;
   }
 
   /* ************************************************************************ */
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 4e1f793a0..6a645150f 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -88,11 +88,16 @@ namespace gtsam {
     /// @name Standard Constructors
     /// @{
 
-    /**
-     * Default constructor creates an empty VectorValues.
-     */
+    /// Default constructor creates an empty VectorValues.
     VectorValues() {}
 
+    /// Construct from initializer list.
+    VectorValues(std::initializer_list<std::pair<Key, Vector>> init) {
+      for (const auto& p : init) {
+        values_.insert(p);  // Insert key-value pair into map
+      }
+    }
+
     /** Merge two VectorValues into one, this is more efficient than inserting
      * elements one by one. */
     VectorValues(const VectorValues& first, const VectorValues& second);
@@ -167,7 +172,7 @@ namespace gtsam {
     /** For all key/value pairs in \c values, replace values with corresponding keys in this class
     *   with those in \c values.  Throws std::out_of_range if any keys in \c values are not present
     *   in this class. */
-    void update(const VectorValues& values);
+    VectorValues& update(const VectorValues& values);
 
     /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
      *  j is already used.
@@ -198,7 +203,7 @@ namespace gtsam {
 
     /** Insert all values from \c values.  Throws an invalid_argument exception if any keys to be
      *  inserted are already used. */
-    void insert(const VectorValues& values);
+    VectorValues& insert(const VectorValues& values);
 
     /** 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
diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp
index 521cc2289..5974f8320 100644
--- a/gtsam/linear/tests/testVectorValues.cpp
+++ b/gtsam/linear/tests/testVectorValues.cpp
@@ -73,6 +73,22 @@ TEST(VectorValues, basics)
   CHECK_EXCEPTION(actual.dim(3), out_of_range);
 }
 
+/* ************************************************************************* */
+
+static const VectorValues kExample = {{99, Vector2(2, 3)}};
+
+// Check insert
+TEST(VectorValues, Insert) {
+  VectorValues actual;
+  EXPECT(assert_equal(kExample, actual.insert(kExample)));
+}
+
+// Check update.
+TEST(VectorValues, Update) {
+  VectorValues actual(kExample);
+  EXPECT(assert_equal(kExample, actual.update(kExample)));
+}
+
 /* ************************************************************************* */
 TEST(VectorValues, combine)
 {