Working on unordered VectorValues.

gtsam/linear/VectorValuesUnordered.cpp

@@ -16,51 +16,22 @@
  * @author Alex Cunningham
  */
 
-#include <gtsam/base/FastVector.h>
 #include <gtsam/linear/VectorValuesUnordered.h>
 
-#include <boost/iterator/counting_iterator.hpp>
+#include <boost/range/combine.hpp>
+#include <boost/range/numeric.hpp>
+#include <boost/range/adaptor/map.hpp>
+#include <boost/range/adaptor/transformed.hpp>
 
 using namespace std;
 
 namespace gtsam {
 
 /* ************************************************************************* */
-VectorValuesUnordered VectorValuesUnordered::Zero(const VectorValuesUnordered& x) {
+void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const {
-  VectorValuesUnordered result;
-  result.values_.resize(x.size());
-  for(size_t j=0; j<x.size(); ++j)
-    result.values_[j] = Vector::Zero(x.values_[j].size());
-  return result;
-}
-
-/* ************************************************************************* */
-vector<size_t> VectorValuesUnordered::dims() const {
-  std::vector<size_t> result(this->size());
-  for(Index j = 0; j < this->size(); ++j)
-    result[j] = this->dim(j);
-  return result;
-}
-
-/* ************************************************************************* */
-void VectorValuesUnordered::insert(Index j, const Vector& value) {
-  // Make sure j does not already exist
-  if(exists(j))
-    throw invalid_argument("VectorValues: requested variable index to insert already exists.");
-
-  // If this adds variables at the end, insert zero-length entries up to j
-  if(j >= size())
-    values_.resize(j+1);
-
-  // Assign value
-  values_[j] = value;
-}
-
-/* ************************************************************************* */
-void VectorValuesUnordered::print(const std::string& str, const IndexFormatter& formatter) const {
   std::cout << str << ": " << size() << " elements\n";
-  for (Index var = 0; var < size(); ++var)
+  BOOST_FOREACH(const value_type& key_value, *this)
-    std::cout << "  " << formatter(var) << ": \n" << (*this)[var] << "\n";
+    std::cout << "  " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n";
   std::cout.flush();
 }
 
@@ -68,67 +39,55 @@ void VectorValuesUnordered::print(const std::string& str, const IndexFormatter&
 bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const {
   if(this->size() != x.size())
     return false;
-  for(Index j=0; j < size(); ++j)
+  typedef boost::tuple<value_type, value_type> ValuePair;
-    if(!equal_with_abs_tol(values_[j], x.values_[j], tol))
+  BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) {
+    if(values.get<0>.first != values.get<1>.first ||
+      !equal_with_abs_tol(values.get<0>.second, values.get<1>.second, tol))
       return false;
+  }
   return true;
 }
 
 /* ************************************************************************* */
-void VectorValuesUnordered::resize(Index nVars, size_t varDim) {
+const Vector VectorValuesUnordered::asVector() const
-  values_.resize(nVars);
+{
-  for(Index j = 0; j < nVars; ++j)
+  using boost::adaptors::map_values;
-    values_[j] = Vector(varDim);
+  using boost::adaptors::transformed;
+
+  // Count dimensions
+  const DenseIndex totalDim = boost::accumulate(*this | map_values | transformed(&Vector::size), 0);
+
+  // Copy vectors
+  Vector result;
+  DenseIndex pos = 0;
+  BOOST_FOREACH(const Vector& v, *this | map_values) {
+    result.segment(pos, v.size()) = v;
+    pos += v.size();
+  }
+
+  return result;
 }
 
 /* ************************************************************************* */
-void VectorValuesUnordered::resizeLike(const VectorValuesUnordered& other) {
+const Vector VectorValuesUnordered::vector(const std::vector<Key>& keys) const
-  values_.resize(other.size());
+{
-  for(Index j = 0; j < other.size(); ++j)
+  // Count dimensions and collect pointers to avoid double lookups
-    values_[j].resize(other.values_[j].size());
+  DenseIndex totalDim = 0;
+  std::vector<const Vector*> items(keys.size());
+  for(size_t i = 0; i < keys.size(); ++i) {
+    items[i] = &at(i);
+    totalDim += items[i]->size();
   }
 
-/* ************************************************************************* */
+  // Copy vectors
-VectorValuesUnordered VectorValuesUnordered::SameStructure(const VectorValuesUnordered& other) {
+  Vector result(totalDim);
-  VectorValuesUnordered ret;
+  DenseIndex pos = 0;
-  ret.resizeLike(other);
+  BOOST_FOREACH(const Vector *v, items) {
-  return ret;
+    result.segment(pos, v->size()) = *v;
+    pos += v->size();
   }
 
-/* ************************************************************************* */
+  return result;
-VectorValuesUnordered VectorValuesUnordered::Zero(Index nVars, size_t varDim) {
-  VectorValuesUnordered ret(nVars, varDim);
-  ret.setZero();
-  return ret;
-}
-
-/* ************************************************************************* */
-void VectorValuesUnordered::setZero() {
-  BOOST_FOREACH(Vector& v, *this) {
-    v.setZero();
-  }
-}
-
-/* ************************************************************************* */
-const Vector VectorValuesUnordered::asVector() const {
-  return internal::extractVectorValuesSlices(*this,
-    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true);
-}
-
-/* ************************************************************************* */
-const Vector VectorValuesUnordered::vector(const std::vector<Index>& indices) const {
-  return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end());
-}
-
-/* ************************************************************************* */
-bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered& other) const {
-  if(this->size() != other.size())
-    return false;
-  for(size_t j = 0; j < size(); ++j)
-    // Directly accessing maps instead of using VV::dim in case some values are empty
-    if(this->values_[j].rows() != other.values_[j].rows())
-      return false;
-  return true;
 }
 
 /* ************************************************************************* */
@@ -137,16 +96,20 @@ void VectorValuesUnordered::swap(VectorValuesUnordered& other) {
 }
 
 /* ************************************************************************* */
-double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const {
+double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
-  double result = 0.0;
+{
   if(this->size() != v.size())
-    throw invalid_argument("VectorValues::dot called with different vector sizes");
+    throw invalid_argument("VectorValues::dot called with a VectorValues of different structure");
-  for(Index j = 0; j < this->size(); ++j)
+  double result = 0.0;
-    // Directly accessing maps instead of using VV::dim in case some values are empty
+  typedef boost::tuple<value_type, value_type> ValuePair;
-    if(this->values_[j].size() == v.values_[j].size())
+  using boost::adaptors::map_values;
-      result += this->values_[j].dot(v.values_[j]);
+  BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) {
-    else
+    assert_throw(values.get<0>().first == values.get<1>.first,
-      throw invalid_argument("VectorValues::dot called with different vector sizes");
+      std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
+    assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
+      std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
+    result += values.get<0>().second.dot(values.get<1>().second);
+  }
   return result;
 }
 
@@ -158,9 +121,9 @@ double VectorValuesUnordered::norm() const {
 /* ************************************************************************* */
 double VectorValuesUnordered::squaredNorm() const {
   double sumSquares = 0.0;
-  for(Index j = 0; j < this->size(); ++j)
+  using boost::adaptors::map_values;
-    // Directly accessing maps instead of using VV::dim in case some values are empty
+  BOOST_FOREACH(const Vector& v, *this | map_values)
-    sumSquares += this->values_[j].squaredNorm();
+    sumSquares += v.squaredNorm();
   return sumSquares;
 }
 

gtsam/linear/VectorValuesUnordered.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/base/Vector.h>
+#include <gtsam/base/FastMap.h>
 #include <gtsam/global_includes.h>
 
 #include <boost/format.hpp>
@@ -39,9 +40,9 @@ namespace gtsam {
    * or creating this class in unit tests and examples where speed is not important,
    * you can use a simple interface:
    *  - The default constructor VectorValues() to create this class
-   *  - insert(Index, const Vector&) to add vector variables
+   *  - insert(Key, const Vector&) to add vector variables
-   *  - operator[](Index) for read and write access to stored variables
+   *  - operator[](Key) for read and write access to stored variables
-   *  - \ref exists (Index) to check if a variable is present
+   *  - \ref exists (Key) to check if a variable is present
    *  - Other facilities like iterators, size(), dim(), etc.
    *
    * Indices can be non-consecutive and inserted out-of-order, but you should not
@@ -75,7 +76,7 @@ namespace gtsam {
    *  - Allocate space ahead of time using a pre-allocating constructor
    *    (\ref AdvancedConstructors "Advanced Constructors"), Zero(),
    *    SameStructure(), resize(), or append().  Do not use
-   *    insert(Index, const Vector&), which always has to re-allocate the
+   *    insert(Key, const Vector&), which always has to re-allocate the
    *    internal vector.
    *  - The vector() function permits access to the underlying Vector, for
    *    doing mathematical or other operations that require all values.
@@ -90,7 +91,7 @@ namespace gtsam {
    */
   class GTSAM_EXPORT VectorValuesUnordered {
   protected:
-    typedef std::vector<Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
+    typedef FastMap<Key, Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
     Values values_; ///< Collection of Vectors making up this VectorValues
 
   public:
@@ -99,6 +100,7 @@ namespace gtsam {
     typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
     typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
     typedef boost::shared_ptr<VectorValuesUnordered> shared_ptr; ///< shared_ptr to this class
+    typedef Values::value_type value_type; ///< Typedef to pair<Key, Vector>, a key-value pair
 
     /// @name Standard Constructors
     /// @{
@@ -108,48 +110,54 @@ namespace gtsam {
      */
     VectorValuesUnordered() {}
 
-    /** Named constructor to create a VectorValues of the same structure of the
-     * specified one, but filled with zeros.
-     * @return
-     */
-    static VectorValuesUnordered Zero(const VectorValuesUnordered& model);
-
     /// @}
     /// @name Standard Interface
     /// @{
 
-    /** Number of variables stored, always 1 more than the highest variable index,
+    /** Number of variables stored. */
-     * even if some variables with lower indices are not present. */
+    Key size() const { return values_.size(); }
-    Index size() const { return values_.size(); }
 
     /** Return the dimension of variable \c j. */
-    size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); }
+    size_t dim(Key j) const { return at(j).rows(); }
 
-    /** Return the dimension of each vector in this container */
+    /** Check whether a variable with key \c j exists. */
-    std::vector<size_t> dims() const;
+    bool exists(Key j) const { return find(j) != end(); }
 
-    /** Check whether a variable with index \c j exists. */
+    /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */
-    bool exists(Index j) const { return j < size() && values_[j].rows() > 0; }
+    Vector& at(Key j) {
+      iterator item = find(j);
+      if(item == end())
+        throw std::out_of_range(
+        "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
+      else
+        return item->second;
+    }
 
-    /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
+    /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */
-    Vector& at(Index j) { checkExists(j); return values_[j]; }
+    const Vector& at(Key j) const {
+      const_iterator item = find(j);
+      if(item == end())
+        throw std::out_of_range(
+        "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
+      else
+        return item->second;
+    }
 
-    /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
+    /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to at(Key). */
-    const Vector& at(Index j) const { checkExists(j); return values_[j]; }
+    Vector& operator[](Key j) { return at(j); }
 
-    /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */
+    /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Key). */
-    Vector& operator[](Index j) { return at(j); }
+    const Vector& operator[](Key j) const { return at(j); }
 
-    /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */
+    /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c j is already used.
-    const Vector& operator[](Index j) const { return at(j); }
-
-    /** Insert a vector \c value with index \c j.
-     * Causes reallocation, but can insert values in any order.
-     * Throws an invalid_argument exception if the index \c j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated.
      */
-    void insert(Index j, const Vector& value);
+    void insert(Key j, const Vector& value) {
+      if(!values_.insert(std::make_pair(j, value)).second)
+        throw std::invalid_argument(
+        "Requested to insert variable '" + DefaultKeyFormatter(j) + "' already in this VectorValues.");
+    }
 
     iterator begin()                      { return values_.begin(); }  ///< Iterator over variables
     const_iterator begin() const          { return values_.begin(); }  ///< Iterator over variables
@@ -160,99 +168,28 @@ namespace gtsam {
     reverse_iterator rend()                { return values_.rend(); }   ///< Reverse iterator over variables
     const_reverse_iterator rend() const   { return values_.rend(); }   ///< Reverse iterator over variables
 
+    /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */
+    iterator find(Key j) { return values_.find(j); }
+
+    /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */
+    const_iterator find(Key j) const { return values_.find(j); }
+
     /** print required by Testable for unit testing */
     void print(const std::string& str = "VectorValues: ",
-        const IndexFormatter& formatter = DefaultIndexFormatter) const;
+        const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
     /** equals required by Testable for unit testing */
     bool equals(const VectorValuesUnordered& x, double tol = 1e-9) const;
 
     /// @{
-    /// \anchor AdvancedConstructors
-    /// @name Advanced Constructors
-    /// @}
-
-    /** Construct from a container of variable dimensions (in variable order), without initializing any values. */
-    template<class CONTAINER>
-    explicit VectorValuesUnordered(const CONTAINER& dimensions) { this->append(dimensions); }
-
-    /** Construct to hold nVars vectors of varDim dimension each. */
-    VectorValuesUnordered(Index nVars, size_t varDim) { this->resize(nVars, varDim); }
-
-    /** Named constructor to create a VectorValues that matches the structure of
-     * the specified VectorValues, but do not initialize the new values. */
-    static VectorValuesUnordered SameStructure(const VectorValuesUnordered& other);
-
-    /** Named constructor to create a VectorValues from a container of variable
-     * dimensions that is filled with zeros.
-     * @param dimensions A container of the dimension of each variable to create.
-     */
-    template<class CONTAINER>
-    static VectorValuesUnordered Zero(const CONTAINER& dimensions);
-
-    /** Named constructor to create a VectorValues filled with zeros that has
-     * \c nVars variables, each of dimension \c varDim
-     * @param nVars The number of variables to create
-     * @param varDim The dimension of each variable
-     * @return The new VectorValues
-     */
-    static VectorValuesUnordered Zero(Index nVars, size_t varDim);
-
-    /// @}
     /// @name Advanced Interface
     /// @{
 
-    /** Resize this VectorValues to have identical structure to other, leaving
-     * this VectorValues with uninitialized values.
-     * @param other The VectorValues whose structure to copy
-     */
-    void resizeLike(const VectorValuesUnordered& other);
-
-    /** Resize the VectorValues to hold \c nVars variables, each of dimension
-     * \c varDim.  Any individual vectors that do not change size will keep
-     * their values, but any new or resized vectors will be uninitialized.
-     * @param nVars The number of variables to create
-     * @param varDim The dimension of each variable
-     */
-    void resize(Index nVars, size_t varDim);
-
-    /** Resize the VectorValues to contain variables of the dimensions stored
-     * in \c dimensions.  Any individual vectors that do not change size will keep
-     * their values, but any new or resized vectors will be uninitialized.
-     * @param dimensions A container of the dimension of each variable to create.
-     */
-    template<class CONTAINER>
-    void resize(const CONTAINER& dimensions);
-
-    /** Append to the VectorValues to additionally contain variables of the
-     * dimensions stored in \c dimensions.  The new variables are uninitialized,
-     * but this function is used to pre-allocate space for performance.  This
-     * function preserves the original data, so all previously-existing variables
-     * are left unchanged.
-     * @param dimensions A container of the dimension of each variable to create.
-     */
-    template<class CONTAINER>
-    void append(const CONTAINER& dimensions);
-
-    /** Removes the last subvector from the VectorValues */
-    void pop_back() { values_.pop_back(); };
-
-    /** Set all entries to zero, does not modify the size. */
-    void setZero();
-
     /** Retrieve the entire solution as a single vector */
     const Vector asVector() const;
 
-    /** Access a vector that is a subset of relevant indices */
+    /** Access a vector that is a subset of relevant keys. */
-    const Vector vector(const std::vector<Index>& indices) const;
+    const Vector vector(const std::vector<Key>& keys) const;
-
-    /** Check whether this VectorValues has the same structure, meaning has the
-     * same number of variables and that all variables are of the same dimension,
-     * as another VectorValues
-     * @param other The other VectorValues with which to compare structure
-     * @return \c true if the structure is the same, \c false if not.
-     */
-    bool hasSameStructure(const VectorValuesUnordered& other) const;
 
     /**
      * Swap the data in this VectorValues with another.
@@ -264,7 +201,8 @@ namespace gtsam {
     /// @{
 
     /** Dot product with another VectorValues, interpreting both as vectors of
-     * their concatenated values. */
+    * their concatenated values.  Both VectorValues must have the
+    * same structure (checked when NDEBUG is not defined). */
     double dot(const VectorValuesUnordered& v) const;
 
     /** Vector L2 norm */
@@ -302,38 +240,26 @@ namespace gtsam {
 
     /// @}
 
-  private:
-    // Throw an exception if j does not exist
-    void checkExists(Index j) const {
-      if(!exists(j)) {
-        const std::string msg =
-            (boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str();
-        throw std::out_of_range(msg);
-      }
-    }
-
-  public:
-
     /**
      * scale a vector by a scalar
      */
     friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) {
       VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v);
-      for(Index j = 0; j < v.size(); ++j)
+      for(Key j = 0; j < v.size(); ++j)
         result.values_[j] = a * v.values_[j];
       return result;
     }
 
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void scal(double alpha, VectorValuesUnordered& x) {
-      for(Index j = 0; j < x.size(); ++j)
+      for(Key j = 0; j < x.size(); ++j)
         x.values_[j] *= alpha;
     }
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void axpy(double alpha, const VectorValuesUnordered& x, VectorValuesUnordered& y) {
       if(x.size() != y.size())
         throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
-      for(Index j = 0; j < x.size(); ++j)
+      for(Key j = 0; j < x.size(); ++j)
         if(x.values_[j].size() == y.values_[j].size())
           y.values_[j] += alpha * x.values_[j];
         else
@@ -341,7 +267,7 @@ namespace gtsam {
     }
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void sqrt(VectorValuesUnordered &x) {
-      for(Index j = 0; j < x.size(); ++j)
+      for(Key j = 0; j < x.size(); ++j)
         x.values_[j] = x.values_[j].cwiseSqrt();
     }
 
@@ -349,7 +275,7 @@ namespace gtsam {
     friend void ediv(const VectorValuesUnordered& numerator, const VectorValuesUnordered& denominator, VectorValuesUnordered &result) {
       if(numerator.size() != denominator.size() || numerator.size() != result.size())
         throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
-      for(Index j = 0; j < numerator.size(); ++j)
+      for(Key j = 0; j < numerator.size(); ++j)
         if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size())
           result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
         else
@@ -360,7 +286,7 @@ namespace gtsam {
     friend void edivInPlace(VectorValuesUnordered& x, const VectorValuesUnordered& y) {
       if(x.size() != y.size())
         throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
-      for(Index j = 0; j < x.size(); ++j)
+      for(Key j = 0; j < x.size(); ++j)
         if(x.values_[j].size() == y.values_[j].size())
           x.values_[j].array() /= y.values_[j].array();
         else
@@ -376,80 +302,4 @@ namespace gtsam {
     }
   }; // VectorValues definition
 
-  // Implementations of template and inline functions
-
-  /* ************************************************************************* */
-  template<class CONTAINER>
-  void VectorValuesUnordered::resize(const CONTAINER& dimensions) {
-    values_.clear();
-    append(dimensions);
-  }
-
-  /* ************************************************************************* */
-  template<class CONTAINER>
-  void VectorValuesUnordered::append(const CONTAINER& dimensions) {
-    size_t i = size();
-    values_.resize(size() + dimensions.size());
-    BOOST_FOREACH(size_t dim, dimensions) {
-      values_[i] = Vector(dim);
-      ++ i;
-    }
-  }
-
-  /* ************************************************************************* */
-  template<class CONTAINER>
-  VectorValuesUnordered VectorValuesUnordered::Zero(const CONTAINER& dimensions) {
-    VectorValuesUnordered ret;
-    ret.values_.resize(dimensions.size());
-    size_t i = 0;
-    BOOST_FOREACH(size_t dim, dimensions) {
-      ret.values_[i] = Vector::Zero(dim);
-      ++ i;
-    }
-    return ret;
-  }
-
-  namespace internal {
-    /* ************************************************************************* */
-    // Helper function, extracts vectors with variable indices
-    // in the first and last iterators, and concatenates them in that order into the
-    // output.
-    template<typename ITERATOR>
-    const Vector extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
-      // Find total dimensionality
-      size_t dim = 0;
-      for(ITERATOR j = first; j != last; ++j)
-        // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
-        if(!allowNonexistant || values.exists(*j))
-          dim += values.dim(*j);
-
-      // Copy vectors
-      Vector ret(dim);
-      size_t varStart = 0;
-      for(ITERATOR j = first; j != last; ++j) {
-        // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
-        if(!allowNonexistant || values.exists(*j)) {
-          ret.segment(varStart, values.dim(*j)) = values[*j];
-          varStart += values.dim(*j);
-        }
-      }
-      return ret;
-    }
-
-    /* ************************************************************************* */
-    // Helper function, writes to the variables in values
-    // with indices iterated over by first and last, interpreting vector as the
-    // concatenated vectors to write.
-    template<class VECTOR, typename ITERATOR>
-    void writeVectorValuesSlices(const VECTOR& vector, VectorValuesUnordered& values, ITERATOR first, ITERATOR last) {
-      // Copy vectors
-      size_t varStart = 0;
-      for(ITERATOR j = first; j != last; ++j) {
-        values[*j] = vector.segment(varStart, values[*j].rows());
-        varStart += values[*j].rows();
-      }
-      assert(varStart == vector.rows());
-    }
-  }
-
 } // \namespace gtsam