Added emplace

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, Vector>& 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<iterator, bool> 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<iterator, bool> 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<value_type, value_type> ValuePair;
+    for(const auto& values: boost::combine(*this, x)) {
-    for(const ValuePair& 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;
   }
 

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.emplace(3, Vector3(1.0, 2.0, 3.0));
-     values.insert(4, Vector2(4.0, 5.0));
+     values.emplace(4, Vector2(4.0, 5.0));
-     values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished());
+     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 {
    *
    * <h2>Advanced Interface and Performance Information</h2>
    *
-   * For advanced usage, or where speed is important:
+   * Access is through the variable Key j, and returns a SubVector,
-   *  - 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,
    * 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) {
+    iterator insert(const std::pair<Key, Vector>& key_value);
-      return insert(std::make_pair(j, 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, Vector>& 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<iterator, bool> 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) {