diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 39e426478..d592ccdfe 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/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; jdim(j) != other.dim(j)) return false; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 445b659e5..0bcafd23c 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -27,8 +27,8 @@ namespace gtsam { /** - * This class stores a collection of vector-valued variables, each referenced - * by a unique variable index. It is typically used to store the variables + * This class represents a collection of vector-valued variables associated + * 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(1, Vector_(2, 3.0, 4.0)); + values.insert(3, Vector_(3, 1.0, 2.0, 3.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 - * 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. + *

Advanced Interface and Performance Information

+ * + * Internally, all vector values are stored as part of one large vector. In + * gtsam this vector is always pre-allocated for efficiency, using the + * advanced interface described below. Accessing and modifying already-allocated + * values is \f$ O(1) \f$. Using the insert() function of the standard interface + * is slow because it requires re-allocating the internal 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 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. */ - bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } // + /** Check whether a variable with index \c j exists. */ + 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]; } // + /** 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& at(Index j) { checkExists(j); return maps_[j]; } - /** Reference a variable by index. */ - const SubVector& operator[](Index j) const { checkExists(j); return maps_[j]; } // + /** 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& 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 - 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 + reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables + const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables + reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse 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 - 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 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 - * it all variables will be uninitialized. + * \c varDim, not preserving any data. After calling this function, 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. + * in \c dimensions, not preserving any data. The new variables are + * uninitialized, but this function is used to pre-allocate space for + * performance. After calling this function all variables will be uninitialized. * @param dimensions A container of the dimension of each variable to create. */ template @@ -205,13 +236,13 @@ namespace gtsam { * @param dimensions A container of the dimension of each variable to create. */ template - 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, diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 613084120..a92b81ddc 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -135,14 +135,17 @@ TEST(VectorValues, copyConstructor) { /* ************************************************************************* */ 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; - VectorValues actual = original; + { + // 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; + } // 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 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 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 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); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 3249cfac3..3b432393d 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -70,7 +70,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) EXPECT(assert_equal(expected, actual)); // obtain solution - VectorValues e(VectorValues::Zero(vector(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 chordalBayesNet = *GaussianMultifrontalSolver(smoother).eliminate(); - VectorValues expectedSolution(VectorValues::Zero(vector(7,2))); + VectorValues expectedSolution(VectorValues::Zero(7,2)); VectorValues actualSolution = optimize(chordalBayesNet); EXPECT(assert_equal(expectedSolution,actualSolution,tol));