diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 66c001831..ceb58e551 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -53,6 +53,24 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + void VectorValues::update(const VectorValues& values) + { + iterator hint = begin(); + BOOST_FOREACH(const KeyValuePair& key_value, values) + { + // Use this trick to find the value using a hint, since we are inserting from another sorted map + size_t oldSize = values_.size(); + hint = values_.insert(hint, key_value); + if(values_.size() > oldSize) { + values_.erase(hint); + throw std::out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first."); + } else { + hint->second = key_value.second; + } + } + } + /* ************************************************************************* */ void VectorValues::insert(const VectorValues& values) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 31ad6525b..39e332263 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -157,24 +157,31 @@ namespace gtsam { return item->second; } - /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to at(Key). */ + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ Vector& operator[](Key j) { return at(j); } - /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Key). */ + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ const Vector& operator[](Key j) const { return at(j); } - /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c j is already used. + /** For all key/value pairs in \c values, replace values with corresponding keys in this class + * with those in \c values. Throws std::out_of_range if any keys in \c values are not present + * in this class. */ + void update(const VectorValues& values); + + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. * @param value The vector to be inserted. - * @param j The index with which the value will be associated. - */ + * @param j The index with which the value will be associated. */ void insert(Key j, const Vector& value) { insert(std::pair(j, value)); // Note only passing a reference to the Vector } - /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c j is already used. + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. * @param value The vector to be inserted. - * @param j The index with which the value will be associated. - */ + * @param j The index with which the value will be associated. */ void insert(std::pair key_value) { // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as // it is inserted into the values_ map. @@ -195,6 +202,12 @@ namespace gtsam { std::pair tryInsert(Key j, const Vector& value) { return values_.insert(std::make_pair(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_.erase(var) == 0) + throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); + } + /** Set all values to zero vectors. */ void setZero();