Added raw vector updates for whole VectorMaps

cpp/VectorMap.cpp

@@ -132,6 +132,33 @@ VectorMap VectorMap::zero(const VectorMap& x) {
 	return cloned.zero();
 }
 
+/* ************************************************************************* */
+Vector VectorMap::vector() const {
+	Vector result(dim());
+
+	size_t cur_dim = 0;
+	Symbol j; Vector vj;
+	FOREACH_PAIR(j, vj, values) {
+		subInsert(result, vj, cur_dim);
+		cur_dim += vj.size();
+	}
+	return result;
+}
+
+/* ************************************************************************* */
+VectorMap VectorMap::vectorUpdate(const Vector& delta) const {
+	VectorMap result;
+
+	size_t cur_dim = 0;
+	Symbol j; Vector vj;
+	FOREACH_PAIR(j, vj, values) {
+		result.insert(j, vj + sub(delta, cur_dim, cur_dim + vj.size()));
+		cur_dim += vj.size();
+	}
+
+	return result;
+}
+
 /* ************************************************************************* */
 VectorMap expmap(const VectorMap& original, const VectorMap& delta)
 {

cpp/VectorMap.h

@@ -39,6 +39,9 @@ namespace gtsam {
     /** return all the nodes in the graph **/
     std::vector<Symbol> get_names() const;
 
+    /** convert into a single large vector */
+    Vector vector() const;
+
     /** Insert a value into the configuration with a given index */
     VectorMap& insert(const Symbol& name, const Vector& v);
 
@@ -111,6 +114,9 @@ namespace gtsam {
     /** Dot product */
     double dot(const VectorMap& b) const;
     
+    /** Adds the contents of a vector to the config - assumes ordering is identical */
+    VectorMap vectorUpdate(const Vector& delta) const;
+
 		/** Set all vectors to zero */
     VectorMap& zero();
 

cpp/testVectorMap.cpp

@@ -59,6 +59,29 @@ TEST( VectorMap, equals2 )
    CHECK(!cfg2.equals(cfg1));
  }
 
+/* ************************************************************************* */
+TEST( VectorMap, fullVector)
+{
+	VectorMap c = smallVectorMap();
+	Vector actual = c.vector();
+	Vector expected = Vector_(6, 0.0, -1.0, 0.0,  0.0, 1.5,  0.0);
+	CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST( VectorMap, vectorUpdate)
+{
+	VectorMap c = smallVectorMap();
+	Vector delta = Vector_(6, 1.0, 2.0, 3.0,  4.0, 5.0,  6.0);
+
+	VectorMap actual = c.vectorUpdate(delta);
+	VectorMap expected;
+	expected.insert(l1, Vector_(2,  1.0, 1.0));
+	expected.insert(x1, Vector_(2,  3.0,  4.0));
+	expected.insert(x2, Vector_(2,  6.5,  6.0));
+	CHECK(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 
 #include <limits>