insert_or_assign

gtsam/hybrid/HybridValues.h

@@ -122,6 +122,16 @@ class GTSAM_EXPORT HybridValues {
    * @param j The index with which the value will be associated. */
   void insert(Key j, size_t value) { discrete_[j] = value; };
 
+  /// insert_or_assign() , similar to Values.h
+  void insert_or_assign(Key j, const Vector& value) {
+    continuous_.insert_or_assign(j, value);
+  }
+
+  /// insert_or_assign() , similar to Values.h
+  void insert_or_assign(Key j, size_t value) {
+    discrete_[j] = value;
+  }
+
   /** Insert all continuous values from \c values.  Throws an invalid_argument
    * exception if any keys to be inserted are already used. */
   HybridValues& insert(const VectorValues& values) {
@@ -152,8 +162,6 @@ class GTSAM_EXPORT HybridValues {
     return *this;
   }
 
-  // TODO(Shangjie)- insert_or_assign() , similar to Values.h
-
   /**
    * Read/write access to the vector value with key \c j, throws
    * std::out_of_range if \c j does not exist.

gtsam/hybrid/hybrid.i

@@ -19,6 +19,9 @@ class HybridValues {
   void insert(gtsam::Key j, int value);
   void insert(gtsam::Key j, const gtsam::Vector& value);
   
+  void insert_or_assign(gtsam::Key j, const gtsam::Vector& value);
+  void insert_or_assign(gtsam::Key j, size_t value);
+
   void insert(const gtsam::VectorValues& values);
   void insert(const gtsam::DiscreteValues& values);
   void insert(const gtsam::HybridValues& values);

gtsam/linear/VectorValues.h

@@ -214,6 +214,14 @@ namespace gtsam {
 #endif
     }
 
+    /// insert_or_assign that mimics the STL map insert_or_assign - if the value already exists, the
+    /// map is updated, otherwise a new value is inserted at j.
+    void insert_or_assign(Key j, const Vector& value) {
+      if (!tryInsert(j, value).second) {
+        (*this)[j] = value;
+      }
+    }
+
     /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */
     void erase(Key var) {
       if (values_.unsafe_erase(var) == 0)