Simplified VectorValues interface and implementation, slight simplification to Permuted<>

gtsam/inference/Permutation.h

@@ -151,12 +151,12 @@ protected:
  *   container[permutation[index2]];
  * but more concise.
  */
-template<typename CONTAINER, typename VALUETYPE = typename CONTAINER::value_reference_type>
+template<typename CONTAINER>
 class Permuted {
   Permutation permutation_;
   CONTAINER& container_;
 public:
-  typedef VALUETYPE value_type;
+  typedef typename CONTAINER::iterator::value_type value_type;
 
   /** Construct as a permuted view on the Container.  The permutation is copied
    * but only a reference to the container is stored.
@@ -169,10 +169,10 @@ public:
   Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {}
 
   /** Access the container through the permutation */
-  value_type operator[](size_t index) const { return container_[permutation_[index]]; }
+  value_type& operator[](size_t index) { return container_[permutation_[index]]; }
 
-//  /** Access the container through the permutation (const version) */
+  /** Access the container through the permutation (const version) */
-//  const_value_type operator[](size_t index) const;
+  const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
 
   /** Permute this view by applying a permutation to the underlying permutation */
   void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }

gtsam/inference/VariableIndex.h

@@ -45,7 +45,7 @@ public:
 
 protected:
   std::vector<Factors> indexUnpermuted_;
-  Permuted<std::vector<Factors>, Factors&> index_; // Permuted view of indexUnpermuted.
+  Permuted<std::vector<Factors> > index_; // Permuted view of indexUnpermuted.
   size_t nFactors_; // Number of factors in the original factor graph.
   size_t nEntries_; // Sum of involved variable counts of each factor.
 

gtsam/linear/GaussianConditional.cpp

@@ -27,6 +27,42 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+// Helper function used only in this file - extracts vectors with variable indices
+// in the first and last iterators, and concatenates them in that order into the
+// output.
+template<typename ITERATOR>
+static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) {
+  // Find total dimensionality
+  int dim = 0;
+  for(ITERATOR j = first; j != last; ++j)
+    dim += values.dim(*j);
+
+  // Copy vectors
+  Vector ret(dim);
+  int varStart = 0;
+  for(ITERATOR j = first; j != last; ++j) {
+    ret.segment(varStart, values.dim(*j)) = values[*j];
+    varStart += values.dim(*j);
+  }
+  return ret;
+}
+
+/* ************************************************************************* */
+// Helper function used only in this file - 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>
+static void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) {
+  // Copy vectors
+  int varStart = 0;
+  for(ITERATOR j = first; j != last; ++j) {
+    values[*j] = vector.segment(varStart, values.dim(*j));
+    varStart += values.dim(*j);
+  }
+  assert(varStart = vector.rows());
+}
+
 /* ************************************************************************* */
 GaussianConditional::GaussianConditional() : rsd_(matrix_) {}
 
@@ -169,14 +205,13 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
 
 /* ************************************************************************* */
 void GaussianConditional::rhs(VectorValues& x) const {
-	Vector d = rhs();
+  writeVectorValuesSlices(get_d(), x, beginFrontals(), endFrontals());
-	x.range(beginFrontals(), endFrontals(), d);
 }
 
 /* ************************************************************************* */
 void GaussianConditional::rhs(Permuted<VectorValues>& x) const {
   // Copy the rhs into x, accounting for the permutation
-  Vector d = rhs();
+  Vector d = get_d();
   size_t rhsPosition = 0;  // We walk through the rhs by variable
   for(const_iterator j = beginFrontals(); j != endFrontals(); ++j) {
     // Get the segment of the rhs for this variable
@@ -190,7 +225,7 @@ void GaussianConditional::rhs(Permuted<VectorValues>& x) const {
 void GaussianConditional::solveInPlace(VectorValues& x) const {
   static const bool debug = false;
   if(debug) print("Solving conditional in place");
-  Vector rhs = x.range(beginFrontals(), endFrontals());
+  Vector rhs = extractVectorValuesSlices(x, beginFrontals(), endFrontals());
 	for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
 		rhs += -get_S(parent) * x[*parent];
 	}
@@ -200,7 +235,7 @@ void GaussianConditional::solveInPlace(VectorValues& x) const {
 		gtsam::print(rhs, "rhs: ");
 	  gtsam::print(soln, "full back-substitution solution: ");
 	}
-	x.range(beginFrontals(), endFrontals(), soln);
+	writeVectorValuesSlices(soln, x, beginFrontals(), endFrontals());
 }
 
 /* ************************************************************************* */
@@ -245,7 +280,7 @@ VectorValues GaussianConditional::solve(const VectorValues& x) const {
 
 /* ************************************************************************* */
 void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
-	Vector frontalVec = gy.range(beginFrontals(), endFrontals());
+	Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
 	// TODO: verify permutation
 	frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
 	GaussianConditional::const_iterator it;
@@ -253,14 +288,14 @@ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
 		const Index i = *it;
 		transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
 	}
-	gy.range(beginFrontals(), endFrontals(), frontalVec);
+	writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
 }
 
 /* ************************************************************************* */
 void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
-	Vector frontalVec = gy.range(beginFrontals(), endFrontals());
+	Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
 	frontalVec = emul(frontalVec, get_sigmas());
-	gy.range(beginFrontals(), endFrontals(), frontalVec);
+	writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
 }
 
 }

gtsam/linear/IterativeOptimizationParameters.h

@@ -12,6 +12,8 @@
 
 namespace gtsam {
 
+struct DimSpec;
+
 // a container for all related parameters
 struct IterativeOptimizationParameters {
 
@@ -31,7 +33,7 @@ public:
   double epsilon_abs_; // absolute error
   verbosityLevel verbosity_;
   size_t nReduce_ ;
-  DimSpec::shared_ptr skeleton_spec_;
+  boost::shared_ptr<DimSpec> skeleton_spec_;
   bool est_cond_ ;
 
 public:
@@ -80,4 +82,28 @@ public:
     return est_cond_ ;
   }
 };
+
+struct DimSpec: public std::vector<size_t> {
+
+  typedef std::vector<size_t> Base;
+  typedef boost::shared_ptr<DimSpec> shared_ptr;
+
+  DimSpec() :
+    Base() {
+  }
+  DimSpec(size_t n) :
+    Base(n) {
+  }
+  DimSpec(size_t n, size_t init) :
+    Base(n, init) {
+  }
+  DimSpec(const VectorValues &V) :
+    Base(V.size()) {
+    const size_t n = V.size();
+    for (size_t i = 0; i < n; ++i) {
+      (*this)[i] = V[i].rows();
+    }
+  }
+};
+
 }

gtsam/linear/JacobianFactor.cpp

@@ -580,7 +580,7 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
     // It is crucial for performance to make a zero-valued clone of x
-    VectorValues g = VectorValues::zero(x);
+    VectorValues g = VectorValues::Zero(x);
     Errors e;
     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
       e.push_back(factor->error_vector(x));
@@ -603,7 +603,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
-    r.makeZero();
+    r.vector() = Vector::Zero(r.dim());
     Index i = 0;
     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
       for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
@@ -615,7 +615,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
-    x.makeZero();
+    x.vector() = Vector::Zero(x.dim());
     Index i = 0;
     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
       for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {

gtsam/linear/SubgraphPreconditioner.cpp

@@ -64,7 +64,7 @@ namespace gtsam {
 	VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) {
 		VectorValues x = sp.x(y); // x = inv(R1)*y
 		Errors e2 = gaussianErrors(*sp.Ab2(),x);
-		VectorValues gx2 = VectorValues::zero(y);
+		VectorValues gx2 = VectorValues::Zero(y);
 		gtsam::transposeMultiplyAdd(*sp.Ab2(),1.0,e2,gx2); // A2'*e2;
 		VectorValues gy2 = gtsam::backSubstituteTranspose(*sp.Rc1(), gx2); // inv(R1')*gx2
 		return y + gy2;
@@ -138,7 +138,7 @@ namespace gtsam {
 		while (it != end)
 		e2.push_back(*(it++));
 
-		VectorValues x = VectorValues::zero(y); // x = 0
+		VectorValues x = VectorValues::Zero(y); // x = 0
 		gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x);   // x += A2'*e2
 		axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
 	}

gtsam/linear/SubgraphPreconditioner.h

@@ -76,8 +76,7 @@ namespace gtsam {
 
 		/* A zero VectorValues with the structure of xbar */
 		VectorValues zero() const {
-			VectorValues V(*xbar_) ;
+			VectorValues V(VectorValues::Zero(*xbar_)) ;
-			V.makeZero();
 			return V ;
 		}
 

gtsam/linear/VectorValues.cpp

@@ -16,56 +16,69 @@
  * @author Alex Cunningham
  */
 
+#include <gtsam/base/FastVector.h>
 #include <gtsam/linear/VectorValues.h>
 
 using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-VectorValues::VectorValues(Index nVars, size_t varDim) :
+VectorValues::VectorValues(const VectorValues& other) {
-		varStarts_(nVars + 1) {
+  *this = other;
-	varStarts_[0] = 0;
-	size_t varStart = 0;
-	for (Index j = 1; j <= nVars; ++j)
-		varStarts_[j] = (varStart += varDim);
-	values_.resize(varStarts_.back());
 }
 
 /* ************************************************************************* */
-VectorValues::VectorValues(const std::vector<size_t>& dimensions,
+VectorValues& VectorValues::operator=(const VectorValues& rhs) {
-		const Vector& values) :
+  if(this != &rhs) {
-		values_(values), varStarts_(dimensions.size() + 1) {
+    resizeLike(rhs);          // Copy structure
-	varStarts_[0] = 0;
+    values_ = rhs.values_;  // Copy values
-	size_t varStart = 0;
+  }
-	Index var = 0;
+  return *this;
-	BOOST_FOREACH(size_t dim, dimensions)
-				varStarts_[++var] = (varStart += dim);
-	assert(varStarts_.back() == (size_t) values.size());
 }
 
 /* ************************************************************************* */
-VectorValues::VectorValues(const std::vector<size_t>& dimensions,
+VectorValues VectorValues::Zero(const VectorValues& x) {
-		const double* values) :
+	VectorValues cloned(SameStructure(x));
-		varStarts_(dimensions.size() + 1) {
+	cloned.vector() = Vector::Zero(x.dim());
-	varStarts_[0] = 0;
+	return cloned;
-	size_t varStart = 0;
-	Index var = 0;
-	BOOST_FOREACH(size_t dim, dimensions)
-				varStarts_[++var] = (varStart += dim);
-	values_ = Vector_(varStart, values);
 }
 
 /* ************************************************************************* */
-VectorValues VectorValues::SameStructure(const VectorValues& otherValues) {
+void VectorValues::insert(Index j, const Vector& value) {
-	VectorValues ret;
+  // Make sure j does not already exist
-	ret.varStarts_ = otherValues.varStarts_;
+  if(exists(j))
-	ret.values_.resize(ret.varStarts_.back());
+    throw invalid_argument("VectorValues: requested variable index to insert already exists.");
-	return ret;
+
+  // Get vector of dimensions
+  FastVector<size_t> dimensions(size());
+  for(size_t k=0; k<maps_.size(); ++k)
+    dimensions[k] = maps_[k].rows();
+
+  // If this adds variables at the end, insert zero-length entries up to j
+  if(j >= size())
+    dimensions.insert(dimensions.end(), j+1-size(), 0);
+
+  // Set correct dimension for j
+  dimensions[j] = value.rows();
+
+  // Make a copy to make assignment easier
+  VectorValues original(*this);
+
+  // Resize to accomodate new variable
+  resize(dimensions);
+
+  // Copy original variables
+  for(Index k = 0; k < original.size(); ++k)
+    if(k != j && exists(k))
+      operator[](k) = original[k];
+
+  // Copy new variable
+  operator[](j) = value;
 }
 
 /* ************************************************************************* */
 void VectorValues::print(const std::string& str) const {
-	std::cout << str << ": " << varStarts_.size() - 1 << " elements\n";
+	std::cout << str << ": " << size() << " elements\n";
 	for (Index var = 0; var < size(); ++var)
 		std::cout << "  " << var << ": \n" << operator[](var) << "\n";
 	std::cout.flush();
@@ -73,61 +86,59 @@ void VectorValues::print(const std::string& str) const {
 
 /* ************************************************************************* */
 bool VectorValues::equals(const VectorValues& x, double tol) const {
-	return varStarts_ == x.varStarts_
+	return hasSameStructure(x) && equal_with_abs_tol(values_, x.values_, tol);
-			&& equal_with_abs_tol(values_, x.values_, tol);
 }
 
 /* ************************************************************************* */
-size_t VectorValues::dim(Index j) const {
+void VectorValues::resize(Index nVars, size_t varDim) {
-	checkVariable(j);
+  maps_.clear();
-	const size_t start = varStarts_[j], n = varStarts_[j + 1] - start;
+  maps_.reserve(nVars);
-	return n;
+  values_.resize(nVars * varDim);
+	int varStart = 0;
+	for (Index j = 0; j < nVars; ++j) {
+		maps_.push_back(values_.segment(varStart, varDim));
+		varStart += varDim;
+	}
 }
 
 /* ************************************************************************* */
-VectorValues::mapped_type VectorValues::operator[](Index j) {
+void VectorValues::resizeLike(const VectorValues& other) {
-	checkVariable(j);
+  values_.resize(other.dim());
-	const size_t start = varStarts_[j], n = varStarts_[j + 1] - start;
+  // Create SubVectors referencing our values_ vector
-	return values_.segment(start, n);
+  maps_.clear();
+  maps_.reserve(other.size());
+  int varStart = 0;
+  BOOST_FOREACH(const SubVector& value, other) {
+    maps_.push_back(values_.segment(varStart, value.rows()));
+    varStart += value.rows();
+  }
 }
 
 /* ************************************************************************* */
-VectorValues::const_mapped_type VectorValues::operator[](Index j) const {
+VectorValues VectorValues::SameStructure(const VectorValues& other) {
-	checkVariable(j);
+  VectorValues ret;
-	const size_t start = varStarts_[j], n = varStarts_[j + 1] - start;
+  ret.resizeLike(other);
-	return values_.segment(start, n);
+  return ret;
 }
 
 /* ************************************************************************* */
-VectorValues VectorValues::zero(const VectorValues& x) {
+bool VectorValues::hasSameStructure(const VectorValues& other) const {
-	VectorValues cloned(x);
+  for(size_t j=0; j<size(); ++j)
-	cloned.makeZero();
+    if(this->dim(j) != other.dim(j))
-	return cloned;
+      return false;
-}
+  return true;
-
-/* ************************************************************************* */
-Index VectorValues::push_back_preallocated(const Vector& vector) {
-	Index var = varStarts_.size() - 1;
-	varStarts_.push_back(varStarts_.back() + vector.size());
-	this->operator[](var) = vector; // This will assert that values_ has enough allocated space.
-	return var;
 }
 
 /* ************************************************************************* */
 VectorValues VectorValues::operator+(const VectorValues& c) const {
-	assert(varStarts_ == c.varStarts_);
+	assert(this->hasSameStructure(c));
-	VectorValues result;
+	VectorValues result(SameStructure(c));
-	result.varStarts_ = varStarts_;
+	result.values_ = this->values_ + c.values_;
-	result.values_ = values_.head(varStarts_.back())
-			+ c.values_.head(varStarts_.back());
 	return result;
 }
 
 /* ************************************************************************* */
 void VectorValues::operator+=(const VectorValues& c) {
-	assert(varStarts_ == c.varStarts_);
+	assert(this->hasSameStructure(c));
-	this->values_ += c.values_.head(varStarts_.back());
+	this->values_ += c.values_;
 }
-
-/* ************************************************************************* */
-

gtsam/linear/VectorValues.h

@@ -22,254 +22,258 @@
 
 #include <boost/foreach.hpp>
 #include <boost/shared_ptr.hpp>
+#include <numeric>
 
 namespace gtsam {
 
   /**
-	 * The class VectorValues stores a number of Vectors.
+   * This class stores a collection of vector-valued variables, each referenced
-	 * Typically, this class is used in back-substitution (Bayes Net solve).
+   * by a unique variable index.  It is typically used to store the variables
+   * of a GaussianFactorGraph.  Optimizing a GaussianFactorGraph or GaussianBayesNet
+   * returns this class.
    *
-	 * There are a number of constructors that simply reserve space
+   * For basic usage, such as receiving a linear solution from gtsam solving functions,
-	 * (Frank is not a big fan of this imperative style, but it is needed for speed)
+   * or creating this class in unit tests and examples where speed is not important,
-	 * and others - safer ones - that actually initialize values.
+   * you can use a simple interface:
-	 * SameStructure and reserve are two other utility functions that manage storage.
+   *  - The default constructor VectorValues() to create this class
+   *  - insert(Index, const Vector&) to add vector variables
+   *  - operator[](Index) for read and write access to stored variables
+   *  - \ref exists (Index) to check if a variable is present
+   *  - Other facilities like iterators, size(), dim(), etc.
+   *
+   * Example:
+   * \code
+     VectorValues values;
+     values.insert(0, Vector_(3, 5.0, 6.0, 7.0));
+     values.insert(1, Vector_(2, 3.0, 4.0));
+
+     // Prints [ 3.0 4.0 ]
+     gtsam::print(values[1]);
+
+     // Prints [ 8.0 9.0 ]
+     values[1] = Vector_(2, 8.0, 9.0);
+     gtsam::print(values[1]);
+     \endcode
+   *
+   * Internally, this class stores all vectors as part of one large vector.  This is
+   * necessary for performance, and the gtsam linear solving code exploits it by
+   * only allocating one large vector to store the solution.  For advanced usage,
+   * or where speed is important, be aware of the following:
+   *  - It is faster to allocate space ahead of time using a pre-allocating constructor
+   *    or the resize() and append() functions, than to use insert(Index, const Vector&),
+   *    which always has to re-allocate the internal vector.
+   *  - The vector() function permits access to the underlying Vector.
+   *  - operator[]() returns a SubVector view of the underlying Vector.
    *
    * Access is through the variable index j, and returns a SubVector,
    * which is a view on the underlying data structure.
+   *
+   * This class is additionally used in gradient descent and dog leg to store the gradient.
+   * \nosubgrouping
    */
   class VectorValues {
   protected:
-		Vector values_;
+    Vector values_; ///< The underlying vector storing the values
-		std::vector<size_t> varStarts_; // start at 0 with size nVars + 1
+    typedef std::vector<SubVector> ValueMaps; ///< Collection of SubVector s
+    ValueMaps maps_; ///< SubVector s referencing each vector variable in values_
 
   public:
-		 // Forward declaration of iterator implementation
+    typedef ValueMaps::iterator iterator; ///< Iterator over vector values
-		template<class C> class _impl_iterator;
+    typedef ValueMaps::const_iterator const_iterator; ///< Const iterator over vector values
-		typedef _impl_iterator<VectorValues> iterator;
+    typedef ValueMaps::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
-		typedef _impl_iterator<const VectorValues> const_iterator;
+    typedef ValueMaps::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
+    typedef boost::shared_ptr<VectorValues> shared_ptr; ///< shared_ptr to this class
 
-		// Some other typedefs
+    /// @name Standard constructors
-		typedef boost::shared_ptr<VectorValues> shared_ptr;
+    /// @{
-		typedef SubVector value_reference_type;
-		typedef ConstSubVector const_value_reference_type;
-		typedef SubVector mapped_type;
-		typedef ConstSubVector const_mapped_type;
-
-		/** Constructors that simply reserve space */
 
     /**
-		 * Default constructor creates an empty VectorValues.  reserve(...) must be
+     * Default constructor creates an empty VectorValues.
-		 * called to allocate space before any values can be added.  This prevents
-		 * slow reallocation of space at runtime.
      */
-		VectorValues() :
+    VectorValues() {} //
-				varStarts_(1, 0) {
+
-		}
+    /** Copy constructor */
+    VectorValues(const VectorValues &other); //
+
+    /** Named constructor to create a VectorValues of the same structure of the
+     * specifed one, but filled with zeros.
+     * @return
+     */
+    static VectorValues Zero(const VectorValues& model);
+
+    /// @}
+    /// @name Standard interface
+    /// @{
+
+    /** Number of variables stored, always 1 more than the highest variable index,
+     * even if some variables with lower indices are not present. */
+    Index size() const { return maps_.size(); } //
+
+    /** 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(); } //
+
+    /** Check whether a variable exists by index. */
+    bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } //
+
+    /** Reference a variable by index. */
+    SubVector& operator[](Index j) { checkExists(j); return maps_[j]; } //
+
+    /** Reference a variable by index. */
+    const SubVector& operator[](Index j) const { checkExists(j); return maps_[j]; } //
+
+    /** Insert a new variable (causes reallocation). */
+    void insert(Index j, const Vector& value); //
+
+    /** Assignment */
+    VectorValues& operator=(const VectorValues& rhs); //
+
+    iterator begin()                      { chk(); return maps_.begin(); }  ///< Iterator over variables
+    const_iterator begin() const          { chk(); return maps_.begin(); }  ///< Iterator over variables
+    iterator end()                        { chk(); return maps_.end(); }    ///< Iterator over variables
+    const_iterator end() const            { chk(); return maps_.end(); }    ///< Iterator over variables
+    reverse_iterator rbegin()             { chk(); return maps_.rbegin(); } ///< Iterator over variables
+    const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Iterator over variables
+    reverse_iterator rend()               { chk(); return maps_.rend(); }   ///< Iterator over variables
+    const_reverse_iterator rend() const   { chk(); return maps_.rend(); }   ///< Iterator over variables
+
+    /** print required by Testable for unit testing */
+    void print(const std::string& str = "VectorValues: ") const; //
+
+    /** equals required by Testable for unit testing */
+    bool equals(const VectorValues& x, double tol = 1e-9) const; //
+
+    /// @}
+    /// @name Advanced constructors
+    /// @{
 
     /** Construct from a container of variable dimensions (in variable order). */
     template<class CONTAINER>
-		VectorValues(const CONTAINER& dimensions);
+    VectorValues(const CONTAINER& dimensions) { append(dimensions); } //
 
     /** Construct to hold nVars vectors of varDim dimension each. */
-		VectorValues(Index nVars, size_t varDim);
+    VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); } //
-
-		/** Constructors that actually initialize values */
-
-		/** Construct from a container of variable dimensions in variable order and
-		 * a combined Vector of all of the variables in order.*/
-		VectorValues(const std::vector<size_t>& dimensions, const Vector& values);
-
-		/** Construct from the variable dimensions in varaible order and a double array that contains actual values */
-		VectorValues(const std::vector<size_t>& dimensions, const double* values);
-
-		/** Copy constructor */
-		VectorValues(const VectorValues &V) :
-				values_(V.values_), varStarts_(V.varStarts_) {
-		}
 
     /** Named constructor to create a VectorValues that matches the structure of
      * the specified VectorValues, but do not initialize the new values. */
-		static VectorValues SameStructure(const VectorValues& otherValues);
+    static VectorValues SameStructure(const VectorValues& other); //
 
-		/** Reserve space for a total number of variables and dimensionality */
+    /** Named constructor to create a VectorValues from a container of variable
-		void reserve(Index nVars, size_t totalDims) {
+     * dimensions that is filled with zeros. */
-			values_.conservativeResize(totalDims);
+    template<class CONTAINER>
-			varStarts_.reserve(nVars + 1);
+    static VectorValues Zero(const CONTAINER& dimensions);
-		}
 
-		/** print required by Testable for unit testing */
+    /// @}
-		void print(const std::string& str = "VectorValues: ") const;
+    /// @name Advanced interface
+    /// @{
 
-		/** equals required by Testable for unit testing */
+    /** Resize this VectorValues to have identical structure to other, leaving
-		bool equals(const VectorValues& x, double tol = 1e-9) const;
+     * this VectorValues with uninitialized values.
-
+     * @param other The VectorValues whose structure to copy
-		/** Number of elements */
-		Index size() const {
-			return varStarts_.size() - 1;
-		}
-
-		/** dimension of a particular element */
-		size_t dim(Index j) const;
-
-		/** Total dimensionality used (could be smaller than what has been allocated
-		 * with reserve(...) ).
      */
-		size_t dim() const {
+    void resizeLike(const VectorValues& other); //
-			return varStarts_.back();
-		}
 
-		/** Total dimensions capacity allocated */
+    /** Resize the VectorValues to hold \c nVars variables, each of dimension
-		size_t dimCapacity() const {
+     * \c varDim.  This function does not preserve any data, after calling
-			return values_.size();
+     * it all variables 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.  The new variables are uninitialized, but this function
+     * is used to pre-allocate space for performance.  This function does not
+     * preserve any data, after calling it all variables 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); //
 
     /** Reference the entire solution vector (const version). */
-		const Vector& vector() const {
+    const Vector& vector() const { chk(); return values_; } //
-			return values_;
-		}
 
     /** Reference the entire solution vector. */
-		Vector& vector() {
+    Vector& vector() { chk(); return values_; } //
-			return values_;
-		}
 
-		/** Individual element access */
+    /** Check whether this VectorValues has the same structure, meaning has the
-		mapped_type operator[](Index j);
+     * same number of variables and that all variables are of the same dimension,
-		const_mapped_type operator[](Index j) const;
+     * as another VectorValues
-
+     * @param other The other VectorValues with which to compare structure
-		/** Iterator access */
+     * @return \c true if the structure is the same, \c false if not.
-		iterator begin() {
-			return _impl_iterator<VectorValues>(*this, 0);
-		}
-		const_iterator begin() const {
-			return _impl_iterator<const VectorValues>(*this, 0);
-		}
-		iterator end() {
-			return _impl_iterator<VectorValues>(*this, varStarts_.size() - 1);
-		}
-		const_iterator end() const {
-			return _impl_iterator<const VectorValues>(*this, varStarts_.size() - 1);
-		}
-
-		/** access a range of indices (of no particular order) as a single vector */
-		template<class ITERATOR>
-		Vector range(const ITERATOR& idx_begin, const ITERATOR& idx_end) const;
-
-		/** set a range of indices as a single vector split across the range */
-		template<class ITERATOR>
-		void range(const ITERATOR& idx_begin, const ITERATOR& idx_end,
-				const Vector& v);
-
-		/** Set all elements to zero */
-		void makeZero() {
-			values_.setZero();
-		}
-
-		/** Copy structure of x, but set all values to zero */
-		static VectorValues zero(const VectorValues& x);
-
-		/**
-		 * Append a variable using the next variable ID, and return that ID.  Space
-		 * must have been allocated ahead of time using reserve(...).
      */
-		Index push_back_preallocated(const Vector& vector);
+    bool hasSameStructure(const VectorValues& other) const;
 
-		/* dot product */
+    /** Dot product with another VectorValues, interpreting both as vectors of
+     * their concatenated values. */
     double dot(const VectorValues& V) const {
       return gtsam::dot(this->values_, V.values_);
     }
 
     /**
-		 * + operator simply adds Vectors.  This checks for structural equivalence
+     * + operator does element-wise addition.  Both VectorValues must have the
-		 * when NDEBUG is not defined.
+     * same structure (checked when NDEBUG is not defined).
      */
     VectorValues operator+(const VectorValues& c) const;
 
+    /**
+     * += operator does element-wise addition.  Both VectorValues must have the
+     * same structure (checked when NDEBUG is not defined).
+     */
     void operator+=(const VectorValues& c);
 
-		/**
+    /// @}
-		 * Iterator (handles both iterator and const_iterator depending on whether
-		 * the template type is const.
-		 */
-		template<class C>
-		class _impl_iterator {
-		protected:
-			C& config_;
-			Index curVariable_;
 
-			_impl_iterator(C& config, Index curVariable) :
+  private:
-					config_(config), curVariable_(curVariable) {
+    // Verifies that the underlying Vector is consistent with the collection of SubVectors
-			}
+    void chk() const;
-			void checkCompat(const _impl_iterator<C>& r) {
+
-				assert(&config_ == &r.config_);
+    // Throw an exception if j does not exist
-			}
+    void checkExists(Index j) const {
-			friend class VectorValues;
+      chk();
-
+      if(!exists(j))
-		public:
+        throw std::out_of_range("VectorValues: requested variable index is not in this VectorValues.");
-			typedef typename const_selector<C, VectorValues,
-					VectorValues::mapped_type, VectorValues::const_mapped_type>::type value_type;
-			_impl_iterator<C>& operator++() {
-				++curVariable_;
-				return *this;
-			}
-			_impl_iterator<C>& operator--() {
-				--curVariable_;
-				return *this;
-			}
-			_impl_iterator<C>& operator++(int) {
-				throw std::runtime_error("Use prefix ++ operator");
-			}
-			_impl_iterator<C>& operator--(int) {
-				throw std::runtime_error("Use prefix -- operator");
-			}
-			_impl_iterator<C>& operator+=(ptrdiff_t step) {
-				curVariable_ += step;
-				return *this;
-			}
-			_impl_iterator<C>& operator-=(ptrdiff_t step) {
-				curVariable_ += step;
-				return *this;
-			}
-			ptrdiff_t operator-(const _impl_iterator<C>& r) {
-				checkCompat(r);
-				return curVariable_ - r.curVariable_;
-			}
-			bool operator==(const _impl_iterator<C>& r) {
-				checkCompat(r);
-				return curVariable_ == r.curVariable_;
-			}
-			bool operator!=(const _impl_iterator<C>& r) {
-				checkCompat(r);
-				return curVariable_ != r.curVariable_;
-			}
-			value_type operator*() {
-				return config_[curVariable_];
-			}
-		};
-
-	protected:
-		void checkVariable(Index j) const {
-			assert(j < varStarts_.size()-1);
     }
 
+    // Resize
+    void copyStructureFrom(const VectorValues& other);
+
   public:
+    /// TODO: linear algebra interface seems to have been added for SPCG.
     friend size_t dim(const VectorValues& V) {
-			return V.varStarts_.back();
+      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_);
     }
+    /// 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_);
     }
+    /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void sqrt(VectorValues &x) {
       Vector y = gtsam::esqrt(x.values_);
       x.values_ = y;
     }
 
+    /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void ediv(const VectorValues& numerator,
         const VectorValues& denominator, VectorValues &result) {
       assert(
@@ -279,6 +283,7 @@ namespace gtsam {
         result.values_[i] = numerator.values_[i] / denominator.values_[i];
     }
 
+    /// TODO: linear algebra interface seems to have been added for SPCG.
     friend void edivInPlace(VectorValues& x, const VectorValues& y) {
       assert(x.dim() == y.dim());
       const size_t sz = x.dim();
@@ -286,100 +291,76 @@ namespace gtsam {
         x.values_[i] /= y.values_[i];
     }
 
-		// check whether there's a zero in the vector
-		friend bool anyZero(const VectorValues& x, double tol = 1e-5) {
-			bool flag = false;
-			size_t i = 0;
-			for (const double *v = x.values_.data(); i < (size_t) x.values_.size();
-					++v) {
-				if (*v < tol && *v > -tol) {
-					flag = true;
-					break;
-				}
-				++i;
-			}
-			return flag;
-		}
-
   private:
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
-		void serialize(ARCHIVE & ar, const unsigned int version) {
+    void save(ARCHIVE & ar, const unsigned int version) const {
+      // The maps_ stores pointers, so we serialize dimensions instead
+      std::vector<size_t> dimensions(size());
+      for(size_t j=0; j<maps_.size(); ++j)
+        dimensions[j] = maps_[j].rows();
+      ar & BOOST_SERIALIZATION_NVP(dimensions);
       ar & BOOST_SERIALIZATION_NVP(values_);
-			ar & BOOST_SERIALIZATION_NVP(varStarts_);
     }
-	};
+    template<class ARCHIVE>
-	// \class VectorValues definition
+    void load(ARCHIVE & ar, const unsigned int version) {
+      std::vector<size_t> dimensions;
+      ar & BOOST_SERIALIZATION_NVP(dimensions); // Load dimensions
+      resize(dimensions); // Allocate space for everything
+      ar & BOOST_SERIALIZATION_NVP(values_); // Load values
+      chk();
+    }
+    BOOST_SERIALIZATION_SPLIT_MEMBER()
+  }; // VectorValues definition
 
-/// Implementations of functions
+  // Implementations of template and inline functions
 
+  /* ************************************************************************* */
   template<class CONTAINER>
-	inline VectorValues::VectorValues(const CONTAINER& dimensions) :
+  void VectorValues::resize(const CONTAINER& dimensions) {
-			varStarts_(dimensions.size() + 1) {
+    maps_.clear();
-		varStarts_[0] = 0;
+    values_.resize(0);
-		size_t varStart = 0;
+    append(dimensions);
-		Index var = 0;
-		BOOST_FOREACH(size_t dim, dimensions)
-				{
-					varStarts_[++var] = (varStart += dim);
-				}
-		values_.resize(varStarts_.back());
   }
 
-	template<class ITERATOR>
+  /* ************************************************************************* */
-	inline Vector VectorValues::range(const ITERATOR& idx_begin,
+  template<class CONTAINER>
-			const ITERATOR& idx_end) const {
+  void VectorValues::append(const CONTAINER& dimensions) {
-		// find the size of the vector to build
+    chk();
-		size_t s = 0;
+    int newDim = std::accumulate(dimensions.begin(), dimensions.end(), 0); // Sum of dimensions
-		for (ITERATOR it = idx_begin; it != idx_end; ++it)
+    values_.conservativeResize(dim() + newDim);
-			s += dim(*it);
+    // Relocate existing maps
-
+    int varStart = 0;
-		// assign vector
+    for(size_t j = 0; j < maps_.size(); ++j) {
-		Vector result(s);
+      new (&maps_[j]) SubVector(values_.segment(varStart, maps_[j].rows()));
-		size_t start = 0;
+      varStart += maps_[j].rows();
-		for (ITERATOR it = idx_begin; it != idx_end; ++it) {
-			ConstSubVector v = (*this)[*it];
-			const size_t d = v.size();
-			result.segment(start, d).operator=(v); // This syntax works around what seems to be a bug in clang++
-			start += d;
     }
-		return result;
+    maps_.reserve(maps_.size() + dimensions.size());
-	}
+    BOOST_FOREACH(size_t dim, dimensions) {
-
+      maps_.push_back(values_.segment(varStart, dim));
-	template<class ITERATOR>
+      varStart += dim; // varStart is continued from first for loop
-	inline void VectorValues::range(const ITERATOR& idx_begin,
-			const ITERATOR& idx_end, const Vector& v) {
-		size_t start = 0;
-		for (ITERATOR it = idx_begin; it != idx_end; ++it) {
-			checkVariable(*it);
-			const size_t d = dim(*it);
-			(*this)[*it] = v.segment(start, d);
-			start += d;
     }
   }
 
-	struct DimSpec: public std::vector<size_t> {
+  /* ************************************************************************* */
+  template<class CONTAINER>
+  VectorValues VectorValues::Zero(const CONTAINER& dimensions) {
+    VectorValues ret(dimensions);
+    ret.vector() = Vector::Zero(ret.dim());
+    return ret;
+  }
 
-		typedef std::vector<size_t> Base;
+  /* ************************************************************************* */
-		typedef boost::shared_ptr<DimSpec> shared_ptr;
+  inline void VectorValues::chk() const {
-
+#ifndef NDEBUG
-		DimSpec() :
+    // Check that the first SubVector points to the beginning of the Vector
-				Base() {
+    if(maps_.size() > 0) {
+      assert(values_.data() == maps_[0].data());
+      // Check that the end of the last SubVector points to the end of the Vector
+      assert(values_.rows() == maps_.back().data() + maps_.back().rows() - maps_.front().data());
     }
-		DimSpec(size_t n) :
+#endif
-				Base(n) {
   }
-		DimSpec(size_t n, size_t init) :
-				Base(n, init) {
-		}
-		DimSpec(const VectorValues &V) :
-				Base(V.size()) {
-			const size_t n = V.size();
-			for (size_t i = 0; i < n; ++i) {
-				(*this)[i] = V[i].size();
-			}
-		}
-	};
 
 } // \namespace gtsam

gtsam/linear/tests/testVectorValues.cpp

@@ -28,51 +28,253 @@ using namespace boost::assign;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST(VectorValues, constructor) {
+TEST(VectorValues, insert) {
+
+  // insert, with out-of-order indices
+  VectorValues actual;
+  actual.insert(0, Vector_(1, 1.0));
+  actual.insert(1, Vector_(2, 2.0, 3.0));
+  actual.insert(5, Vector_(2, 6.0, 7.0));
+  actual.insert(2, Vector_(2, 4.0, 5.0));
+
+  // 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));
+  LONGS_EQUAL(2, actual.dim(5));
+
+  // Logic
+  EXPECT(actual.exists(0));
+  EXPECT(actual.exists(1));
+  EXPECT(actual.exists(2));
+  EXPECT(!actual.exists(3));
+  EXPECT(!actual.exists(4));
+  EXPECT(actual.exists(5));
+  EXPECT(!actual.exists(6));
+
+  // Check values
+  EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
+  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()));
+
+  // Check exceptions
+  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
+  CHECK_EXCEPTION(actual.dim(3), out_of_range);
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, dimsConstructor) {
 	vector<size_t> dims;
 	dims.push_back(1);
 	dims.push_back(2);
 	dims.push_back(2);
-	double v[] = {1., 2., 3., 4., 5.};
 
-	VectorValues actual(dims, v);
+	VectorValues actual(dims);
+	actual[0] = Vector_(1, 1.0);
+	actual[1] = Vector_(2, 2.0, 3.0);
+	actual[2] = Vector_(2, 4.0, 5.0);
+
+	// Check dimensions
 	LONGS_EQUAL(3, actual.size());
-	DOUBLES_EQUAL(1., actual[0][0], 1e-15);
+	LONGS_EQUAL(5, actual.dim());
-	DOUBLES_EQUAL(2., actual[1][0], 1e-15);
+	LONGS_EQUAL(1, actual.dim(0));
-	DOUBLES_EQUAL(4., actual[2][0], 1e-15);
+	LONGS_EQUAL(2, actual.dim(1));
+	LONGS_EQUAL(2, actual.dim(2));
+
+	// Check values
+	EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
+  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_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
 }
 
 /* ************************************************************************* */
-TEST(VectorValues, standard) {
+TEST(VectorValues, copyConstructor) {
-  Vector v1 = Vector_(3, 1.0,2.0,3.0);
-  Vector v2 = Vector_(2, 4.0,5.0);
-  Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
 
-  vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
+  // insert, with out-of-order indices
-  VectorValues combined(dims);
+  VectorValues original;
-  EXPECT_LONGS_EQUAL(3, combined.size());
+  original.insert(0, Vector_(1, 1.0));
-  EXPECT_LONGS_EQUAL(9, combined.dim());
+  original.insert(1, Vector_(2, 2.0, 3.0));
-  EXPECT_LONGS_EQUAL(9, combined.dimCapacity());
+  original.insert(5, Vector_(2, 6.0, 7.0));
-  EXPECT_LONGS_EQUAL(3, combined.dim(0));
+  original.insert(2, Vector_(2, 4.0, 5.0));
-  EXPECT_LONGS_EQUAL(2, combined.dim(1));
-  EXPECT_LONGS_EQUAL(4, combined.dim(2));
-  combined[0] = v1;
-  combined[1] = v2;
-  combined[2] = v3;
 
-  CHECK(assert_equal(combined[0], v1))
+  VectorValues actual(original);
-  CHECK(assert_equal(combined[1], v2))
-  CHECK(assert_equal(combined[2], v3))
 
-  VectorValues incremental;
+  // Check dimensions
-  incremental.reserve(3, 9);
+  LONGS_EQUAL(6, actual.size());
-  incremental.push_back_preallocated(v1);
+  LONGS_EQUAL(7, actual.dim());
-  incremental.push_back_preallocated(v2);
+  LONGS_EQUAL(1, actual.dim(0));
-  incremental.push_back_preallocated(v3);
+  LONGS_EQUAL(2, actual.dim(1));
+  LONGS_EQUAL(2, actual.dim(2));
+  LONGS_EQUAL(2, actual.dim(5));
 
-  CHECK(assert_equal(incremental[0], v1))
+  // Logic
-  CHECK(assert_equal(incremental[1], v2))
+  EXPECT(actual.exists(0));
-  CHECK(assert_equal(incremental[2], v3))
+  EXPECT(actual.exists(1));
+  EXPECT(actual.exists(2));
+  EXPECT(!actual.exists(3));
+  EXPECT(!actual.exists(4));
+  EXPECT(actual.exists(5));
+  EXPECT(!actual.exists(6));
+
+  // Check values
+  EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
+  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()));
+
+  // Check exceptions
+  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, assignment) {
+
+  // insert, with out-of-order indices
+  VectorValues original;
+  original.insert(0, Vector_(1, 1.0));
+  original.insert(1, Vector_(2, 2.0, 3.0));
+  original.insert(5, Vector_(2, 6.0, 7.0));
+  original.insert(2, Vector_(2, 4.0, 5.0));
+
+  VectorValues actual = original;
+
+  // 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));
+  LONGS_EQUAL(2, actual.dim(5));
+
+  // Logic
+  EXPECT(actual.exists(0));
+  EXPECT(actual.exists(1));
+  EXPECT(actual.exists(2));
+  EXPECT(!actual.exists(3));
+  EXPECT(!actual.exists(4));
+  EXPECT(actual.exists(5));
+  EXPECT(!actual.exists(6));
+
+  // Check values
+  EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
+  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()));
+
+  // Check exceptions
+  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, SameStructure) {
+  // insert, with out-of-order indices
+  VectorValues original;
+  original.insert(0, Vector_(1, 1.0));
+  original.insert(1, Vector_(2, 2.0, 3.0));
+  original.insert(5, Vector_(2, 6.0, 7.0));
+  original.insert(2, Vector_(2, 4.0, 5.0));
+
+  VectorValues actual(VectorValues::SameStructure(original));
+
+  // 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));
+  LONGS_EQUAL(2, actual.dim(5));
+
+  // Logic
+  EXPECT(actual.exists(0));
+  EXPECT(actual.exists(1));
+  EXPECT(actual.exists(2));
+  EXPECT(!actual.exists(3));
+  EXPECT(!actual.exists(4));
+  EXPECT(actual.exists(5));
+  EXPECT(!actual.exists(6));
+
+  // Check exceptions
+  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, resizeLike) {
+  // insert, with out-of-order indices
+  VectorValues original;
+  original.insert(0, Vector_(1, 1.0));
+  original.insert(1, Vector_(2, 2.0, 3.0));
+  original.insert(5, Vector_(2, 6.0, 7.0));
+  original.insert(2, Vector_(2, 4.0, 5.0));
+
+  VectorValues actual(10, 3);
+  actual.resizeLike(original);
+
+  // 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));
+  LONGS_EQUAL(2, actual.dim(5));
+
+  // Logic
+  EXPECT(actual.exists(0));
+  EXPECT(actual.exists(1));
+  EXPECT(actual.exists(2));
+  EXPECT(!actual.exists(3));
+  EXPECT(!actual.exists(4));
+  EXPECT(actual.exists(5));
+  EXPECT(!actual.exists(6));
+
+  // Check exceptions
+  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, append) {
+  // insert
+  VectorValues actual;
+  actual.insert(0, Vector_(1, 1.0));
+  actual.insert(1, Vector_(2, 2.0, 3.0));
+  actual.insert(2, Vector_(2, 4.0, 5.0));
+
+  // append
+  vector<size_t> dims(2);
+  dims[0] = 3;
+  dims[1] = 5;
+  actual.append(dims);
+
+  // 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));
+  LONGS_EQUAL(3, actual.dim(3));
+  LONGS_EQUAL(5, actual.dim(4));
+
+  // Logic
+  EXPECT(actual.exists(0));
+  EXPECT(actual.exists(1));
+  EXPECT(actual.exists(2));
+  EXPECT(actual.exists(3));
+  EXPECT(actual.exists(4));
+  EXPECT(!actual.exists(5));
+
+  // Check values
+  EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
+  EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
+  EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
+
+  // Check exceptions
+  CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument);
 }
 
 /* ************************************************************************* */
@@ -124,143 +326,52 @@ TEST(VectorValues, permuted_combined) {
 
 }
 
-/* ************************************************************************* */
+///* ************************************************************************* */
-TEST(VectorValues, permuted_incremental) {
+//TEST(VectorValues, range ) {
-  Vector v1 = Vector_(3, 1.0,2.0,3.0);
+//	VectorValues v(7,2);
-  Vector v2 = Vector_(2, 4.0,5.0);
+//	v.makeZero();
-  Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
+//	v[1] = Vector_(2, 1.0, 2.0);
-
+//	v[2] = Vector_(2, 3.0, 4.0);
-  VectorValues incremental;
+//	v[3] = Vector_(2, 5.0, 6.0);
-  incremental.reserve(3, 9);
+//
-  incremental.push_back_preallocated(v1);
+//	vector<size_t> idx1, idx2;
-  incremental.push_back_preallocated(v2);
+//	idx1 += 0, 1, 2, 3, 4, 5, 6; // ordered
-  incremental.push_back_preallocated(v3);
+//	idx2 += 1, 0, 2;  // unordered
-
+//
-  Permutation perm1(3);
+//	// test access
-  perm1[0] = 1;
+//
-  perm1[1] = 2;
+//	Vector actRange1 = v.range(idx1.begin(), idx1.begin() + 2);
-  perm1[2] = 0;
+//	EXPECT(assert_equal(Vector_(4, 0.0, 0.0, 1.0, 2.0), actRange1));
-
+//
-  Permutation perm2(3);
+//	Vector actRange2 = v.range(idx1.begin()+1, idx1.begin()+2);
-  perm2[0] = 1;
+//	EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actRange2));
-  perm2[1] = 2;
+//
-  perm2[2] = 0;
+//	Vector actRange3 = v.range(idx2.begin(), idx2.end());
-
+//	EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 0.0, 0.0, 3.0, 4.0), actRange3));
-  Permuted<VectorValues> permuted1(incremental);
+//
-  CHECK(assert_equal(v1, permuted1[0]))
+//	// test setting values
-  CHECK(assert_equal(v2, permuted1[1]))
+//	VectorValues act1 = v, act2 = v, act3 = v;
-  CHECK(assert_equal(v3, permuted1[2]))
+//
-
+//	Vector a = Vector_(2, 0.1, 0.2);
-  permuted1.permute(perm1);
+//	VectorValues exp1 = act1;	exp1[0] = a;
-  CHECK(assert_equal(v1, permuted1[2]))
+//	act1.range(idx1.begin(), idx1.begin()+1, a);
-  CHECK(assert_equal(v2, permuted1[0]))
+//	EXPECT(assert_equal(exp1, act1));
-  CHECK(assert_equal(v3, permuted1[1]))
+//
-
+//	Vector bc = Vector_(4, 0.1, 0.2, 0.3, 0.4);
-  permuted1.permute(perm2);
+//	VectorValues exp2 = act2;
-  CHECK(assert_equal(v1, permuted1[1]))
+//	exp2[2] = Vector_(2, 0.1, 0.2);
-  CHECK(assert_equal(v2, permuted1[2]))
+//	exp2[3] = Vector_(2, 0.3, 0.4);
-  CHECK(assert_equal(v3, permuted1[0]))
+//	act2.range(idx1.begin()+2, idx1.begin()+4, bc);
-
+//	EXPECT(assert_equal(exp2, act2));
-  Permuted<VectorValues> permuted2(perm1, incremental);
+//
-  CHECK(assert_equal(v1, permuted2[2]))
+//	Vector def = Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
-  CHECK(assert_equal(v2, permuted2[0]))
+//	VectorValues exp3 = act3;
-  CHECK(assert_equal(v3, permuted2[1]))
+//	exp3[1] = Vector_(2, 0.1, 0.2);
-
+//	exp3[0] = Vector_(2, 0.3, 0.4);
-  permuted2.permute(perm2);
+//	exp3[2] = Vector_(2, 0.5, 0.6);
-  CHECK(assert_equal(v1, permuted2[1]))
+//	act3.range(idx2.begin(), idx2.end(), def);
-  CHECK(assert_equal(v2, permuted2[2]))
+//	EXPECT(assert_equal(exp3, act3));
-  CHECK(assert_equal(v3, permuted2[0]))
+//}
-
-}
-
-/* ************************************************************************* */
-TEST(VectorValues, makeZero ) {
-	VectorValues values(7, 2);
-	EXPECT_LONGS_EQUAL(14, values.dim());
-	EXPECT_LONGS_EQUAL(7, values.size());
-	EXPECT_LONGS_EQUAL(14, values.vector().size());
-	values.makeZero();
-	EXPECT(assert_equal(zero(14), values.vector()));
-}
-
-/* ************************************************************************* */
-TEST(VectorValues, reserve ) {
-  Vector v1 = Vector_(3, 1.0,2.0,3.0);
-  Vector v2 = Vector_(2, 4.0,5.0);
-  Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
-  Vector v4(2); v4 << 10, 11;
-  Vector v5(3); v5 << 12, 13, 14;
-
-  // Expected has all 5 variables
-  vector<size_t> dimsExp(5); dimsExp[0]=3; dimsExp[1]=2; dimsExp[2]=4; dimsExp[3]=2; dimsExp[4]=3;
-  VectorValues expected(dimsExp);
-  expected[0] = v1;
-  expected[1] = v2;
-  expected[2] = v3;
-  expected[3] = v4;
-  expected[4] = v5;
-
-  // Start with 3 variables
-  vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
-  VectorValues actual(dims);
-  actual[0] = v1;
-  actual[1] = v2;
-  actual[2] = v3;
-
-  // Now expand to all 5
-  actual.reserve(5, 14);
-  actual.push_back_preallocated(v4);
-  actual.push_back_preallocated(v5);
-
-  EXPECT(assert_equal(expected, actual));
-}
-
-/* ************************************************************************* */
-TEST(VectorValues, range ) {
-	VectorValues v(7,2);
-	v.makeZero();
-	v[1] = Vector_(2, 1.0, 2.0);
-	v[2] = Vector_(2, 3.0, 4.0);
-	v[3] = Vector_(2, 5.0, 6.0);
-
-	vector<size_t> idx1, idx2;
-	idx1 += 0, 1, 2, 3, 4, 5, 6; // ordered
-	idx2 += 1, 0, 2;  // unordered
-
-	// test access
-
-	Vector actRange1 = v.range(idx1.begin(), idx1.begin() + 2);
-	EXPECT(assert_equal(Vector_(4, 0.0, 0.0, 1.0, 2.0), actRange1));
-
-	Vector actRange2 = v.range(idx1.begin()+1, idx1.begin()+2);
-	EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actRange2));
-
-	Vector actRange3 = v.range(idx2.begin(), idx2.end());
-	EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 0.0, 0.0, 3.0, 4.0), actRange3));
-
-	// test setting values
-	VectorValues act1 = v, act2 = v, act3 = v;
-
-	Vector a = Vector_(2, 0.1, 0.2);
-	VectorValues exp1 = act1;	exp1[0] = a;
-	act1.range(idx1.begin(), idx1.begin()+1, a);
-	EXPECT(assert_equal(exp1, act1));
-
-	Vector bc = Vector_(4, 0.1, 0.2, 0.3, 0.4);
-	VectorValues exp2 = act2;
-	exp2[2] = Vector_(2, 0.1, 0.2);
-	exp2[3] = Vector_(2, 0.3, 0.4);
-	act2.range(idx1.begin()+2, idx1.begin()+4, bc);
-	EXPECT(assert_equal(exp2, act2));
-
-	Vector def = Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
-	VectorValues exp3 = act3;
-	exp3[1] = Vector_(2, 0.1, 0.2);
-	exp3[0] = Vector_(2, 0.3, 0.4);
-	exp3[2] = Vector_(2, 0.5, 0.6);
-	act3.range(idx2.begin(), idx2.end(), def);
-	EXPECT(assert_equal(exp3, act3));
-}
 
 /* ************************************************************************* */
 int main() {

gtsam/nonlinear/GaussianISAM2.cpp

@@ -62,7 +62,7 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, doubl
     for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
       if(!valuesChanged) {
         const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
-        const VectorValues::mapped_type& newValue(delta[*it]);
+        const SubVector& newValue(delta[*it]);
         if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
           valuesChanged = true;
           break;

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -125,14 +125,15 @@ struct ISAM2<CONDITIONAL, VALUES>::Impl {
 struct _VariableAdder {
   Ordering& ordering;
   Permuted<VectorValues>& vconfig;
-  _VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig) : ordering(_ordering), vconfig(_vconfig) {}
+  Index nextVar;
+  _VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){}
   template<typename I>
   void operator()(I xIt) {
     const bool debug = ISDEBUG("ISAM2 AddVariables");
-    Index var = vconfig->push_back_preallocated(zero(xIt->second.dim()));
+    vconfig.permutation()[nextVar] = nextVar;
-    vconfig.permutation()[var] = var;
+    ordering.insert(xIt->first, nextVar);
-    ordering.insert(xIt->first, var);
+    if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl;
-    if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl;
+    ++ nextVar;
   }
 };
 
@@ -145,15 +146,18 @@ void ISAM2<CONDITIONAL,VALUES>::Impl::AddVariables(
   theta.insert(newTheta);
   if(debug) newTheta.print("The new variables are: ");
   // Add the new keys onto the ordering, add zeros to the delta for the new variables
-  vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary(ordering.nVars())));
+  vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
   if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
-  delta.container().reserve(delta->size() + newTheta.size(), delta->dim() + accumulate(dims.begin(), dims.end(), 0));
+  const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
-  delta.permutation().resize(delta->size() + newTheta.size());
+  const size_t originalDim = delta->dim();
+  const size_t originalnVars = delta->size();
+  delta.container().append(dims);
+  delta.container().vector().segment(originalDim, newDim) = Vector::Zero(newDim);
+  delta.permutation().resize(originalnVars + newTheta.size());
   {
-    _VariableAdder vadder(ordering, delta);
+    _VariableAdder vadder(ordering, delta, originalnVars);
     newTheta.apply(vadder);
     assert(delta.permutation().size() == delta.container().size());
-    assert(delta.container().dim() == delta.container().dimCapacity());
     assert(ordering.nVars() == delta.size());
     assert(ordering.size() == delta.size());
   }

gtsam/nonlinear/LieValues-inl.h

@@ -77,9 +77,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class J>
   VectorValues LieValues<J>::zero(const Ordering& ordering) const {
-  	VectorValues z(this->dims(ordering));
+  	return VectorValues::Zero(this->dims(ordering));
-  	z.makeZero();
-  	return z;
   }
 
   /* ************************************************************************* */

gtsam/nonlinear/TupleValues.h

@@ -172,9 +172,7 @@ namespace gtsam {
 
 	  /** zero: create VectorValues of appropriate structure */
 	  VectorValues zero(const Ordering& ordering) const {
-		  VectorValues z(this->dims(ordering));
+		  return VectorValues::Zero(this->dims(ordering));
-		  z.makeZero();
-		  return z;
 	  }
 
 	  /** @return number of key/value pairs stored */
@@ -313,9 +311,7 @@ namespace gtsam {
 	  const Value1& at(const Key1& j) const { return first_.at(j); }
 
 	  VectorValues zero(const Ordering& ordering) const {
-		  VectorValues z(this->dims(ordering));
+		  return VectorValues::Zero(this->dims(ordering));
-		  z.makeZero();
-		  return z;
 	  }
 
 	  size_t size() const { return first_.size(); }

gtsam/slam/tests/testPose2SLAM.cpp

@@ -352,16 +352,14 @@ TEST( Pose2Prior, error )
 	    boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
 
 	// Check error at x0, i.e. delta = zero !
-	VectorValues delta(x0.dims(ordering));
+	VectorValues delta(VectorValues::Zero(x0.dims(ordering)));
-	delta.makeZero();
 	delta[ordering["x1"]] = zero(3);
 	Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
 	CHECK(assert_equal(error_at_zero,factor.whitenedError(x0)));
 	CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
 
 	// Check error after increasing p2
-	VectorValues addition(x0.dims(ordering));
+	VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
-	addition.makeZero();
 	addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
 	VectorValues plus = delta + addition;
 	Pose2Values x1 = x0.expmap(plus, ordering);

gtsam/slam/tests/testPose3SLAM.cpp

@@ -217,11 +217,12 @@ TEST( Pose3Values, expmap )
 	// so shifting to East requires little thought, different from with Pose2 !!!
 
 	Ordering ordering(*expected.orderingArbitrary());
-	VectorValues delta(expected.dims(ordering), Vector_(24,
+	VectorValues delta(expected.dims(ordering));
+	delta.vector() = Vector_(24,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
-			0.0,0.0,0.0,  0.1, 0.0, 0.0));
+			0.0,0.0,0.0,  0.1, 0.0, 0.0);
 	Pose3Values actual = pose3SLAM::circle(4,1.0).expmap(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }

tests/testGaussianISAM.cpp

@@ -70,8 +70,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
 	EXPECT(assert_equal(expected, actual));
 
 	// obtain solution
-	VectorValues e(vector<size_t>(7,2)); // expected solution
+	VectorValues e(VectorValues::Zero(vector<size_t>(7,2))); // expected solution
-	e.makeZero();
 	VectorValues optimized = optimize(actual); // actual solution
 	EXPECT(assert_equal(e, optimized));
 }
@@ -182,8 +181,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
   // Create the Bayes tree
   BayesTree<GaussianConditional> chordalBayesNet = *GaussianMultifrontalSolver(smoother).eliminate();
 
-	VectorValues expectedSolution(7, 2);
+	VectorValues expectedSolution(VectorValues::Zero(vector<size_t>(7,2)));
-	expectedSolution.makeZero();
 	VectorValues actualSolution = optimize(chordalBayesNet);
 	EXPECT(assert_equal(expectedSolution,actualSolution,tol));
 

tests/testGaussianISAM2.cpp

@@ -39,9 +39,8 @@ TEST(ISAM2, AddVariables) {
   newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
 
   VectorValues deltaUnpermuted;
-  deltaUnpermuted.reserve(2, 5);
+  deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3));
-  { Vector a(3); a << .1, .2, .3; deltaUnpermuted.push_back_preallocated(a); }
+  deltaUnpermuted.insert(1, Vector_(2, .4, .5));
-  { Vector a(2); a << .4, .5; deltaUnpermuted.push_back_preallocated(a); }
 
   Permutation permutation(2);
   permutation[0] = 1;
@@ -66,10 +65,9 @@ TEST(ISAM2, AddVariables) {
   thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
 
   VectorValues deltaUnpermutedExpected;
-  deltaUnpermutedExpected.reserve(3, 8);
+  deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
-  { Vector a(3); a << .1, .2, .3; deltaUnpermutedExpected.push_back_preallocated(a); }
+  deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5));
-  { Vector a(2); a << .4, .5; deltaUnpermutedExpected.push_back_preallocated(a); }
+  deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
-  { Vector a(3); a << 0, 0, 0; deltaUnpermutedExpected.push_back_preallocated(a); }
 
   Permutation permutationExpected(3);
   permutationExpected[0] = 1;

tests/testSerialization.cpp

@@ -314,12 +314,10 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
 
 /* ************************************************************************* */
 TEST (Serialization, linear_factors) {
-	vector<size_t> dims;
+  VectorValues values;
-	dims.push_back(1);
+  values.insert(0, Vector_(1, 1.0));
-	dims.push_back(2);
+  values.insert(1, Vector_(2, 2.0,3.0));
-	dims.push_back(2);
+  values.insert(2, Vector_(2, 4.0,5.0));
-	double v[] = {1., 2., 3., 4., 5.};
-	VectorValues values(dims, v);
 	EXPECT(equalsObj<VectorValues>(values));
 	EXPECT(equalsXML<VectorValues>(values));