diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 47ad0ca6f..b92e54143 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -125,8 +125,7 @@ namespace gtsam { /* ************************************************************************* */ void Values::insert(Key j, const Value& val) { - Key key = j; // Non-const duplicate to deal with non-const insert argument - std::pair insertResult = values_.insert(key, val.clone_()); + std::pair insertResult = tryInsert(j, val); if(!insertResult.second) throw ValuesKeyAlreadyExists(j); } @@ -139,6 +138,12 @@ namespace gtsam { } } + /* ************************************************************************* */ + std::pair Values::tryInsert(Key j, const Value& value) { + std::pair result = values_.insert(j, value.clone_()); + return std::make_pair(boost::make_transform_iterator(result.first, &make_deref_pair), result.second); + } + /* ************************************************************************* */ void Values::update(Key j, const Value& val) { // Find the value to update diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 9ba1282b8..e364d1fc2 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -232,6 +232,12 @@ namespace gtsam { /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ void insert(const Values& 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 + * exist, it is inserted and an iterator pointing to the new element, along with 'true', is + * returned. */ + std::pair tryInsert(Key j, const Value& value); + /** single element change of existing element */ void update(Key j, const Value& val);