diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index bcd106a0e..af13231fb 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -179,6 +179,12 @@ namespace gtsam { template explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); } + /** Construct from a container of variable dimensions (in variable order), with initial values. */ + template + explicit VectorValues(const Vector& d, const CONTAINER& dimensions) { + this->append(d, dimensions); + } + /** Construct to hold nVars vectors of varDim dimension each. */ VectorValues(Index nVars, size_t varDim) { this->resize(nVars, varDim); } @@ -237,6 +243,19 @@ namespace gtsam { template void append(const CONTAINER& dimensions); + /** Append to the VectorValues to additionally contain variables of the + * dimensions stored in \c dimensions. Initial values for the new variables + * are extracted from the input vector, holding values for all components + * in the same order specified in the dimensions container. + * This function preserves the original data, so all previously-existing + * variables are left unchanged. + * @param d A vector holding values for all variables, which order + * specified in the below container + * @param dimensions A container of the dimension of each variable to create. + */ + template + void append(const Vector& d, const CONTAINER& dimensions); + /** Removes the last subvector from the VectorValues */ void pop_back() { values_.pop_back(); }; @@ -409,6 +428,19 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + void VectorValues::append(const Vector& d, const CONTAINER& dimensions) { + size_t i = size(); + size_t idx = 0; + values_.resize(size() + dimensions.size()); + BOOST_FOREACH(size_t dim, dimensions) { + values_[i] = sub(d, idx, idx+dim); + ++ i; + idx += dim; + } + } + /* ************************************************************************* */ template VectorValues VectorValues::Zero(const CONTAINER& dimensions) { diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 8bfd42781..c416937ed 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -395,6 +395,36 @@ TEST(VectorValues, append) { CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument); } +/* ************************************************************************* */ +TEST(VectorValues, append_withValuesFromVector) { + // Constructor with initial values + vector dims(3); + dims[0] = 1; + dims[1] = 2; + dims[2] = 2; + Vector d = Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0); + VectorValues actual(d, dims); + + VectorValues expected; + expected.insert(0, Vector_(1, 1.0)); + expected.insert(1, Vector_(2, 2.0, 3.0)); + expected.insert(2, Vector_(2, 4.0, 5.0)); + + EXPECT(assert_equal(expected, actual)); + + // Append with initial values + expected.insert(3, Vector_(1, 6.0)); + expected.insert(4, Vector_(3, 7.0, 8.0, 9.0)); + + vector dims2(2); + dims2[0] = 1; + dims2[1] = 3; + Vector d2 = Vector_(4, 6.0, 7.0, 8.0, 9.0); + actual.append(d2, dims2); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST(VectorValues, hasSameStructure) { VectorValues v1(2, 3);