diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 8ebdcab17c..dfcb7e174c 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))); } + // insert_or_assign with templated value + template + 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 ebc9c51f67..adadc99c06 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/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); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 207f355407..cfe6347b50 100644 --- a/gtsam/nonlinear/Values.h +++ b/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 + 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 152c4b8e74..8407668cba 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 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& 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, 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(X), x)); + + // This should perform an update. + double y = 2; + values.insert_or_assign(X, y); + EXPECT(assert_equal(values.at(X), y)); +} + /* ************************************************************************* */ TEST(Values, basic_functions) {