From 8c3ce253ae7145542400736d39e7482525548634 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Dec 2021 01:14:14 -0500 Subject: [PATCH 1/2] Add new upsert method to Values --- gtsam/nonlinear/Values-inl.h | 6 ++++++ gtsam/nonlinear/Values.cpp | 18 ++++++++++++++++++ gtsam/nonlinear/Values.h | 10 ++++++++++ gtsam/nonlinear/nonlinear.i | 27 +++++++++++++++++++++++++++ gtsam/nonlinear/tests/testValues.cpp | 16 ++++++++++++++++ 5 files changed, 77 insertions(+) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 8ebdcab17c..e3e5346680 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -391,4 +391,10 @@ namespace gtsam { update(j, static_cast(GenericValue(val))); } + // upsert with templated value + template + void Values::upsert(Key j, const ValueType& val) { + upsert(j, static_cast(GenericValue(val))); + } + } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index ebc9c51f67..c866cc3b51 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -171,6 +171,24 @@ namespace gtsam { } } + /* ************************************************************************* */ + void Values::upsert(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::upsert(const Values& values) { + for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { + this->upsert(key_value->key, key_value->value); + } + } + /* ************************************************************************* */ void Values::erase(Key j) { KeyValueMap::iterator item = values_.find(j); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 207f355407..8c318ef935 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -285,6 +285,16 @@ namespace gtsam { /** update the current available values without adding new ones */ void update(const Values& values); + /** Update a variable with key j. If j does not exist, then perform an insert. */ + void upsert(Key j, const Value& val); + + /** Update a set of variables. If any variable key doe not exist, then perform an insert. */ + void upsert(const Values& values); + + /** Templated version to upsert (update/insert) a variable with the given j. */ + template + void upsert(Key j, const ValueType& val); + /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ void erase(Key j); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 152c4b8e74..6951178473 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -275,6 +275,7 @@ class Values { void insert(const gtsam::Values& values); void update(const gtsam::Values& values); + void upsert(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 upsert(size_t j, const gtsam::Point2& point2); + void upsert(size_t j, const gtsam::Point3& point3); + void upsert(size_t j, const gtsam::Rot2& rot2); + void upsert(size_t j, const gtsam::Pose2& pose2); + void upsert(size_t j, const gtsam::SO3& R); + void upsert(size_t j, const gtsam::SO4& Q); + void upsert(size_t j, const gtsam::SOn& P); + void upsert(size_t j, const gtsam::Rot3& rot3); + void upsert(size_t j, const gtsam::Pose3& pose3); + void upsert(size_t j, const gtsam::Unit3& unit3); + void upsert(size_t j, const gtsam::Cal3_S2& cal3_s2); + void upsert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void upsert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void upsert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void upsert(size_t j, const gtsam::Cal3Unified& cal3unified); + void upsert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void upsert(size_t j, const gtsam::PinholeCamera& camera); + void upsert(size_t j, const gtsam::PinholeCamera& camera); + void upsert(size_t j, const gtsam::PinholeCamera& camera); + void upsert(size_t j, const gtsam::PinholeCamera& camera); + void upsert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void upsert(size_t j, const gtsam::NavState& nav_state); + void upsert(size_t j, Vector vector); + void upsert(size_t j, Matrix matrix); + void upsert(size_t j, double c); + template (key1))); } +TEST(Values, upsert) { + Values values; + Key X(0); + double x = 1; + + CHECK(values.size() == 0); + // This should perform an insert. + values.upsert(X, x); + EXPECT(assert_equal(values.at(X), x)); + + // This should perform an update. + double y = 2; + values.upsert(X, y); + EXPECT(assert_equal(values.at(X), y)); +} + /* ************************************************************************* */ TEST(Values, basic_functions) { From 96652cad073795c75c451c768a20323f8e0e4a9f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 15 Dec 2021 17:19:13 -0500 Subject: [PATCH 2/2] replace upsert with insert_or_assign --- gtsam/nonlinear/Values-inl.h | 6 ++-- gtsam/nonlinear/Values.cpp | 13 +++---- gtsam/nonlinear/Values.h | 15 ++++---- gtsam/nonlinear/nonlinear.i | 52 ++++++++++++++-------------- gtsam/nonlinear/tests/testValues.cpp | 6 ++-- 5 files changed, 48 insertions(+), 44 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index e3e5346680..dfcb7e174c 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -391,10 +391,10 @@ namespace gtsam { update(j, static_cast(GenericValue(val))); } - // upsert with templated value + // insert_or_assign with templated value template - void Values::upsert(Key j, const ValueType& val) { - upsert(j, static_cast(GenericValue(val))); + void Values::insert_or_assign(Key j, const ValueType& val) { + insert_or_assign(j, static_cast(GenericValue(val))); } } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index c866cc3b51..adadc99c06 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -171,8 +171,8 @@ namespace gtsam { } } - /* ************************************************************************* */ - void Values::upsert(Key j, const Value& val) { + /* ************************************************************************ */ + 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); @@ -182,10 +182,11 @@ namespace gtsam { } } - /* ************************************************************************* */ - void Values::upsert(const Values& values) { - for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { - this->upsert(key_value->key, key_value->value); + /* ************************************************************************ */ + 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); } } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 8c318ef935..cfe6347b50 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -285,15 +285,18 @@ namespace gtsam { /** update the current available values without adding new ones */ void update(const Values& values); - /** Update a variable with key j. If j does not exist, then perform an insert. */ - void upsert(Key j, const Value& val); + /// 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 upsert(const Values& values); + /** + * 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 upsert (update/insert) a variable with the given j. */ + /// Templated version to insert_or_assign a variable with the given j. template - void upsert(Key j, const ValueType& val); + void insert_or_assign(Key j, const ValueType& val); /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ void erase(Key j); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 6951178473..8407668cba 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -275,7 +275,7 @@ class Values { void insert(const gtsam::Values& values); void update(const gtsam::Values& values); - void upsert(const gtsam::Values& values); + void insert_or_assign(const gtsam::Values& values); void erase(size_t j); void swap(gtsam::Values& values); @@ -352,31 +352,31 @@ class Values { void update(size_t j, Matrix matrix); void update(size_t j, double c); - void upsert(size_t j, const gtsam::Point2& point2); - void upsert(size_t j, const gtsam::Point3& point3); - void upsert(size_t j, const gtsam::Rot2& rot2); - void upsert(size_t j, const gtsam::Pose2& pose2); - void upsert(size_t j, const gtsam::SO3& R); - void upsert(size_t j, const gtsam::SO4& Q); - void upsert(size_t j, const gtsam::SOn& P); - void upsert(size_t j, const gtsam::Rot3& rot3); - void upsert(size_t j, const gtsam::Pose3& pose3); - void upsert(size_t j, const gtsam::Unit3& unit3); - void upsert(size_t j, const gtsam::Cal3_S2& cal3_s2); - void upsert(size_t j, const gtsam::Cal3DS2& cal3ds2); - void upsert(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void upsert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); - void upsert(size_t j, const gtsam::Cal3Unified& cal3unified); - void upsert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void upsert(size_t j, const gtsam::PinholeCamera& camera); - void upsert(size_t j, const gtsam::PinholeCamera& camera); - void upsert(size_t j, const gtsam::PinholeCamera& camera); - void upsert(size_t j, const gtsam::PinholeCamera& camera); - void upsert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void upsert(size_t j, const gtsam::NavState& nav_state); - void upsert(size_t j, Vector vector); - void upsert(size_t j, Matrix matrix); - void upsert(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& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& 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 (key1))); } -TEST(Values, upsert) { +TEST(Values, InsertOrAssign) { Values values; Key X(0); double x = 1; CHECK(values.size() == 0); // This should perform an insert. - values.upsert(X, x); + values.insert_or_assign(X, x); EXPECT(assert_equal(values.at(X), x)); // This should perform an update. double y = 2; - values.upsert(X, y); + values.insert_or_assign(X, y); EXPECT(assert_equal(values.at(X), y)); }