From 2c53df3ee7e4db6017d64ac7d38ff23cb8bc45b7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 26 Oct 2011 02:04:06 +0000 Subject: [PATCH] Simplified VectorValues interface and implementation, slight simplification to Permuted<> --- gtsam/inference/Permutation.h | 10 +- gtsam/inference/VariableIndex.h | 2 +- gtsam/linear/GaussianConditional.cpp | 53 +- .../linear/IterativeOptimizationParameters.h | 28 +- gtsam/linear/JacobianFactor.cpp | 6 +- gtsam/linear/SubgraphPreconditioner.cpp | 4 +- gtsam/linear/SubgraphPreconditioner.h | 3 +- gtsam/linear/VectorValues.cpp | 145 +++-- gtsam/linear/VectorValues.h | 599 +++++++++--------- gtsam/linear/tests/testVectorValues.cpp | 449 ++++++++----- gtsam/nonlinear/GaussianISAM2.cpp | 2 +- gtsam/nonlinear/ISAM2-impl-inl.h | 24 +- gtsam/nonlinear/LieValues-inl.h | 4 +- gtsam/nonlinear/TupleValues.h | 8 +- gtsam/slam/tests/testPose2SLAM.cpp | 6 +- gtsam/slam/tests/testPose3SLAM.cpp | 5 +- tests/testGaussianISAM.cpp | 6 +- tests/testGaussianISAM2.cpp | 12 +- tests/testSerialization.cpp | 10 +- 19 files changed, 765 insertions(+), 611 deletions(-) diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 43129e0d4..421d03ad5 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -151,12 +151,12 @@ protected: * container[permutation[index2]]; * but more concise. */ -template +template 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) */ -// const_value_type operator[](size_t index) const; + /** Access the container through the permutation (const version) */ + 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); } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 0d4456718..0c9461bfe 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -45,7 +45,7 @@ public: protected: std::vector indexUnpermuted_; - Permuted, Factors&> index_; // Permuted view of indexUnpermuted. + Permuted > 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. diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 91de58b50..37d9c1add 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/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 +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 +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(); - x.range(beginFrontals(), endFrontals(), d); + writeVectorValuesSlices(get_d(), x, beginFrontals(), endFrontals()); } /* ************************************************************************* */ void GaussianConditional::rhs(Permuted& 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& 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()); } } diff --git a/gtsam/linear/IterativeOptimizationParameters.h b/gtsam/linear/IterativeOptimizationParameters.h index dd421f394..cc9a648aa 100644 --- a/gtsam/linear/IterativeOptimizationParameters.h +++ b/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 skeleton_spec_; bool est_cond_ ; public: @@ -80,4 +82,28 @@ public: return est_cond_ ; } }; + +struct DimSpec: public std::vector { + + typedef std::vector Base; + typedef boost::shared_ptr 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(); + } + } +}; + } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2295213ec..50d6474ee 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -580,7 +580,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues gradient(const FactorGraph& 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& 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& 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) { diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 3aa0f32c5..f3ca8445a 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/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 } diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index aca2398e4..1e7e6e972 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -76,8 +76,7 @@ namespace gtsam { /* A zero VectorValues with the structure of xbar */ VectorValues zero() const { - VectorValues V(*xbar_) ; - V.makeZero(); + VectorValues V(VectorValues::Zero(*xbar_)) ; return V ; } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index b33e44c1d..39e426478 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -16,56 +16,69 @@ * @author Alex Cunningham */ +#include #include using namespace std; using namespace gtsam; /* ************************************************************************* */ -VectorValues::VectorValues(Index nVars, size_t varDim) : - varStarts_(nVars + 1) { - 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 VectorValues& other) { + *this = other; } /* ************************************************************************* */ -VectorValues::VectorValues(const std::vector& dimensions, - const Vector& values) : - values_(values), varStarts_(dimensions.size() + 1) { - varStarts_[0] = 0; - size_t varStart = 0; - Index var = 0; - BOOST_FOREACH(size_t dim, dimensions) - varStarts_[++var] = (varStart += dim); - assert(varStarts_.back() == (size_t) values.size()); +VectorValues& VectorValues::operator=(const VectorValues& rhs) { + if(this != &rhs) { + resizeLike(rhs); // Copy structure + values_ = rhs.values_; // Copy values + } + return *this; } /* ************************************************************************* */ -VectorValues::VectorValues(const std::vector& dimensions, - const double* values) : - varStarts_(dimensions.size() + 1) { - varStarts_[0] = 0; - size_t varStart = 0; - Index var = 0; - BOOST_FOREACH(size_t dim, dimensions) - varStarts_[++var] = (varStart += dim); - values_ = Vector_(varStart, values); +VectorValues VectorValues::Zero(const VectorValues& x) { + VectorValues cloned(SameStructure(x)); + cloned.vector() = Vector::Zero(x.dim()); + return cloned; } /* ************************************************************************* */ -VectorValues VectorValues::SameStructure(const VectorValues& otherValues) { - VectorValues ret; - ret.varStarts_ = otherValues.varStarts_; - ret.values_.resize(ret.varStarts_.back()); - return ret; +void VectorValues::insert(Index j, const Vector& value) { + // Make sure j does not already exist + if(exists(j)) + throw invalid_argument("VectorValues: requested variable index to insert already exists."); + + // Get vector of dimensions + FastVector dimensions(size()); + for(size_t k=0; k= 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_ - && equal_with_abs_tol(values_, x.values_, tol); + return hasSameStructure(x) && equal_with_abs_tol(values_, x.values_, tol); } /* ************************************************************************* */ -size_t VectorValues::dim(Index j) const { - checkVariable(j); - const size_t start = varStarts_[j], n = varStarts_[j + 1] - start; - return n; +void VectorValues::resize(Index nVars, size_t varDim) { + maps_.clear(); + maps_.reserve(nVars); + 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) { - checkVariable(j); - const size_t start = varStarts_[j], n = varStarts_[j + 1] - start; - return values_.segment(start, n); +void VectorValues::resizeLike(const VectorValues& other) { + values_.resize(other.dim()); + // Create SubVectors referencing our values_ vector + 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 { - checkVariable(j); - const size_t start = varStarts_[j], n = varStarts_[j + 1] - start; - return values_.segment(start, n); +VectorValues VectorValues::SameStructure(const VectorValues& other) { + VectorValues ret; + ret.resizeLike(other); + return ret; } /* ************************************************************************* */ -VectorValues VectorValues::zero(const VectorValues& x) { - VectorValues cloned(x); - cloned.makeZero(); - return cloned; -} - -/* ************************************************************************* */ -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; +bool VectorValues::hasSameStructure(const VectorValues& other) const { + for(size_t j=0; jdim(j) != other.dim(j)) + return false; + return true; } /* ************************************************************************* */ VectorValues VectorValues::operator+(const VectorValues& c) const { - assert(varStarts_ == c.varStarts_); - VectorValues result; - result.varStarts_ = varStarts_; - result.values_ = values_.head(varStarts_.back()) - + c.values_.head(varStarts_.back()); + assert(this->hasSameStructure(c)); + VectorValues result(SameStructure(c)); + result.values_ = this->values_ + c.values_; return result; } /* ************************************************************************* */ void VectorValues::operator+=(const VectorValues& c) { - assert(varStarts_ == c.varStarts_); - this->values_ += c.values_.head(varStarts_.back()); + assert(this->hasSameStructure(c)); + this->values_ += c.values_; } - -/* ************************************************************************* */ - diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index eeca4026c..bc7bf036c 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -22,364 +22,345 @@ #include #include +#include namespace gtsam { - /** - * The class VectorValues stores a number of Vectors. - * Typically, this class is used in back-substitution (Bayes Net solve). - * - * There are a number of constructors that simply reserve space - * (Frank is not a big fan of this imperative style, but it is needed for speed) - * and others - safer ones - that actually initialize values. - * SameStructure and reserve are two other utility functions that manage storage. - * - * Access is through the variable index j, and returns a SubVector, - * which is a view on the underlying data structure. - */ - class VectorValues { - protected: - Vector values_; - std::vector varStarts_; // start at 0 with size nVars + 1 + /** + * This class stores a collection of vector-valued variables, each referenced + * by a unique variable index. It is typically used to store the variables + * of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet + * returns this class. + * + * For basic usage, such as receiving a linear solution from gtsam solving functions, + * or creating this class in unit tests and examples where speed is not important, + * you can use a simple interface: + * - The default constructor VectorValues() to create this class + * - insert(Index, const Vector&) to add vector variables + * - 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)); - public: - // Forward declaration of iterator implementation - template class _impl_iterator; - typedef _impl_iterator iterator; - typedef _impl_iterator const_iterator; + // Prints [ 3.0 4.0 ] + gtsam::print(values[1]); - // Some other typedefs - typedef boost::shared_ptr shared_ptr; - typedef SubVector value_reference_type; - typedef ConstSubVector const_value_reference_type; - typedef SubVector mapped_type; - typedef ConstSubVector const_mapped_type; + // 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_; ///< The underlying vector storing the values + typedef std::vector ValueMaps; ///< Collection of SubVector s + ValueMaps maps_; ///< SubVector s referencing each vector variable in values_ - /** Constructors that simply reserve space */ + public: + typedef ValueMaps::iterator iterator; ///< Iterator over vector values + typedef ValueMaps::const_iterator const_iterator; ///< Const iterator over vector values + typedef ValueMaps::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values + 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 - /** - * Default constructor creates an empty VectorValues. reserve(...) must be - * called to allocate space before any values can be added. This prevents - * slow reallocation of space at runtime. - */ - VectorValues() : - varStarts_(1, 0) { - } + /// @name Standard constructors + /// @{ - /** Construct from a container of variable dimensions (in variable order). */ - template - VectorValues(const CONTAINER& dimensions); + /** + * Default constructor creates an empty VectorValues. + */ + VectorValues() {} // - /** Construct to hold nVars vectors of varDim dimension each. */ - VectorValues(Index nVars, size_t varDim); + /** Copy constructor */ + VectorValues(const VectorValues &other); // - /** Constructors that actually initialize values */ + /** 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); - /** 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& dimensions, const Vector& values); + /// @} + /// @name Standard interface + /// @{ - /** Construct from the variable dimensions in varaible order and a double array that contains actual values */ - VectorValues(const std::vector& dimensions, const double* values); + /** 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(); } // - /** Copy constructor */ - VectorValues(const VectorValues &V) : - values_(V.values_), varStarts_(V.varStarts_) { - } + /** Return the dimension of variable \c j. */ + size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } // - /** 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); + /** Return the summed dimensionality of all variables. */ + size_t dim() const { return values_.rows(); } // - /** Reserve space for a total number of variables and dimensionality */ - void reserve(Index nVars, size_t totalDims) { - values_.conservativeResize(totalDims); - varStarts_.reserve(nVars + 1); - } + /** Check whether a variable exists by index. */ + bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } // - /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ") const; + /** Reference a variable by index. */ + SubVector& operator[](Index j) { checkExists(j); return maps_[j]; } // - /** equals required by Testable for unit testing */ - bool equals(const VectorValues& x, double tol = 1e-9) const; + /** Reference a variable by index. */ + const SubVector& operator[](Index j) const { checkExists(j); return maps_[j]; } // - /** Number of elements */ - Index size() const { - return varStarts_.size() - 1; - } + /** Insert a new variable (causes reallocation). */ + void insert(Index j, const Vector& value); // - /** dimension of a particular element */ - size_t dim(Index j) const; + /** Assignment */ + VectorValues& operator=(const VectorValues& rhs); // - /** Total dimensionality used (could be smaller than what has been allocated - * with reserve(...) ). - */ - size_t dim() const { - return varStarts_.back(); - } + 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 - /** Total dimensions capacity allocated */ - size_t dimCapacity() const { - return values_.size(); - } + /** print required by Testable for unit testing */ + void print(const std::string& str = "VectorValues: ") const; // - /** Reference the entire solution vector (const version). */ - const Vector& vector() const { - return values_; - } + /** equals required by Testable for unit testing */ + bool equals(const VectorValues& x, double tol = 1e-9) const; // - /** Reference the entire solution vector. */ - Vector& vector() { - return values_; - } + /// @} + /// @name Advanced constructors + /// @{ - /** Individual element access */ - mapped_type operator[](Index j); - const_mapped_type operator[](Index j) const; + /** Construct from a container of variable dimensions (in variable order). */ + template + VectorValues(const CONTAINER& dimensions) { append(dimensions); } // - /** Iterator access */ - iterator begin() { - return _impl_iterator(*this, 0); - } - const_iterator begin() const { - return _impl_iterator(*this, 0); - } - iterator end() { - return _impl_iterator(*this, varStarts_.size() - 1); - } - const_iterator end() const { - return _impl_iterator(*this, varStarts_.size() - 1); - } + /** Construct to hold nVars vectors of varDim dimension each. */ + VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); } // - /** access a range of indices (of no particular order) as a single vector */ - template - Vector range(const ITERATOR& idx_begin, const ITERATOR& idx_end) const; + /** 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); // - /** set a range of indices as a single vector split across the range */ - template - void range(const ITERATOR& idx_begin, const ITERATOR& idx_end, - const Vector& v); + /** Named constructor to create a VectorValues from a container of variable + * dimensions that is filled with zeros. */ + template + static VectorValues Zero(const CONTAINER& dimensions); - /** Set all elements to zero */ - void makeZero() { - values_.setZero(); - } + /// @} + /// @name Advanced interface + /// @{ - /** Copy structure of x, but set all values to zero */ - static VectorValues zero(const VectorValues& x); + /** 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); // - /** - * 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); + /** 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. + * @param nVars The number of variables to create + * @param varDim The dimension of each variable + */ + void resize(Index nVars, size_t varDim); - /* dot product */ - double dot(const VectorValues& V) const { - return gtsam::dot(this->values_, V.values_); - } + /** 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 + void resize(const CONTAINER& dimensions); - /** - * + operator simply adds Vectors. This checks for structural equivalence - * when NDEBUG is not defined. - */ - VectorValues operator+(const VectorValues& c) const; + /** 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 + void append(const CONTAINER& dimensions); // - void operator+=(const VectorValues& c); + /** Reference the entire solution vector (const version). */ + const Vector& vector() const { chk(); return values_; } // - /** - * Iterator (handles both iterator and const_iterator depending on whether - * the template type is const. - */ - template - class _impl_iterator { - protected: - C& config_; - Index curVariable_; + /** Reference the entire solution vector. */ + Vector& vector() { chk(); return values_; } // - _impl_iterator(C& config, Index curVariable) : - config_(config), curVariable_(curVariable) { - } - void checkCompat(const _impl_iterator& r) { - assert(&config_ == &r.config_); - } - friend class VectorValues; + /** Check whether this VectorValues has the same structure, meaning has the + * same number of variables and that all variables are of the same dimension, + * as another VectorValues + * @param other The other VectorValues with which to compare structure + * @return \c true if the structure is the same, \c false if not. + */ + bool hasSameStructure(const VectorValues& other) const; - public: - typedef typename const_selector::type value_type; - _impl_iterator& operator++() { - ++curVariable_; - return *this; - } - _impl_iterator& operator--() { - --curVariable_; - return *this; - } - _impl_iterator& operator++(int) { - throw std::runtime_error("Use prefix ++ operator"); - } - _impl_iterator& operator--(int) { - throw std::runtime_error("Use prefix -- operator"); - } - _impl_iterator& operator+=(ptrdiff_t step) { - curVariable_ += step; - return *this; - } - _impl_iterator& operator-=(ptrdiff_t step) { - curVariable_ += step; - return *this; - } - ptrdiff_t operator-(const _impl_iterator& r) { - checkCompat(r); - return curVariable_ - r.curVariable_; - } - bool operator==(const _impl_iterator& r) { - checkCompat(r); - return curVariable_ == r.curVariable_; - } - bool operator!=(const _impl_iterator& r) { - checkCompat(r); - return curVariable_ != r.curVariable_; - } - value_type operator*() { - return config_[curVariable_]; - } - }; + /** 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_); + } - protected: - void checkVariable(Index j) const { - assert(j < varStarts_.size()-1); - } + /** + * + operator does element-wise addition. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). + */ + VectorValues operator+(const VectorValues& c) const; - public: - friend size_t dim(const VectorValues& V) { - return V.varStarts_.back(); - } - friend double dot(const VectorValues& V1, const VectorValues& V2) { - return gtsam::dot(V1.values_, V2.values_); - } - friend void scal(double alpha, VectorValues& x) { - gtsam::scal(alpha, x.values_); - } - friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { - gtsam::axpy(alpha, x.values_, y.values_); - } - friend void sqrt(VectorValues &x) { - Vector y = gtsam::esqrt(x.values_); - x.values_ = y; - } + /** + * += operator does element-wise addition. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). + */ + void operator+=(const VectorValues& c); - friend void ediv(const VectorValues& numerator, - const VectorValues& denominator, VectorValues &result) { - assert( - numerator.dim() == denominator.dim() && denominator.dim() == result.dim()); - const size_t sz = result.dim(); - for (size_t i = 0; i < sz; ++i) - result.values_[i] = numerator.values_[i] / denominator.values_[i]; - } + /// @} - friend void edivInPlace(VectorValues& x, const VectorValues& y) { - assert(x.dim() == y.dim()); - const size_t sz = x.dim(); - for (size_t i = 0; i < sz; ++i) - x.values_[i] /= y.values_[i]; - } + private: + // Verifies that the underlying Vector is consistent with the collection of SubVectors + void chk() const; - // 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; - } + // Throw an exception if j does not exist + void checkExists(Index j) const { + chk(); + if(!exists(j)) + throw std::out_of_range("VectorValues: requested variable index is not in this VectorValues."); + } - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(values_); - ar & BOOST_SERIALIZATION_NVP(varStarts_); - } - }; - // \class VectorValues definition + // Resize + void copyStructureFrom(const VectorValues& other); -/// Implementations of functions + public: + /// TODO: linear algebra interface seems to have been added for SPCG. + friend size_t dim(const VectorValues& V) { + return V.dim(); + } + /// TODO: linear algebra interface seems to have been added for SPCG. + friend double dot(const VectorValues& V1, const VectorValues& V2) { + return gtsam::dot(V1.values_, V2.values_); + } + /// TODO: linear algebra interface seems to have been added for SPCG. + friend void scal(double alpha, VectorValues& x) { + gtsam::scal(alpha, x.values_); + } + /// 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; + } - template - inline VectorValues::VectorValues(const CONTAINER& dimensions) : - varStarts_(dimensions.size() + 1) { - varStarts_[0] = 0; - size_t varStart = 0; - Index var = 0; - BOOST_FOREACH(size_t dim, dimensions) - { - varStarts_[++var] = (varStart += dim); - } - values_.resize(varStarts_.back()); - } + /// TODO: linear algebra interface seems to have been added for SPCG. + friend void ediv(const VectorValues& numerator, + const VectorValues& denominator, VectorValues &result) { + assert( + numerator.dim() == denominator.dim() && denominator.dim() == result.dim()); + const size_t sz = result.dim(); + for (size_t i = 0; i < sz; ++i) + result.values_[i] = numerator.values_[i] / denominator.values_[i]; + } - template - inline Vector VectorValues::range(const ITERATOR& idx_begin, - const ITERATOR& idx_end) const { - // find the size of the vector to build - size_t s = 0; - for (ITERATOR it = idx_begin; it != idx_end; ++it) - s += dim(*it); + /// 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(); + for (size_t i = 0; i < sz; ++i) + x.values_[i] /= y.values_[i]; + } - // assign vector - Vector result(s); - size_t start = 0; - 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; - } + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void save(ARCHIVE & ar, const unsigned int version) const { + // The maps_ stores pointers, so we serialize dimensions instead + std::vector dimensions(size()); + for(size_t j=0; j + void load(ARCHIVE & ar, const unsigned int version) { + std::vector 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 - template - 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; - } - } + // Implementations of template and inline functions - struct DimSpec: public std::vector { + /* ************************************************************************* */ + template + void VectorValues::resize(const CONTAINER& dimensions) { + maps_.clear(); + values_.resize(0); + append(dimensions); + } - typedef std::vector Base; - typedef boost::shared_ptr shared_ptr; + /* ************************************************************************* */ + template + void VectorValues::append(const CONTAINER& dimensions) { + chk(); + int newDim = std::accumulate(dimensions.begin(), dimensions.end(), 0); // Sum of dimensions + values_.conservativeResize(dim() + newDim); + // Relocate existing maps + int varStart = 0; + for(size_t j = 0; j < maps_.size(); ++j) { + new (&maps_[j]) SubVector(values_.segment(varStart, maps_[j].rows())); + varStart += maps_[j].rows(); + } + maps_.reserve(maps_.size() + dimensions.size()); + BOOST_FOREACH(size_t dim, dimensions) { + maps_.push_back(values_.segment(varStart, dim)); + varStart += dim; // varStart is continued from first for loop + } + } - 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].size(); - } - } - }; + /* ************************************************************************* */ + template + VectorValues VectorValues::Zero(const CONTAINER& dimensions) { + VectorValues ret(dimensions); + ret.vector() = Vector::Zero(ret.dim()); + return ret; + } + + /* ************************************************************************* */ + inline void VectorValues::chk() const { +#ifndef NDEBUG + // Check that the first SubVector points to the beginning of the Vector + 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()); + } +#endif + } } // \namespace gtsam diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 82b6c2dac..613084120 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/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 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); - DOUBLES_EQUAL(2., actual[1][0], 1e-15); - DOUBLES_EQUAL(4., actual[2][0], 1e-15); + 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, standard) { - 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); +TEST(VectorValues, copyConstructor) { - vector dims(3); dims[0]=3; dims[1]=2; dims[2]=4; - VectorValues combined(dims); - EXPECT_LONGS_EQUAL(3, combined.size()); - EXPECT_LONGS_EQUAL(9, combined.dim()); - EXPECT_LONGS_EQUAL(9, combined.dimCapacity()); - EXPECT_LONGS_EQUAL(3, combined.dim(0)); - EXPECT_LONGS_EQUAL(2, combined.dim(1)); - EXPECT_LONGS_EQUAL(4, combined.dim(2)); - combined[0] = v1; - combined[1] = v2; - combined[2] = v3; + // 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)); - CHECK(assert_equal(combined[0], v1)) - CHECK(assert_equal(combined[1], v2)) - CHECK(assert_equal(combined[2], v3)) + VectorValues actual(original); - VectorValues incremental; - incremental.reserve(3, 9); - incremental.push_back_preallocated(v1); - incremental.push_back_preallocated(v2); - incremental.push_back_preallocated(v3); + // 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)); - CHECK(assert_equal(incremental[0], v1)) - CHECK(assert_equal(incremental[1], v2)) - CHECK(assert_equal(incremental[2], v3)) + // 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, 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 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) { - 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); - - VectorValues incremental; - incremental.reserve(3, 9); - incremental.push_back_preallocated(v1); - incremental.push_back_preallocated(v2); - incremental.push_back_preallocated(v3); - - Permutation perm1(3); - perm1[0] = 1; - perm1[1] = 2; - perm1[2] = 0; - - Permutation perm2(3); - perm2[0] = 1; - perm2[1] = 2; - perm2[2] = 0; - - Permuted permuted1(incremental); - CHECK(assert_equal(v1, permuted1[0])) - CHECK(assert_equal(v2, permuted1[1])) - CHECK(assert_equal(v3, permuted1[2])) - - permuted1.permute(perm1); - CHECK(assert_equal(v1, permuted1[2])) - CHECK(assert_equal(v2, permuted1[0])) - CHECK(assert_equal(v3, permuted1[1])) - - permuted1.permute(perm2); - CHECK(assert_equal(v1, permuted1[1])) - CHECK(assert_equal(v2, permuted1[2])) - CHECK(assert_equal(v3, permuted1[0])) - - Permuted permuted2(perm1, incremental); - CHECK(assert_equal(v1, permuted2[2])) - CHECK(assert_equal(v2, permuted2[0])) - CHECK(assert_equal(v3, permuted2[1])) - - permuted2.permute(perm2); - CHECK(assert_equal(v1, permuted2[1])) - CHECK(assert_equal(v2, permuted2[2])) - 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 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 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 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)); -} +///* ************************************************************************* */ +//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() { diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index ada2f82f6..e4476b260 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -62,7 +62,7 @@ void optimize2(const BayesTree::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() >= threshold) { valuesChanged = true; break; diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 64f593cd8..d94865cf2 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -125,14 +125,15 @@ struct ISAM2::Impl { struct _VariableAdder { Ordering& ordering; Permuted& vconfig; - _VariableAdder(Ordering& _ordering, Permuted& _vconfig) : ordering(_ordering), vconfig(_vconfig) {} + Index nextVar; + _VariableAdder(Ordering& _ordering, Permuted& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){} template void operator()(I xIt) { const bool debug = ISDEBUG("ISAM2 AddVariables"); - Index var = vconfig->push_back_preallocated(zero(xIt->second.dim())); - vconfig.permutation()[var] = var; - ordering.insert(xIt->first, var); - if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl; + vconfig.permutation()[nextVar] = nextVar; + ordering.insert(xIt->first, nextVar); + if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl; + ++ nextVar; } }; @@ -145,15 +146,18 @@ void ISAM2::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 dims(newTheta.dims(*newTheta.orderingArbitrary(ordering.nVars()))); + vector 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)); - delta.permutation().resize(delta->size() + newTheta.size()); + const size_t newDim = accumulate(dims.begin(), dims.end(), 0); + 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()); } diff --git a/gtsam/nonlinear/LieValues-inl.h b/gtsam/nonlinear/LieValues-inl.h index 8a44d5488..2e7f2b3f6 100644 --- a/gtsam/nonlinear/LieValues-inl.h +++ b/gtsam/nonlinear/LieValues-inl.h @@ -77,9 +77,7 @@ namespace gtsam { /* ************************************************************************* */ template VectorValues LieValues::zero(const Ordering& ordering) const { - VectorValues z(this->dims(ordering)); - z.makeZero(); - return z; + return VectorValues::Zero(this->dims(ordering)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index fe753f835..977f361d3 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/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)); - z.makeZero(); - return z; + return VectorValues::Zero(this->dims(ordering)); } /** @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)); - z.makeZero(); - return z; + return VectorValues::Zero(this->dims(ordering)); } size_t size() const { return first_.size(); } diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index a2e7c2f90..f938d055f 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -352,16 +352,14 @@ TEST( Pose2Prior, error ) boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); // Check error at x0, i.e. delta = zero ! - VectorValues delta(x0.dims(ordering)); - delta.makeZero(); + VectorValues delta(VectorValues::Zero(x0.dims(ordering))); 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)); - addition.makeZero(); + VectorValues addition(VectorValues::Zero(x0.dims(ordering))); addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); VectorValues plus = delta + addition; Pose2Values x1 = x0.expmap(plus, ordering); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 58dc9df30..0efd2c5f5 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/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)); } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index a8c2f0101..3249cfac3 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -70,8 +70,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) EXPECT(assert_equal(expected, actual)); // obtain solution - VectorValues e(vector(7,2)); // expected solution - e.makeZero(); + VectorValues e(VectorValues::Zero(vector(7,2))); // expected solution 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 chordalBayesNet = *GaussianMultifrontalSolver(smoother).eliminate(); - VectorValues expectedSolution(7, 2); - expectedSolution.makeZero(); + VectorValues expectedSolution(VectorValues::Zero(vector(7,2))); VectorValues actualSolution = optimize(chordalBayesNet); EXPECT(assert_equal(expectedSolution,actualSolution,tol)); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 975b94a08..1940a151b 100644 --- a/tests/testGaussianISAM2.cpp +++ b/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); - { Vector a(3); a << .1, .2, .3; deltaUnpermuted.push_back_preallocated(a); } - { Vector a(2); a << .4, .5; deltaUnpermuted.push_back_preallocated(a); } + deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3)); + deltaUnpermuted.insert(1, Vector_(2, .4, .5)); 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); - { Vector a(3); a << .1, .2, .3; deltaUnpermutedExpected.push_back_preallocated(a); } - { Vector a(2); a << .4, .5; deltaUnpermutedExpected.push_back_preallocated(a); } - { Vector a(3); a << 0, 0, 0; deltaUnpermutedExpected.push_back_preallocated(a); } + deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5)); + deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); Permutation permutationExpected(3); permutationExpected[0] = 1; diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 5a22b3f4f..1bc251f99 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -314,12 +314,10 @@ TEST (Serialization, SharedDiagonal_noiseModels) { /* ************************************************************************* */ TEST (Serialization, linear_factors) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - double v[] = {1., 2., 3., 4., 5.}; - VectorValues values(dims, v); + VectorValues values; + values.insert(0, Vector_(1, 1.0)); + values.insert(1, Vector_(2, 2.0,3.0)); + values.insert(2, Vector_(2, 4.0,5.0)); EXPECT(equalsObj(values)); EXPECT(equalsXML(values));