VectorValues documentation and interface cleanup, additional unit tests and bug fix.

gtsam/linear/VectorValues.cpp

@@ -121,8 +121,17 @@ VectorValues VectorValues::SameStructure(const VectorValues& other) {
   return ret;
 }
 
+/* ************************************************************************* */
+VectorValues VectorValues::Zero(Index nVars, size_t varDim) {
+  VectorValues ret(nVars, varDim);
+  ret.vector() = Vector::Zero(ret.dim());
+  return ret;
+}
+
 /* ************************************************************************* */
 bool VectorValues::hasSameStructure(const VectorValues& other) const {
+  if(this->size() != other.size())
+    return false;
   for(size_t j=0; j<size(); ++j)
     if(this->dim(j) != other.dim(j))
       return false;

gtsam/linear/VectorValues.h

@@ -27,8 +27,8 @@
 namespace gtsam {
 
   /**
-   * This class stores a collection of vector-valued variables, each referenced
+   * This class represents a collection of vector-valued variables associated
-   * by a unique variable index.  It is typically used to store the variables
+   * each with a unique integer index.  It is typically used to store the variables
    * of a GaussianFactorGraph.  Optimizing a GaussianFactorGraph or GaussianBayesNet
    * returns this class.
    *
@@ -41,11 +41,16 @@ namespace gtsam {
    *  - \ref exists (Index) to check if a variable is present
    *  - Other facilities like iterators, size(), dim(), etc.
    *
+   * Indices can be non-consecutive and inserted out-of-order, but you should not
+   * use indices that are larger than a reasonable array size because the indices
+   * correspond to positions in an internal array.
+   *
    * Example:
    * \code
      VectorValues values;
-     values.insert(0, Vector_(3, 5.0, 6.0, 7.0));
+     values.insert(3, Vector_(3, 1.0, 2.0, 3.0));
-     values.insert(1, Vector_(2, 3.0, 4.0));
+     values.insert(4, Vector_(2, 4.0, 5.0));
+     values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0));
 
      // Prints [ 3.0 4.0 ]
      gtsam::print(values[1]);
@@ -55,15 +60,24 @@ namespace gtsam {
      gtsam::print(values[1]);
      \endcode
    *
-   * Internally, this class stores all vectors as part of one large vector.  This is
+   * <h2>Advanced Interface and Performance Information</h2>
-   * necessary for performance, and the gtsam linear solving code exploits it by
+   *
-   * only allocating one large vector to store the solution.  For advanced usage,
+   * Internally, all vector values are stored as part of one large vector.  In
-   * or where speed is important, be aware of the following:
+   * gtsam this vector is always pre-allocated for efficiency, using the
-   *  - It is faster to allocate space ahead of time using a pre-allocating constructor
+   * advanced interface described below.  Accessing and modifying already-allocated
-   *    or the resize() and append() functions, than to use insert(Index, const Vector&),
+   * values is \f$ O(1) \f$.  Using the insert() function of the standard interface
-   *    which always has to re-allocate the internal vector.
+   * is slow because it requires re-allocating the internal vector.
-   *  - The vector() function permits access to the underlying Vector.
+   *
-   *  - operator[]() returns a SubVector view of the underlying Vector.
+   * For advanced usage, or where speed is important:
+   *  - Allocate space ahead of time using a pre-allocating constructor
+   *    (\ref AdvancedConstructors "Advanced Constructors"), Zero(),
+   *    SameStructure(), resize(), or append().  Do not use
+   *    insert(Index, const Vector&), which always has to re-allocate the
+   *    internal vector.
+   *  - The vector() function permits access to the underlying Vector, for
+   *    doing mathematical or other operations that require all values.
+   *  - operator[]() returns a SubVector view of the underlying Vector,
+   *    without copying any data.
    *
    * Access is through the variable index j, and returns a SubVector,
    * which is a view on the underlying data structure.
@@ -84,16 +98,16 @@ namespace gtsam {
     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
 
-    /// @name Standard constructors
+    /// @name Standard Constructors
     /// @{
 
     /**
      * Default constructor creates an empty VectorValues.
      */
-    VectorValues() {} //
+    VectorValues() {}
 
     /** Copy constructor */
-    VectorValues(const VectorValues &other); //
+    VectorValues(const VectorValues &other);
 
     /** Named constructor to create a VectorValues of the same structure of the
      * specifed one, but filled with zeros.
@@ -102,27 +116,33 @@ namespace gtsam {
     static VectorValues Zero(const VectorValues& model);
 
     /// @}
-    /// @name Standard interface
+    /// @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(); } //
+    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(); } //
+    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(); } //
+    size_t dim() const { return values_.rows(); }
 
-    /** Check whether a variable exists by index. */
+    /** Check whether a variable with index \c j exists. */
-    bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } //
+    bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; }
 
-    /** Reference a variable by index. */
+    /** 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). */
-    SubVector& operator[](Index j) { checkExists(j); return maps_[j]; } //
+    SubVector& at(Index j) { checkExists(j); return maps_[j]; }
 
-    /** Reference a variable by index. */
+    /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
-    const SubVector& operator[](Index j) const { checkExists(j); return maps_[j]; } //
+    const SubVector& at(Index j) const { checkExists(j); return maps_[j]; }
+
+    /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */
+    SubVector& operator[](Index j) { return at(j); }
+
+    /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */
+    const SubVector& 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
@@ -130,68 +150,79 @@ namespace gtsam {
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated.
      */
-    void insert(Index j, const Vector& value); //
+    void insert(Index j, const Vector& value);
 
     /** Assignment */
-    VectorValues& operator=(const VectorValues& rhs); //
+    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
+    reverse_iterator rbegin()             { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
-    const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Iterator over variables
+    const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
-    reverse_iterator rend()               { chk(); return maps_.rend(); }   ///< Iterator over variables
+    reverse_iterator rend()               { chk(); return maps_.rend(); }   ///< Reverse iterator over variables
-    const_reverse_iterator rend() const   { chk(); return maps_.rend(); }   ///< Iterator over variables
+    const_reverse_iterator rend() const   { chk(); return maps_.rend(); }   ///< Reverse iterator over variables
 
     /** print required by Testable for unit testing */
-    void print(const std::string& str = "VectorValues: ") const; //
+    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; //
+    bool equals(const VectorValues& x, double tol = 1e-9) const;
 
     /// @}
-    /// @name Advanced constructors
+    /// \anchor AdvancedConstructors
+    /// @name Advanced Constructors
     /// @{
 
     /** Construct from a container of variable dimensions (in variable order). */
     template<class CONTAINER>
-    VectorValues(const CONTAINER& dimensions) { append(dimensions); } //
+    VectorValues(const CONTAINER& dimensions) { 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) { resize(nVars, varDim); }
 
     /** Named constructor to create a VectorValues that matches the structure of
      * the specified VectorValues, but do not initialize the new values. */
-    static VectorValues SameStructure(const VectorValues& other); //
+    static VectorValues SameStructure(const VectorValues& other);
 
     /** Named constructor to create a VectorValues from a container of variable
-     * dimensions that is filled with zeros. */
+     * dimensions that is filled with zeros.
+     * @param dimensions A container of the dimension of each variable to create.
+     */
     template<class CONTAINER>
     static VectorValues Zero(const CONTAINER& dimensions);
 
+    /** Named constructor to create a VectorValues filled with zeros that has
+     * \c nVars variables, each of dimension \c varDim
+     * @param nVars The number of variables to create
+     * @param varDim The dimension of each variable
+     * @return The new VectorValues
+     */
+    static VectorValues Zero(Index nVars, size_t varDim);
+
     /// @}
-    /// @name Advanced interface
+    /// @name Advanced Interface
     /// @{
 
     /** Resize this VectorValues to have identical structure to other, leaving
      * this VectorValues with uninitialized values.
      * @param other The VectorValues whose structure to copy
      */
-    void resizeLike(const VectorValues& other); //
+    void resizeLike(const VectorValues& other);
 
     /** Resize the VectorValues to hold \c nVars variables, each of dimension
-     * \c varDim.  This function does not preserve any data, after calling
+     * \c varDim, not preserving any data.  After calling this function, all
-     * it all variables will be uninitialized.
+     * 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
+     * in \c dimensions, not preserving any data.  The new variables are
-     * is used to pre-allocate space for performance.  This function does not
+     * uninitialized, but this function is used to pre-allocate space for
-     * preserve any data, after calling it all variables 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>
@@ -205,13 +236,13 @@ namespace gtsam {
      * @param dimensions A container of the dimension of each variable to create.
      */
     template<class CONTAINER>
-    void append(const CONTAINER& dimensions); //
+    void append(const CONTAINER& dimensions);
 
     /** Reference the entire solution vector (const version). */
-    const Vector& vector() const { chk(); return values_; } //
+    const Vector& vector() const { chk(); return values_; }
 
     /** Reference the entire solution vector. */
-    Vector& vector() { chk(); return values_; } //
+    Vector& vector() { chk(); return values_; }
 
     /** Check whether this VectorValues has the same structure, meaning has the
      * same number of variables and that all variables are of the same dimension,

gtsam/linear/tests/testVectorValues.cpp

@@ -135,14 +135,17 @@ TEST(VectorValues, copyConstructor) {
 /* ************************************************************************* */
 TEST(VectorValues, assignment) {
 
+  VectorValues actual;
+
+  {
     // 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));
-
+    actual = original;
-  VectorValues actual = original;
+  }
 
   // Check dimensions
   LONGS_EQUAL(6, actual.size());
@@ -204,6 +207,83 @@ TEST(VectorValues, SameStructure) {
   CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
 }
 
+/* ************************************************************************* */
+TEST(VectorValues, Zero_fromModel) {
+  // 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::Zero(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));
+
+  // Values
+  EXPECT(assert_equal(Vector::Zero(1), actual[0]));
+  EXPECT(assert_equal(Vector::Zero(2), actual[1]));
+  EXPECT(assert_equal(Vector::Zero(2), actual[5]));
+  EXPECT(assert_equal(Vector::Zero(2), actual[2]));
+
+  // 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, Zero_fromDims) {
+  vector<size_t> dims;
+  dims.push_back(1);
+  dims.push_back(2);
+  dims.push_back(2);
+
+  VectorValues actual(VectorValues::Zero(dims));
+
+  // 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));
+
+  // Values
+  EXPECT(assert_equal(Vector::Zero(1), actual[0]));
+  EXPECT(assert_equal(Vector::Zero(2), actual[1]));
+  EXPECT(assert_equal(Vector::Zero(2), actual[2]));
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, Zero_fromUniform) {
+  VectorValues actual(VectorValues::Zero(3, 2));
+
+  // 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));
+
+  // Values
+  EXPECT(assert_equal(Vector::Zero(2), actual[0]));
+  EXPECT(assert_equal(Vector::Zero(2), actual[1]));
+  EXPECT(assert_equal(Vector::Zero(2), actual[2]));
+}
+
 /* ************************************************************************* */
 TEST(VectorValues, resizeLike) {
   // insert, with out-of-order indices
@@ -237,6 +317,56 @@ TEST(VectorValues, resizeLike) {
   CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
 }
 
+/* ************************************************************************* */
+TEST(VectorValues, resize_fromUniform) {
+  VectorValues actual(4, 10);
+  actual.resize(3, 2);
+
+  actual[0] = Vector_(2, 1.0, 2.0);
+  actual[1] = Vector_(2, 2.0, 3.0);
+  actual[2] = Vector_(2, 4.0, 5.0);
+
+  // 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));
+
+  // Check values
+  EXPECT(assert_equal(Vector_(2, 1.0, 2.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_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
+}
+
+/* ************************************************************************* */
+TEST(VectorValues, resize_fromDims) {
+  vector<size_t> dims;
+  dims.push_back(1);
+  dims.push_back(2);
+  dims.push_back(2);
+
+  VectorValues actual(4, 10);
+  actual.resize(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());
+  LONGS_EQUAL(5, actual.dim());
+  LONGS_EQUAL(1, actual.dim(0));
+  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, append) {
   // insert
@@ -277,6 +407,20 @@ TEST(VectorValues, append) {
   CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument);
 }
 
+/* ************************************************************************* */
+TEST(VectorValues, hasSameStructure) {
+  VectorValues v1(2, 3);
+  VectorValues v2(3, 2);
+  VectorValues v3(4, 2);
+  VectorValues v4(4, 2);
+
+  EXPECT(!v1.hasSameStructure(v2));
+  EXPECT(!v2.hasSameStructure(v3));
+  EXPECT(v3.hasSameStructure(v4));
+  EXPECT(VectorValues().hasSameStructure(VectorValues()));
+  EXPECT(!v1.hasSameStructure(VectorValues()));
+}
+
 /* ************************************************************************* */
 TEST(VectorValues, permuted_combined) {
   Vector v1 = Vector_(3, 1.0,2.0,3.0);
@@ -323,56 +467,8 @@ TEST(VectorValues, permuted_combined) {
   CHECK(assert_equal(v1, permuted2[1]))
   CHECK(assert_equal(v2, permuted2[2]))
   CHECK(assert_equal(v3, permuted2[0]))
-
 }
 
-///* ************************************************************************* */
-//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() {
   TestResult tr; return TestRegistry::runAllTests(tr);

tests/testGaussianISAM.cpp

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