VectorValues constructor and append with initial values

gtsam/linear/VectorValues.h

@@ -179,6 +179,12 @@ namespace gtsam {
     template<class CONTAINER>
     explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); }
 
+    /** Construct from a container of variable dimensions (in variable order), with initial values. */
+    template<class CONTAINER>
+    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<class CONTAINER>
     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<class CONTAINER>
+    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<class CONTAINER>
+  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<class CONTAINER>
   VectorValues VectorValues::Zero(const CONTAINER& dimensions) {

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<size_t> 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<size_t> 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);