Add new insert_or_assign method to Values

gtsam/nonlinear/Values-inl.h

@@ -391,4 +391,10 @@ namespace gtsam {
     update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
   }
 
+  // insert_or_assign with templated value
+  template <typename ValueType>
+  void Values::insert_or_assign(Key j, const ValueType& val) {
+    insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
+  }
+
 }

gtsam/nonlinear/Values.cpp

@@ -171,6 +171,25 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************ */
+  void Values::insert_or_assign(Key j, const Value& val) {
+    if (this->exists(j)) {
+      // If key already exists, perform an update.
+      this->update(j, val);
+    } else {
+      // If key does not exist, perform an insert.
+      this->insert(j, val);
+    }
+  }
+
+  /* ************************************************************************ */
+  void Values::insert_or_assign(const Values& values) {
+    for (const_iterator key_value = values.begin(); key_value != values.end();
+         ++key_value) {
+      this->insert_or_assign(key_value->key, key_value->value);
+    }
+  }
+
   /* ************************************************************************* */
   void Values::erase(Key j) {
     KeyValueMap::iterator item = values_.find(j);

gtsam/nonlinear/Values.h

@@ -285,6 +285,19 @@ namespace gtsam {
     /** update the current available values without adding new ones */
     void update(const Values& values);
 
+    /// If key j exists, update value, else perform an insert.
+    void insert_or_assign(Key j, const Value& val);
+
+    /**
+     * Update a set of variables.
+     * If any variable key doe not exist, then perform an insert.
+     */
+    void insert_or_assign(const Values& values);
+
+    /// Templated version to insert_or_assign a variable with the given j.
+    template <typename ValueType>
+    void insert_or_assign(Key j, const ValueType& val);
+
     /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
     void erase(Key j);
 

gtsam/nonlinear/nonlinear.i

@@ -275,6 +275,7 @@ class Values {
 
   void insert(const gtsam::Values& values);
   void update(const gtsam::Values& values);
+  void insert_or_assign(const gtsam::Values& values);
   void erase(size_t j);
   void swap(gtsam::Values& values);
 
@@ -351,6 +352,32 @@ class Values {
   void update(size_t j, Matrix matrix);
   void update(size_t j, double c);
 
+  void insert_or_assign(size_t j, const gtsam::Point2& point2);
+  void insert_or_assign(size_t j, const gtsam::Point3& point3);
+  void insert_or_assign(size_t j, const gtsam::Rot2& rot2);
+  void insert_or_assign(size_t j, const gtsam::Pose2& pose2);
+  void insert_or_assign(size_t j, const gtsam::SO3& R);
+  void insert_or_assign(size_t j, const gtsam::SO4& Q);
+  void insert_or_assign(size_t j, const gtsam::SOn& P);
+  void insert_or_assign(size_t j, const gtsam::Rot3& rot3);
+  void insert_or_assign(size_t j, const gtsam::Pose3& pose3);
+  void insert_or_assign(size_t j, const gtsam::Unit3& unit3);
+  void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2);
+  void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2);
+  void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
+  void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified);
+  void insert_or_assign(size_t j, const gtsam::EssentialMatrix& essential_matrix);
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
+  void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
+  void insert_or_assign(size_t j, Vector vector);
+  void insert_or_assign(size_t j, Matrix matrix);
+  void insert_or_assign(size_t j, double c);
+
   template <T = {gtsam::Point2,
                  gtsam::Point3,
                  gtsam::Rot2,

gtsam/nonlinear/tests/testValues.cpp

@@ -172,6 +172,22 @@ TEST( Values, update_element )
   CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
 }
 
+TEST(Values, InsertOrAssign) {
+  Values values;
+  Key X(0);
+  double x = 1;
+
+  CHECK(values.size() == 0);
+  // This should perform an insert.
+  values.insert_or_assign(X, x);
+  EXPECT(assert_equal(values.at<double>(X), x));
+
+  // This should perform an update.
+  double y = 2;
+  values.insert_or_assign(X, y);
+  EXPECT(assert_equal(values.at<double>(X), y));
+}
+
 /* ************************************************************************* */
 TEST(Values, basic_functions)
 {