addInPlace_ (does not require structure to be the same)

gtsam/linear/VectorValues.cpp

@@ -278,6 +278,19 @@ namespace gtsam {
     return *this += c;
   }
 
+  /* ************************************************************************* */
+  VectorValues& VectorValues::addInPlace_(const VectorValues& c)
+  {
+    for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
+      pair<VectorValues::iterator, bool> xi = tryInsert(j2->first, Vector());
+      if(xi.second)
+        xi.first->second = j2->second;
+      else
+        xi.first->second += j2->second;
+    }
+    return *this;
+  }
+
   /* ************************************************************************* */
   VectorValues VectorValues::operator-(const VectorValues& c) const
   {

gtsam/linear/VectorValues.h

@@ -291,6 +291,9 @@ namespace gtsam {
      * same structure (checked when NDEBUG is not defined). */
     VectorValues& addInPlace(const VectorValues& c);
 
+    /** Element-wise addition in-place, but allows for empty slots in *this. Slower */
+    VectorValues& addInPlace_(const VectorValues& c);
+
     /** Element-wise subtraction, synonym for subtract().  Both VectorValues must have the same
      *  structure (checked when NDEBUG is not defined). */
     VectorValues operator-(const VectorValues& c) const;

gtsam/linear/tests/testVectorValuesUnordered.cpp

@@ -147,6 +147,12 @@ TEST(VectorValues, LinearAlgebra)
   sum2Actual += test1;
   EXPECT(assert_equal(sum2Expected, sum2Actual.vector()));
 
+  // Add to empty
+  VectorValues sumActual3;
+  sumActual3.addInPlace_(test1);
+  sumActual3.addInPlace_(test2);
+  EXPECT(assert_equal(sumExpected, sumActual3.vector()));
+
   // Subtraction
   Vector difExpected = test1.vector() - test2.vector();
   VectorValues difActual = test1 - test2;