diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 8a0e7fa00..9ca17e2fe 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/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 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 { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 1a82c0bbb..4ea01dfbe 100644 --- a/gtsam/linear/VectorValues.h +++ b/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; diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValuesUnordered.cpp index fbe56cb33..74e0b2ca2 100644 --- a/gtsam/linear/tests/testVectorValuesUnordered.cpp +++ b/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;