diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 5ac6600f9..614389132 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -103,7 +103,7 @@ public: Vector& vector() { return values_; } /** Reserve space for a total number of variables and dimensionality */ - void reserve(Index nVars, size_t totalDims) { values_.resize(totalDims); varStarts_.reserve(nVars+1); } + void reserve(Index nVars, size_t totalDims) { values_.conservativeResize(totalDims); varStarts_.reserve(nVars+1); } /** access a range of indices (of no particular order) as a single vector */ template diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 0c7da0ca4..1746552c5 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -183,6 +183,38 @@ TEST(VectorValues, makeZero ) { EXPECT(assert_equal(zero(14), values.vector())); } +/* ************************************************************************* */ +TEST(VectorValues, reserve ) { + Vector v1 = Vector_(3, 1.0,2.0,3.0); + Vector v2 = Vector_(2, 4.0,5.0); + Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0); + Vector v4(2); v4 << 10, 11; + Vector v5(3); v5 << 12, 13, 14; + + // Expected has all 5 variables + vector dimsExp(5); dimsExp[0]=3; dimsExp[1]=2; dimsExp[2]=4; dimsExp[3]=2; dimsExp[4]=3; + VectorValues expected(dimsExp); + expected[0] = v1; + expected[1] = v2; + expected[2] = v3; + expected[3] = v4; + expected[4] = v5; + + // Start with 3 variables + vector dims(3); dims[0]=3; dims[1]=2; dims[2]=4; + VectorValues actual(dims); + actual[0] = v1; + actual[1] = v2; + actual[2] = v3; + + // Now expand to all 5 + actual.reserve(5, 14); + actual.push_back_preallocated(v4); + actual.push_back_preallocated(v5); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST(VectorValues, range ) { VectorValues v(7,2);