Unit tests pass with piecewise VectorValues implementation (for fast permutations)

gtsam/linear/GaussianFactorGraph.cpp

@@ -607,11 +607,11 @@ break;
 
   /* ************************************************************************* */
   void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
-    r.asVector() = Vector::Zero(r.dim());
+    r.setZero();
     Index i = 0;
     BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
-      SubVector &y = r[i];
+      Vector &y = r[i];
       for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
         y += Ai->getA(j) * x[*j];
       }
@@ -621,7 +621,7 @@ break;
 
   /* ************************************************************************* */
   void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
-    x.asVector() = Vector::Zero(x.dim());
+    x.setZero();
     Index i = 0;
     BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);

gtsam/linear/VectorValues.cpp

@@ -20,6 +20,8 @@
 #include <gtsam/inference/Permutation.h>
 #include <gtsam/linear/VectorValues.h>
 
+#include <boost/iterator/counting_iterator.hpp>
+
 using namespace std;
 
 namespace gtsam {
@@ -29,13 +31,13 @@ VectorValues VectorValues::Zero(const VectorValues& x) {
   VectorValues result;
   result.values_.resize(x.size());
   for(size_t j=0; j<x.size(); ++j)
-    result.values_[j] = Vector::Zero(x.dim(j));
+    result.values_[j] = Vector::Zero(x.values_[j].size());
   return result;
 }
 
 /* ************************************************************************* */
 vector<size_t> VectorValues::dims() const {
-  vector<size_t> result(this->size());
+  std::vector<size_t> result(this->size());
   for(Index j = 0; j < this->size(); ++j)
     result[j] = this->dim(j);
   return result;
@@ -67,7 +69,7 @@ void VectorValues::print(const std::string& str, const IndexFormatter& formatter
 bool VectorValues::equals(const VectorValues& x, double tol) const {
   if(this->size() != x.size())
     return false;
-  for(size_t j=0; j<size(); ++j)
+  for(Index j=0; j < size(); ++j)
     if(!equal_with_abs_tol(values_[j], x.values_[j], tol))
       return false;
   return true;
@@ -75,27 +77,16 @@ bool VectorValues::equals(const VectorValues& x, double tol) const {
 
 /* ************************************************************************* */
 void VectorValues::resize(Index nVars, size_t varDim) {
-  maps_.clear();
+  values_.resize(nVars);
-  maps_.reserve(nVars);
+  for(Index j = 0; j < nVars; ++j)
-  values_.resize(nVars * varDim);
+    values_[j] = Vector(varDim);
-  int varStart = 0;
-  for (Index j = 0; j < nVars; ++j) {
-    maps_.push_back(values_.segment(varStart, varDim));
-    varStart += varDim;
-  }
 }
 
 /* ************************************************************************* */
 void VectorValues::resizeLike(const VectorValues& other) {
-  values_.resize(other.dim());
+  values_.resize(other.size());
-  // Create SubVectors referencing our values_ vector
+  for(Index j = 0; j < other.size(); ++j)
-  maps_.clear();
+    values_[j].resize(other.values_[j].size());
-  maps_.reserve(other.size());
-  int varStart = 0;
-  BOOST_FOREACH(const SubVector& value, other) {
-    maps_.push_back(values_.segment(varStart, value.rows()));
-    varStart += value.rows();
-  }
 }
 
 /* ************************************************************************* */
@@ -114,7 +105,20 @@ VectorValues VectorValues::Zero(Index nVars, size_t varDim) {
 
 /* ************************************************************************* */
 void VectorValues::setZero() {
-  values_.setZero();
+  BOOST_FOREACH(Vector& v, *this) {
+    v.setZero();
+  }
+}
+
+/* ************************************************************************* */
+const Vector VectorValues::asVector() const {
+  return internal::extractVectorValuesSlices(*this,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true);
+}
+
+/* ************************************************************************* */
+const Vector VectorValues::vector(const std::vector<Index>& indices) const {
+  return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end());
 }
 
 /* ************************************************************************* */
@@ -123,90 +127,95 @@ bool VectorValues::hasSameStructure(const VectorValues& other) const {
     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->maps_[j].rows() != other.maps_[j].rows())
+    if(this->values_[j].rows() != other.values_[j].rows())
       return false;
   return true;
 }
 
 /* ************************************************************************* */
-VectorValues VectorValues::permute(const Permutation& permutation) const {
+void VectorValues::permuteInPlace(const Permutation& permutation) {
-  // Create result and allocate space
+  // Allocate new values
-  VectorValues lhs;
+  Values newValues(this->size());
-  lhs.values_.resize(this->dim());
-  lhs.maps_.reserve(this->size());
 
-  // Copy values from this VectorValues to the permuted VectorValues
+  // Swap values from this VectorValues to the permuted VectorValues
-  size_t lhsPos = 0;
+  for(size_t i = 0; i < this->size(); ++i)
-  for(size_t i = 0; i < this->size(); ++i) {
+    newValues[i].swap(this->at(permutation[i]));
-    // Map the next LHS subvector to the next slice of the LHS vector
-    lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size()));
-    // Copy the data from the RHS subvector to the LHS subvector
-    lhs.maps_[i] = this->at(permutation[i]);
-    // Increment lhs position
-    lhsPos += lhs.maps_[i].size();
-  }
 
-  return lhs;
+  // Swap the values into this VectorValues
+  values_.swap(newValues);
 }
 
 /* ************************************************************************* */
 void VectorValues::swap(VectorValues& other) {
   this->values_.swap(other.values_);
-  this->maps_.swap(other.maps_);
 }
 
 /* ************************************************************************* */
-double VectorValues::dot(const VectorValues& V) const {
+double VectorValues::dot(const VectorValues& v) const {
-
+  double result = 0.0;
+  if(this->size() != v.size())
+    throw invalid_argument("VectorValues::dot called with different vector sizes");
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    if(this->values_[j].size() == v.values_[j].size())
+      result += this->values_[j].dot(v.values_[j]);
+    else
+      throw invalid_argument("VectorValues::dot called with different vector sizes");
+  return result;
 }
 
 /* ************************************************************************* */
 double VectorValues::norm() const {
+  return std::sqrt(this->squaredNorm());
+}
 
+/* ************************************************************************* */
+double VectorValues::squaredNorm() const {
+  double sumSquares = 0.0;
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    sumSquares += this->values_[j].squaredNorm();
+  return sumSquares;
 }
 
 /* ************************************************************************* */
 VectorValues VectorValues::operator+(const VectorValues& c) const {
-  assert(this->hasSameStructure(c));
+  VectorValues result = SameStructure(*this);
-  VectorValues result(SameStructure(c));
+  if(this->size() != c.size())
-  result.values_ = this->values_ + c.values_;
+    throw invalid_argument("VectorValues::operator+ called with different vector sizes");
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    if(this->values_[j].size() == c.values_[j].size())
+      result.values_[j] = this->values_[j] + c.values_[j];
+    else
+      throw invalid_argument("VectorValues::operator- called with different vector sizes");
   return result;
 }
 
 /* ************************************************************************* */
 VectorValues VectorValues::operator-(const VectorValues& c) const {
-  assert(this->hasSameStructure(c));
+  VectorValues result = SameStructure(*this);
-  VectorValues result(SameStructure(c));
+  if(this->size() != c.size())
-  result.values_ = this->values_ - c.values_;
+    throw invalid_argument("VectorValues::operator- called with different vector sizes");
+  for(Index j = 0; j < this->size(); ++j)
+    // Directly accessing maps instead of using VV::dim in case some values are empty
+    if(this->values_[j].size() == c.values_[j].size())
+      result.values_[j] = this->values_[j] - c.values_[j];
+    else
+      throw invalid_argument("VectorValues::operator- called with different vector sizes");
   return result;
 }
 
 /* ************************************************************************* */
 void VectorValues::operator+=(const VectorValues& c) {
-  assert(this->hasSameStructure(c));
+  if(this->size() != c.size())
-  this->values_ += c.values_;
+    throw invalid_argument("VectorValues::operator+= called with different vector sizes");
-}
+  for(Index j = 0; j < this->size(); ++j)
-
+    // Directly accessing maps instead of using VV::dim in case some values are empty
-/* ************************************************************************* */
+    if(this->values_[j].size() == c.values_[j].size())
-Vector VectorValues::vector(const std::vector<Index>& indices) const {
+      this->values_[j] += c.values_[j];
-  if (indices.empty())
+    else
-    return Vector();
+      throw invalid_argument("VectorValues::operator+= called with different vector sizes");
-
-  // find dimensions
-  size_t d = 0;
-  BOOST_FOREACH(const Index& idx, indices)
-    d += dim(idx);
-
-  // copy out values
-  Vector result(d);
-  size_t curHead = 0;
-  BOOST_FOREACH(const Index& j, indices) {
-    const SubVector& vj = at(j);
-    size_t dj = (size_t) vj.rows();
-    result.segment(curHead, dj) = vj;
-    curHead += dj;
-  }
-  return result;
 }
 
 /* ************************************************************************* */

gtsam/linear/VectorValues.h

@@ -111,7 +111,7 @@ namespace gtsam {
     VectorValues() {}
 
     /** Named constructor to create a VectorValues of the same structure of the
-     * specifed one, but filled with zeros.
+     * specified one, but filled with zeros.
      * @return
      */
     static VectorValues Zero(const VectorValues& model);
@@ -127,14 +127,11 @@ namespace gtsam {
     /** Return the dimension of variable \c j. */
     size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); }
 
-    /** Return the summed dimensionality of all variables. */
-    //size_t dim() const { return values_.rows(); }
-
     /** Return the dimension of each vector in this container */
     std::vector<size_t> dims() const;
 
     /** Check whether a variable with index \c j exists. */
-    bool exists(Index j) const { return j < size() && (*this)[j].rows() > 0; }
+    bool exists(Index j) const { return j < size() && values_[j].rows() > 0; }
 
     /** 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). */
     Vector& at(Index j) { checkExists(j); return values_[j]; }
@@ -149,8 +146,8 @@ namespace gtsam {
     const Vector& operator[](Index j) const { return at(j); }
 
     /** Insert a vector \c value with index \c j.
-     * Causes reallocation. Can be used to insert values in any order, but
+     * Causes reallocation, but can insert values in any order.
-     * throws an invalid_argument exception if the index \c j is already used.
+     * 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.
      */
@@ -179,10 +176,10 @@ namespace gtsam {
 
     /** Construct from a container of variable dimensions (in variable order), without initializing any values. */
     template<class CONTAINER>
-    explicit VectorValues(const CONTAINER& dimensions) { append(dimensions); }
+    explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); }
 
     /** Construct to hold nVars vectors of varDim dimension each. */
-    VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); }
+    VectorValues(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. */
@@ -214,17 +211,16 @@ namespace gtsam {
     void resizeLike(const VectorValues& other);
 
     /** Resize the VectorValues to hold \c nVars variables, each of dimension
-     * \c varDim, not preserving any data.  After calling this function, all
+     * \c varDim.  Any individual vectors that do not change size will keep
-     * variables will be uninitialized.
+     * 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, not preserving any data.  The new variables are
+     * in \c dimensions.  Any individual vectors that do not change size will keep
-     * uninitialized, but this function is used to pre-allocate space for
+     * their values, but any new or resized vectors will be uninitialized.
-     * performance.  After calling this function all variables will be uninitialized.
      * @param dimensions A container of the dimension of each variable to create.
      */
     template<class CONTAINER>
@@ -243,14 +239,11 @@ namespace gtsam {
     /** Set all entries to zero, does not modify the size. */
     void setZero();
 
-    /** Reference the entire solution vector (const version). */
+    /** Retrieve the entire solution as a single vector */
-    //const Vector& asVector() const { return values_; }
+    const Vector asVector() const;
-
-    /** Reference the entire solution vector. */
-    //Vector& asVector() { return values_; }
 
     /** Access a vector that is a subset of relevant indices */
-    Vector vector(const std::vector<Index>& indices) const;
+    const Vector vector(const std::vector<Index>& indices) 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,
@@ -263,7 +256,7 @@ namespace gtsam {
     /**
      * Permute the entries of this VectorValues in place
      */
-    void permute(const Permutation& permutation);
+    void permuteInPlace(const Permutation& permutation);
 
     /**
      * Swap the data in this VectorValues with another.
@@ -276,11 +269,14 @@ namespace gtsam {
 
     /** Dot product with another VectorValues, interpreting both as vectors of
      * their concatenated values. */
-    double dot(const VectorValues& V) const;
+    double dot(const VectorValues& v) const;
 
     /** Vector L2 norm */
     double norm() const;
 
+    /** Squared vector L2 norm */
+    double squaredNorm() const;
+
     /**
      * + operator does element-wise addition.  Both VectorValues must have the
      * same structure (checked when NDEBUG is not defined).
@@ -316,50 +312,54 @@ namespace gtsam {
     /**
      * scale a vector by a scalar
      */
-    friend VectorValues operator*(const double a, const VectorValues &V) {
+    friend VectorValues operator*(const double a, const VectorValues &v) {
-      VectorValues result(VectorValues::SameStructure(V));
+      VectorValues result = VectorValues::SameStructure(v);
-      result.values_ = a * V.values_;
+      for(Index 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 size_t dim(const VectorValues& V) {
-      return V.dim();
-    }
-    /// TODO: linear algebra interface seems to have been added for SPCG.
-    friend double dot(const VectorValues& V1, const VectorValues& V2) {
-      return gtsam::dot(V1.values_, V2.values_);
-    }
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void scal(double alpha, VectorValues& x) {
-      gtsam::scal(alpha, x.values_);
+      for(Index 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 VectorValues& x, VectorValues& y) {
-      gtsam::axpy(alpha, x.values_, y.values_);
+      if(x.size() != y.size())
+        throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
+      for(Index j = 0; j < x.size(); ++j)
+        if(x.values_[j].size() == y.values_[j].size())
+          y.values_[j] += alpha * x.values_[j];
+        else
+          throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
     }
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void sqrt(VectorValues &x) {
-      Vector y = gtsam::esqrt(x.values_);
+      for(Index j = 0; j < x.size(); ++j)
-      x.values_ = y;
+        x.values_[j] = x.values_[j].cwiseSqrt();
     }
 
     /// TODO: linear algebra interface seems to have been added for SPCG.
-    friend void ediv(const VectorValues& numerator,
+    friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) {
-        const VectorValues& denominator, VectorValues &result) {
+      if(numerator.size() != denominator.size() || numerator.size() != result.size())
-      assert(
+        throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
-          numerator.dim() == denominator.dim() && denominator.dim() == result.dim());
+      for(Index j = 0; j < numerator.size(); ++j)
-      const size_t sz = result.dim();
+        if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size())
-      for (size_t i = 0; i < sz; ++i)
+          result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
-        result.values_[i] = numerator.values_[i] / denominator.values_[i];
+        else
+          throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
     }
 
     /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void edivInPlace(VectorValues& x, const VectorValues& y) {
-      assert(x.dim() == y.dim());
+      if(x.size() != y.size())
-      const size_t sz = x.dim();
+        throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
-      for (size_t i = 0; i < sz; ++i)
+      for(Index j = 0; j < x.size(); ++j)
-        x.values_[i] /= y.values_[i];
+        if(x.values_[j].size() == y.values_[j].size())
+          x.values_[j].array() /= y.values_[j].array();
+        else
+          throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
     }
 
   private:
@@ -383,8 +383,8 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CONTAINER>
   void VectorValues::append(const CONTAINER& dimensions) {
-    values_.resize(size() + dimensions.size());
     size_t i = size();
+    values_.resize(size() + dimensions.size());
     BOOST_FOREACH(size_t dim, dimensions) {
       values_[i] = Vector(dim);
       ++ i;
@@ -395,10 +395,10 @@ namespace gtsam {
   template<class CONTAINER>
   VectorValues VectorValues::Zero(const CONTAINER& dimensions) {
     VectorValues ret;
-    values_.resize(dimensions.size());
+    ret.values_.resize(dimensions.size());
     size_t i = 0;
     BOOST_FOREACH(size_t dim, dimensions) {
-      values_[i] = Vector::Zero(dim);
+      ret.values_[i] = Vector::Zero(dim);
       ++ i;
     }
     return ret;
@@ -410,18 +410,23 @@ namespace gtsam {
     // in the first and last iterators, and concatenates them in that order into the
     // output.
     template<typename ITERATOR>
-  Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) {
+    const Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
       // Find total dimensionality
       int dim = 0;
       for(ITERATOR j = first; j != last; ++j)
-      dim += values[*j].rows();
+        // 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);
       int varStart = 0;
       for(ITERATOR j = first; j != last; ++j) {
-      ret.segment(varStart, values[*j].rows()) = values[*j];
+        // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
-      varStart += values[*j].rows();
+        if(!allowNonexistant || values.exists(*j)) {
+          ret.segment(varStart, values.dim(*j)) = values[*j];
+          varStart += values.dim(*j);
+        }
       }
       return ret;
     }

gtsam/linear/tests/testVectorValues.cpp

@@ -39,7 +39,6 @@ TEST(VectorValues, insert) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -59,7 +58,7 @@ TEST(VectorValues, insert) {
   EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
   EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
   EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
-  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
+  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
 
   // Check exceptions
   CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@@ -80,7 +79,6 @@ TEST(VectorValues, dimsConstructor) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(5, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -106,7 +104,6 @@ TEST(VectorValues, copyConstructor) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -149,7 +146,6 @@ TEST(VectorValues, assignment) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -188,7 +184,6 @@ TEST(VectorValues, SameStructure) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -220,7 +215,6 @@ TEST(VectorValues, Zero_fromModel) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -256,7 +250,6 @@ TEST(VectorValues, Zero_fromDims) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(5, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -273,7 +266,6 @@ TEST(VectorValues, Zero_fromUniform) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(6, actual.dim());
   LONGS_EQUAL(2, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -298,7 +290,6 @@ TEST(VectorValues, resizeLike) {
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
-  LONGS_EQUAL(7, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -328,7 +319,6 @@ TEST(VectorValues, resize_fromUniform) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(6, actual.dim());
   LONGS_EQUAL(2, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -355,7 +345,6 @@ TEST(VectorValues, resize_fromDims) {
 
   // Check dimensions
   LONGS_EQUAL(3, actual.size());
-  LONGS_EQUAL(5, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -383,7 +372,6 @@ TEST(VectorValues, append) {
 
   // Check dimensions
   LONGS_EQUAL(5, actual.size());
-  LONGS_EQUAL(13, actual.dim());
   LONGS_EQUAL(1, actual.dim(0));
   LONGS_EQUAL(2, actual.dim(1));
   LONGS_EQUAL(2, actual.dim(2));
@@ -443,7 +431,8 @@ TEST(VectorValues, permute) {
   permutation[2] = 3;
   permutation[3] = 1;
 
-  VectorValues actual = original.permute(permutation);
+  VectorValues actual = original;
+  actual.permuteInPlace(permutation);
 
   EXPECT(assert_equal(expected, actual));
 }

gtsam/nonlinear/DoglegOptimizerImpl.cpp

@@ -28,13 +28,12 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
   // Get magnitude of each update and find out which segment Delta falls in
   assert(Delta >= 0.0);
   double DeltaSq = Delta*Delta;
-  double x_u_norm_sq = dx_u.asVector().squaredNorm();
+  double x_u_norm_sq = dx_u.squaredNorm();
-  double x_n_norm_sq = dx_n.asVector().squaredNorm();
+  double x_n_norm_sq = dx_n.squaredNorm();
   if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl;
   if(DeltaSq < x_u_norm_sq) {
     // Trust region is smaller than steepest descent update
-    VectorValues x_d = VectorValues::SameStructure(dx_u);
+    VectorValues x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u;
-    x_d.asVector() = dx_u.asVector() * std::sqrt(DeltaSq / x_u_norm_sq);
     if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
     return x_d;
   } else if(DeltaSq < x_n_norm_sq) {
@@ -79,8 +78,7 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues&
 
   // Compute blended point
   if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
-  VectorValues blend = VectorValues::SameStructure(x_u);
+  VectorValues blend = (1. - tau) * x_u;  axpy(tau, x_n, blend);
-  blend.asVector() = (1. - tau) * x_u.asVector() + tau * x_n.asVector();
   return blend;
 }
 

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -177,7 +177,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
     result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose);
     gttoc(Dog_leg_point);
 
-    if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << std::endl;
+    if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl;
 
     gttic(retract);
     // Compute expmapped solution
@@ -208,7 +208,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
 
     if(rho >= 0.75) {
       // M agrees very well with f, so try to increase lambda
-      const double dx_d_norm = result.dx_d.vector().norm();
+      const double dx_d_norm = result.dx_d.norm();
       const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
 
       if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)

gtsam/nonlinear/ISAM2-impl.cpp

@@ -39,15 +39,15 @@ void ISAM2::Impl::AddVariables(
   // Add the new keys onto the ordering, add zeros to the delta for the new variables
   std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
   if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
-  const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
-  const size_t originalDim = delta.dim();
   const size_t originalnVars = delta.size();
   delta.append(dims);
-  delta.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
   deltaNewton.append(dims);
-  deltaNewton.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
   RgProd.append(dims);
-  RgProd.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
+  for(Index j = originalnVars; j < delta.size(); ++j) {
+    delta[j].setZero();
+    deltaNewton[j].setZero();
+    RgProd[j].setZero();
+  }
   {
     Index nextVar = originalnVars;
     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {

gtsam/nonlinear/ISAM2-inl.h

@@ -29,7 +29,7 @@ namespace gtsam {
 template<class VALUE>
 VALUE ISAM2::calculateEstimate(Key key) const {
   const Index index = getOrdering()[key];
-  const SubVector delta = getDelta()[index];
+  const Vector& delta = getDelta()[index];
   return theta_.at<VALUE>(key).retract(delta);
 }
 
@@ -158,7 +158,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
     for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
       if(!valuesChanged) {
         const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
-        const SubVector& newValue(delta[*it]);
+        const Vector& newValue(delta[*it]);
         if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
           valuesChanged = true;
           break;

gtsam/nonlinear/ISAM2.cpp

@@ -348,9 +348,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     variableIndex_.permuteInPlace(*colamd);
     gttoc(permute_global_variable_index);
     gttic(permute_delta);
-    delta_ = delta_.permute(*colamd);
+    delta_.permuteInPlace(*colamd);
-    deltaNewton_ = deltaNewton_.permute(*colamd);
+    deltaNewton_.permuteInPlace(*colamd);
-    RgProd_ = RgProd_.permute(*colamd);
+    RgProd_.permuteInPlace(*colamd);
     gttoc(permute_delta);
     gttic(permute_ordering);
     ordering_.permuteWithInverse(*colamdInverse);
@@ -484,9 +484,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
     gttoc(permute_global_variable_index);
     gttic(permute_delta);
-    delta_ = delta_.permute(partialSolveResult.fullReordering);
+    delta_.permuteInPlace(partialSolveResult.fullReordering);
-    deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
+    deltaNewton_.permuteInPlace(partialSolveResult.fullReordering);
-    RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
+    RgProd_.permuteInPlace(partialSolveResult.fullReordering);
     gttoc(permute_delta);
     gttic(permute_ordering);
     ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
@@ -911,7 +911,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
 
   gttic(Compute_point);
   // Compute steepest descent point
-  grad.asVector() *= step;
+  scal(step, grad);
   gttoc(Compute_point);
 }
 

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -110,7 +110,7 @@ void LevenbergMarquardtOptimizer::iterate() {
       // Solve Damped Gaussian Factor Graph
       const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_);
 
-      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.asVector().norm() << endl;
+      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl;
       if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
 
       // update values

gtsam/nonlinear/LinearContainerFactor.cpp

@@ -198,6 +198,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
 
   // Determine delta between linearization points using new ordering
   VectorValues delta = linearizationPoint_->localCoordinates(subsetC, localOrdering);
+  Vector deltaVector = delta.asVector();
 
   // clone and reorder linear factor to final ordering
   GaussianFactor::shared_ptr linFactor = order(localOrdering);
@@ -208,8 +209,8 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
     HessianFactor::shared_ptr hesFactor = boost::shared_dynamic_cast<HessianFactor>(linFactor);
     size_t dim = hesFactor->linearTerm().size();
     Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
-    Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * delta.vector();
+    Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
-    hesFactor->constantTerm() += delta.vector().dot(G_delta) + delta.vector().dot(hesFactor->linearTerm());
+    hesFactor->constantTerm() += deltaVector.dot(G_delta) + deltaVector.dot(hesFactor->linearTerm());
     hesFactor->linearTerm() += G_delta;
   }
 

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -44,7 +44,7 @@ NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradient
 }
 NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const {
   Gradient step = g;
-  step.asVector() *= alpha;
+  scal(alpha, step);
   return current.retract(step, ordering_);
 }
 

gtsam/nonlinear/Values.cpp

@@ -81,7 +81,7 @@ namespace gtsam {
     Values result;
 
     for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
+      const Vector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
       Key key = key_value->key;  // Non-const duplicate to deal with non-const insert argument
       Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
       result.values_.insert(key, retractedValue); // Add retracted result directly to result values

tests/testDoglegOptimizer.cpp

@@ -32,6 +32,7 @@
 #pragma GCC diagnostic pop
 #include <boost/assign/list_of.hpp> // for 'list_of()'
 #include <functional>
+#include <boost/iterator/counting_iterator.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -45,7 +46,8 @@ double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
 
   // Convert Vector to VectorValues
   VectorValues vv = *allocateVectorValues(gbn);
-  vv.asVector() = values;
+  internal::writeVectorValuesSlices(values, vv,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
 
   // Convert to factor graph
   GaussianFactorGraph gfg(gbn);
@@ -57,7 +59,8 @@ double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector
 
   // Convert Vector to VectorValues
   VectorValues vv = *allocateVectorValues(gbt);
-  vv.asVector() = values;
+  internal::writeVectorValuesSlices(values, vv,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
 
   // Convert to factor graph
   GaussianFactorGraph gfg(gbt);
@@ -96,13 +99,15 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
   Vector gradient = numericalGradient(
       boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
       LieVector(VectorValues::Zero(gradientValues).asVector()));
-  gradientValues.asVector() = gradient;
+  internal::writeVectorValuesSlices(gradient, gradientValues,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
 
   // Compute the gradient using dense matrices
   Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
   LONGS_EQUAL(11, augmentedHessian.cols());
   VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
-  denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10);
+  internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
   EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
 
   // Compute the steepest descent point
@@ -276,13 +281,15 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
   Vector gradient = numericalGradient(
       boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
       LieVector(VectorValues::Zero(gradientValues).asVector()));
-  gradientValues.asVector() = gradient;
+  internal::writeVectorValuesSlices(gradient, gradientValues,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
 
   // Compute the gradient using dense matrices
   Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
   LONGS_EQUAL(11, augmentedHessian.cols());
   VectorValues denseMatrixGradient = *allocateVectorValues(bt);
-  denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10);
+  internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
+    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
   EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
 
   // Compute the steepest descent point

tests/testGaussianISAM2.cpp

@@ -207,6 +207,7 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) {
   EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
   EXPECT(assert_equal(orderingExpected, ordering));
 }
+
 /* ************************************************************************* */
 TEST_UNSAFE(ISAM2, ImplRemoveVariables) {