From 9de0cacc273c05a120427270949fea6afc70e3de Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 26 Oct 2013 22:04:37 +0000
Subject: [PATCH] Added vector(dims) method, and introduced Dims typedef

---
 gtsam/linear/VectorValues.cpp                 | 18 +++++++++++-
 gtsam/linear/VectorValues.h                   |  6 +++-
 .../tests/testVectorValuesUnordered.cpp       | 28 +++++++++++++++++++
 3 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index cbf573e98..8a0e7fa00 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -45,7 +45,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  VectorValues::VectorValues(const Vector& x, const map<Key, size_t>& dims) {
+  VectorValues::VectorValues(const Vector& x, const Dims& dims) {
     typedef pair<Key, size_t> Pair;
     size_t j = 0;
     BOOST_FOREACH(const Pair& v, dims) {
@@ -162,6 +162,22 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  Vector VectorValues::vector(const Dims& keys) const
+  {
+    // Count dimensions
+    DenseIndex totalDim = 0;
+    BOOST_FOREACH(size_t dim, keys | map_values)
+      totalDim += dim;
+    Vector result(totalDim);
+    size_t j = 0;
+    BOOST_FOREACH(const Dims::value_type& it, keys) {
+      result.segment(j,it.second) = at(it.first);
+      j += it.second;
+    }
+    return result;
+  }
+
   /* ************************************************************************* */
   void VectorValues::swap(VectorValues& other) {
     this->values_.swap(other.values_);
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index d27db427f..1a82c0bbb 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -100,6 +100,7 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
     typedef Values::value_type value_type; ///< Typedef to pair<Key, Vector>, a key-value pair
     typedef value_type KeyValuePair; ///< Typedef to pair<Key, Vector>, a key-value pair
+    typedef std::map<Key,size_t> Dims;
 
     /// @name Standard Constructors
     /// @{
@@ -124,7 +125,7 @@ namespace gtsam {
     VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {}
 
     /** Constructor from Vector. */
-    VectorValues(const Vector& c, const std::map<Key,size_t>& dims);
+    VectorValues(const Vector& c, const Dims& dims);
 
     /** Create a VectorValues with the same structure as \c other, but filled with zeros. */
     static VectorValues Zero(const VectorValues& other);
@@ -250,6 +251,9 @@ namespace gtsam {
     /** Access a vector that is a subset of relevant keys. */
     Vector vector(const FastVector<Key>& keys) const;
 
+    /** Access a vector that is a subset of relevant keys, dims version. */
+    Vector vector(const Dims& dims) const;
+
     /** Swap the data in this VectorValues with another. */
     void swap(VectorValues& other);
 
diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValuesUnordered.cpp
index 7075b9f51..fbe56cb33 100644
--- a/gtsam/linear/tests/testVectorValuesUnordered.cpp
+++ b/gtsam/linear/tests/testVectorValuesUnordered.cpp
@@ -185,6 +185,34 @@ TEST(VectorValues, convert)
   // Test other direction, note vector() is not guaranteed to give right result
   FastVector<Key> keys = list_of(0)(1)(2)(5);
   EXPECT(assert_equal(x, actual.vector(keys)));
+
+  // Test version with dims argument
+  EXPECT(assert_equal(x, actual.vector(dims)));
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, vector_sub)
+{
+  VectorValues vv;
+  vv.insert(0, (Vec(1) << 1));
+  vv.insert(1, (Vec(2) << 2, 3));
+  vv.insert(2, (Vec(2) << 4, 5));
+  vv.insert(5, (Vec(2) << 6, 7));
+  vv.insert(7, (Vec(2) << 8, 9));
+
+  std::map<Key,size_t> dims;
+  dims.insert(make_pair(0,1));
+  dims.insert(make_pair(5,2));
+
+  Vector expected(3);
+  expected << 1, 6, 7;
+
+  // Test FastVector version
+  FastVector<Key> keys = list_of(0)(5);
+  EXPECT(assert_equal(expected, vv.vector(keys)));
+
+  // Test version with dims argument
+  EXPECT(assert_equal(expected, vv.vector(dims)));
 }
 
 /* ************************************************************************* */