diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index ca95313cf..e8304e6e7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -51,7 +51,7 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; - values_.insert(make_pair(key, x.segment(j, n))); + values_.emplace(key, x.segment(j, n)); j += n; } } @@ -60,7 +60,7 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { - values_.insert(make_pair(v.key, x.segment(j, v.dimension))); + values_.emplace(v.key, x.segment(j, v.dimension)); j += v.dimension; } } @@ -70,14 +70,12 @@ namespace gtsam { { VectorValues result; for(const KeyValuePair& v: other) - result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); + result.values_.emplace(v.first, Vector::Zero(v.second.size())); return result; } /* ************************************************************************* */ VectorValues::iterator VectorValues::insert(const 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. std::pair result = values_.insert(key_value); if(!result.second) throw std::invalid_argument( @@ -86,6 +84,16 @@ namespace gtsam { return result.first; } + /* ************************************************************************* */ + VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { + std::pair result = values_.emplace(j, value); + if(!result.second) + throw std::invalid_argument( + "Requested to emplace variable '" + DefaultKeyFormatter(j) + + "' already in this VectorValues."); + return result.first; + } + /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { @@ -132,8 +140,7 @@ namespace gtsam { bool VectorValues::equals(const VectorValues& x, double tol) const { if(this->size() != x.size()) return false; - typedef boost::tuple ValuePair; - for(const ValuePair& values: boost::combine(*this, x)) { + for(const auto& values: boost::combine(*this, x)) { if(values.get<0>().first != values.get<1>().first || !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) return false; @@ -240,7 +247,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); + result.values_.emplace(j1->first, j1->second + j2->second); return result; } @@ -298,7 +305,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); + result.values_.emplace(j1->first, j1->second - j2->second); return result; } @@ -314,7 +321,7 @@ namespace gtsam { { VectorValues result; for(const VectorValues::KeyValuePair& key_v: v) - result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); + result.values_.emplace(key_v.first, a * key_v.second); return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 39abe1b56..e0bacfd12 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -50,9 +50,9 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, Vector3(1.0, 2.0, 3.0)); - values.insert(4, Vector2(4.0, 5.0)); - values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); + values.emplace(3, Vector3(1.0, 2.0, 3.0)); + values.emplace(4, Vector2(4.0, 5.0)); + values.emplace(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); @@ -64,18 +64,7 @@ namespace gtsam { * *

Advanced Interface and Performance Information

* - * For advanced usage, or where speed is important: - * - Allocate space ahead of time using a pre-allocating constructor - * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), - * SameStructure(), resize(), or append(). Do not use - * insert(Key, const Vector&), which always has to re-allocate the - * internal vector. - * - The vector() function permits access to the underlying Vector, for - * doing mathematical or other operations that require all values. - * - operator[]() returns a SubVector view of the underlying Vector, - * without copying any data. - * - * Access is through the variable index j, and returns a SubVector, + * Access is through the variable Key j, and returns a SubVector, * which is a view on the underlying data structure. * * This class is additionally used in gradient descent and dog leg to store the gradient. @@ -183,15 +172,21 @@ namespace gtsam { * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); - } + iterator insert(const std::pair& key_value); + + /** Emplace 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. */ + iterator emplace(Key j, const Vector& value); /** 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. */ - iterator insert(const std::pair& key_value); + iterator insert(Key j, const Vector& value) { + return insert(std::make_pair(j, value)); + } /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be * inserted are already used. */ @@ -202,7 +197,8 @@ namespace gtsam { * exist, it is inserted and an iterator pointing to the new element, along with 'true', is * returned. */ std::pair tryInsert(Key j, const Vector& value) { - return values_.insert(std::make_pair(j, value)); } + return values_.emplace(j, value); + } /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) {