diff --git a/cpp/VectorMap.cpp b/cpp/VectorMap.cpp index ae56ea1ba..88577c47d 100644 --- a/cpp/VectorMap.cpp +++ b/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) { diff --git a/cpp/VectorMap.h b/cpp/VectorMap.h index b1944e034..744e3b957 100644 --- a/cpp/VectorMap.h +++ b/cpp/VectorMap.h @@ -39,6 +39,9 @@ namespace gtsam { /** return all the nodes in the graph **/ std::vector 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(); diff --git a/cpp/testVectorMap.cpp b/cpp/testVectorMap.cpp index d2c9d1ea7..7178ea20e 100644 --- a/cpp/testVectorMap.cpp +++ b/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