diff --git a/gtsam/linear/VectorValuesUnordered.cpp b/gtsam/linear/VectorValuesUnordered.cpp index 9fdf19182..ff796213f 100644 --- a/gtsam/linear/VectorValuesUnordered.cpp +++ b/gtsam/linear/VectorValuesUnordered.cpp @@ -16,51 +16,22 @@ * @author Alex Cunningham */ -#include #include -#include +#include +#include +#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -VectorValuesUnordered VectorValuesUnordered::Zero(const VectorValuesUnordered& x) { - VectorValuesUnordered result; - result.values_.resize(x.size()); - for(size_t j=0; j VectorValuesUnordered::dims() const { - std::vector result(this->size()); - for(Index j = 0; j < this->size(); ++j) - result[j] = this->dim(j); - return result; -} - -/* ************************************************************************* */ -void VectorValuesUnordered::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."); - - // If this adds variables at the end, insert zero-length entries up to j - if(j >= size()) - values_.resize(j+1); - - // Assign value - values_[j] = value; -} - -/* ************************************************************************* */ -void VectorValuesUnordered::print(const std::string& str, const IndexFormatter& formatter) const { +void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const { std::cout << str << ": " << size() << " elements\n"; - for (Index var = 0; var < size(); ++var) - std::cout << " " << formatter(var) << ": \n" << (*this)[var] << "\n"; + BOOST_FOREACH(const value_type& key_value, *this) + std::cout << " " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n"; std::cout.flush(); } @@ -68,85 +39,77 @@ void VectorValuesUnordered::print(const std::string& str, const IndexFormatter& bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const { if(this->size() != x.size()) return false; - for(Index j=0; j < size(); ++j) - if(!equal_with_abs_tol(values_[j], x.values_[j], tol)) + typedef boost::tuple ValuePair; + BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { + if(values.get<0>.first != values.get<1>.first || + !equal_with_abs_tol(values.get<0>.second, values.get<1>.second, tol)) return false; - return true; -} - -/* ************************************************************************* */ -void VectorValuesUnordered::resize(Index nVars, size_t varDim) { - values_.resize(nVars); - for(Index j = 0; j < nVars; ++j) - values_[j] = Vector(varDim); -} - -/* ************************************************************************* */ -void VectorValuesUnordered::resizeLike(const VectorValuesUnordered& other) { - values_.resize(other.size()); - for(Index j = 0; j < other.size(); ++j) - values_[j].resize(other.values_[j].size()); -} - -/* ************************************************************************* */ -VectorValuesUnordered VectorValuesUnordered::SameStructure(const VectorValuesUnordered& other) { - VectorValuesUnordered ret; - ret.resizeLike(other); - return ret; -} - -/* ************************************************************************* */ -VectorValuesUnordered VectorValuesUnordered::Zero(Index nVars, size_t varDim) { - VectorValuesUnordered ret(nVars, varDim); - ret.setZero(); - return ret; -} - -/* ************************************************************************* */ -void VectorValuesUnordered::setZero() { - BOOST_FOREACH(Vector& v, *this) { - v.setZero(); } -} - -/* ************************************************************************* */ -const Vector VectorValuesUnordered::asVector() const { - return internal::extractVectorValuesSlices(*this, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true); -} - -/* ************************************************************************* */ -const Vector VectorValuesUnordered::vector(const std::vector& indices) const { - return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end()); -} - -/* ************************************************************************* */ -bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered& other) const { - if(this->size() != other.size()) - return false; - for(size_t j = 0; j < size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].rows() != other.values_[j].rows()) - return false; return true; } +/* ************************************************************************* */ +const Vector VectorValuesUnordered::asVector() const +{ + using boost::adaptors::map_values; + using boost::adaptors::transformed; + + // Count dimensions + const DenseIndex totalDim = boost::accumulate(*this | map_values | transformed(&Vector::size), 0); + + // Copy vectors + Vector result; + DenseIndex pos = 0; + BOOST_FOREACH(const Vector& v, *this | map_values) { + result.segment(pos, v.size()) = v; + pos += v.size(); + } + + return result; +} + +/* ************************************************************************* */ +const Vector VectorValuesUnordered::vector(const std::vector& keys) const +{ + // Count dimensions and collect pointers to avoid double lookups + DenseIndex totalDim = 0; + std::vector items(keys.size()); + for(size_t i = 0; i < keys.size(); ++i) { + items[i] = &at(i); + totalDim += items[i]->size(); + } + + // Copy vectors + Vector result(totalDim); + DenseIndex pos = 0; + BOOST_FOREACH(const Vector *v, items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } + + return result; +} + /* ************************************************************************* */ void VectorValuesUnordered::swap(VectorValuesUnordered& other) { this->values_.swap(other.values_); } /* ************************************************************************* */ -double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const { - double result = 0.0; +double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const +{ if(this->size() != v.size()) - throw invalid_argument("VectorValues::dot called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == v.values_[j].size()) - result += this->values_[j].dot(v.values_[j]); - else - throw invalid_argument("VectorValues::dot called with different vector sizes"); + throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); + double result = 0.0; + typedef boost::tuple ValuePair; + using boost::adaptors::map_values; + BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { + assert_throw(values.get<0>().first == values.get<1>.first, + std::invalid_argument("VectorValues::dot called with a VectorValues of different structure")); + assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), + std::invalid_argument("VectorValues::dot called with a VectorValues of different structure")); + result += values.get<0>().second.dot(values.get<1>().second); + } return result; } @@ -158,9 +121,9 @@ double VectorValuesUnordered::norm() const { /* ************************************************************************* */ double VectorValuesUnordered::squaredNorm() const { double sumSquares = 0.0; - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - sumSquares += this->values_[j].squaredNorm(); + using boost::adaptors::map_values; + BOOST_FOREACH(const Vector& v, *this | map_values) + sumSquares += v.squaredNorm(); return sumSquares; } diff --git a/gtsam/linear/VectorValuesUnordered.h b/gtsam/linear/VectorValuesUnordered.h index 0072dec51..5eff65df2 100644 --- a/gtsam/linear/VectorValuesUnordered.h +++ b/gtsam/linear/VectorValuesUnordered.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include @@ -39,9 +40,9 @@ namespace gtsam { * 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 + * - insert(Key, const Vector&) to add vector variables + * - operator[](Key) for read and write access to stored variables + * - \ref exists (Key) 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 @@ -75,7 +76,7 @@ namespace gtsam { * - 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 + * insert(Key, 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. @@ -90,7 +91,7 @@ namespace gtsam { */ class GTSAM_EXPORT VectorValuesUnordered { protected: - typedef std::vector Values; ///< Typedef for the collection of Vectors making up a VectorValues + typedef FastMap Values; ///< Typedef for the collection of Vectors making up a VectorValues Values values_; ///< Collection of Vectors making up this VectorValues public: @@ -99,6 +100,7 @@ namespace gtsam { typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair /// @name Standard Constructors /// @{ @@ -108,48 +110,54 @@ namespace gtsam { */ VectorValuesUnordered() {} - /** Named constructor to create a VectorValues of the same structure of the - * specified one, but filled with zeros. - * @return - */ - static VectorValuesUnordered Zero(const VectorValuesUnordered& model); - /// @} /// @name Standard Interface /// @{ - /** Number of variables stored, always 1 more than the highest variable index, - * even if some variables with lower indices are not present. */ - Index size() const { return values_.size(); } + /** Number of variables stored. */ + Key size() const { return values_.size(); } /** Return the dimension of variable \c j. */ - size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } + size_t dim(Key j) const { return at(j).rows(); } - /** Return the dimension of each vector in this container */ - std::vector dims() const; + /** Check whether a variable with key \c j exists. */ + bool exists(Key j) const { return find(j) != end(); } - /** Check whether a variable with index \c j exists. */ - bool exists(Index j) const { return j < size() && values_[j].rows() > 0; } + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + Vector& at(Key j) { + iterator item = find(j); + if(item == end()) + throw std::out_of_range( + "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); + else + return item->second; + } - /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ - Vector& at(Index j) { checkExists(j); return values_[j]; } + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + const Vector& at(Key j) const { + const_iterator item = find(j); + if(item == end()) + throw std::out_of_range( + "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); + else + return item->second; + } - /** 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 Vector& at(Index j) const { checkExists(j); return values_[j]; } + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to at(Key). */ + Vector& operator[](Key j) { return at(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). */ - Vector& operator[](Index j) { return at(j); } + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Key). */ + const Vector& operator[](Key j) const { 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 Vector& operator[](Index j) const { return at(j); } - - /** Insert a vector \c value with index \c j. - * Causes reallocation, but can insert values in any order. - * Throws an invalid_argument exception if the index \c j is already used. + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c j is already used. * @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(Key j, const Vector& value) { + if(!values_.insert(std::make_pair(j, value)).second) + throw std::invalid_argument( + "Requested to insert variable '" + DefaultKeyFormatter(j) + "' already in this VectorValues."); + } iterator begin() { return values_.begin(); } ///< Iterator over variables const_iterator begin() const { return values_.begin(); } ///< Iterator over variables @@ -160,99 +168,28 @@ namespace gtsam { reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + iterator find(Key j) { return values_.find(j); } + + /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + const_iterator find(Key j) const { return values_.find(j); } + /** print required by Testable for unit testing */ void print(const std::string& str = "VectorValues: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; /** equals required by Testable for unit testing */ bool equals(const VectorValuesUnordered& x, double tol = 1e-9) const; /// @{ - /// \anchor AdvancedConstructors - /// @name Advanced Constructors - /// @} - - /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ - template - explicit VectorValuesUnordered(const CONTAINER& dimensions) { this->append(dimensions); } - - /** Construct to hold nVars vectors of varDim dimension each. */ - VectorValuesUnordered(Index nVars, size_t varDim) { this->resize(nVars, varDim); } - - /** Named constructor to create a VectorValues that matches the structure of - * the specified VectorValues, but do not initialize the new values. */ - static VectorValuesUnordered SameStructure(const VectorValuesUnordered& other); - - /** Named constructor to create a VectorValues from a container of variable - * dimensions that is filled with zeros. - * @param dimensions A container of the dimension of each variable to create. - */ - template - static VectorValuesUnordered 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 VectorValuesUnordered Zero(Index nVars, size_t varDim); - - /// @} /// @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 VectorValuesUnordered& other); - - /** Resize the VectorValues to hold \c nVars variables, each of dimension - * \c varDim. Any individual vectors that do not change size will keep - * their values, but any new or resized vectors will be uninitialized. - * @param nVars The number of variables to create - * @param varDim The dimension of each variable - */ - void resize(Index nVars, size_t varDim); - - /** Resize the VectorValues to contain variables of the dimensions stored - * in \c dimensions. Any individual vectors that do not change size will keep - * their values, but any new or resized vectors will be uninitialized. - * @param dimensions A container of the dimension of each variable to create. - */ - template - void resize(const CONTAINER& dimensions); - - /** Append to the VectorValues to additionally contain variables of the - * dimensions stored in \c dimensions. The new variables are uninitialized, - * but this function is used to pre-allocate space for performance. This - * function preserves the original data, so all previously-existing variables - * are left unchanged. - * @param dimensions A container of the dimension of each variable to create. - */ - template - void append(const CONTAINER& dimensions); - - /** Removes the last subvector from the VectorValues */ - void pop_back() { values_.pop_back(); }; - - /** Set all entries to zero, does not modify the size. */ - void setZero(); - /** Retrieve the entire solution as a single vector */ const Vector asVector() const; - /** Access a vector that is a subset of relevant indices */ - const Vector vector(const std::vector& indices) const; - - /** Check whether this VectorValues has the same structure, meaning has the - * same number of variables and that all variables are of the same dimension, - * 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 VectorValuesUnordered& other) const; + /** Access a vector that is a subset of relevant keys. */ + const Vector vector(const std::vector& keys) const; /** * Swap the data in this VectorValues with another. @@ -264,7 +201,8 @@ namespace gtsam { /// @{ /** Dot product with another VectorValues, interpreting both as vectors of - * their concatenated values. */ + * their concatenated values. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ double dot(const VectorValuesUnordered& v) const; /** Vector L2 norm */ @@ -302,38 +240,26 @@ namespace gtsam { /// @} - private: - // Throw an exception if j does not exist - void checkExists(Index j) const { - if(!exists(j)) { - const std::string msg = - (boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str(); - throw std::out_of_range(msg); - } - } - - public: - /** * scale a vector by a scalar */ friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) { VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v); - for(Index j = 0; j < v.size(); ++j) + for(Key j = 0; j < v.size(); ++j) result.values_[j] = a * v.values_[j]; return result; } /// TODO: linear algebra interface seems to have been added for SPCG. friend void scal(double alpha, VectorValuesUnordered& x) { - for(Index j = 0; j < x.size(); ++j) + for(Key j = 0; j < x.size(); ++j) x.values_[j] *= alpha; } /// TODO: linear algebra interface seems to have been added for SPCG. friend void axpy(double alpha, const VectorValuesUnordered& x, VectorValuesUnordered& y) { if(x.size() != y.size()) throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - for(Index j = 0; j < x.size(); ++j) + for(Key j = 0; j < x.size(); ++j) if(x.values_[j].size() == y.values_[j].size()) y.values_[j] += alpha * x.values_[j]; else @@ -341,7 +267,7 @@ namespace gtsam { } /// TODO: linear algebra interface seems to have been added for SPCG. friend void sqrt(VectorValuesUnordered &x) { - for(Index j = 0; j < x.size(); ++j) + for(Key j = 0; j < x.size(); ++j) x.values_[j] = x.values_[j].cwiseSqrt(); } @@ -349,7 +275,7 @@ namespace gtsam { friend void ediv(const VectorValuesUnordered& numerator, const VectorValuesUnordered& denominator, VectorValuesUnordered &result) { if(numerator.size() != denominator.size() || numerator.size() != result.size()) throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - for(Index j = 0; j < numerator.size(); ++j) + for(Key j = 0; j < numerator.size(); ++j) if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); else @@ -360,7 +286,7 @@ namespace gtsam { friend void edivInPlace(VectorValuesUnordered& x, const VectorValuesUnordered& y) { if(x.size() != y.size()) throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - for(Index j = 0; j < x.size(); ++j) + for(Key j = 0; j < x.size(); ++j) if(x.values_[j].size() == y.values_[j].size()) x.values_[j].array() /= y.values_[j].array(); else @@ -376,80 +302,4 @@ namespace gtsam { } }; // VectorValues definition - // Implementations of template and inline functions - - /* ************************************************************************* */ - template - void VectorValuesUnordered::resize(const CONTAINER& dimensions) { - values_.clear(); - append(dimensions); - } - - /* ************************************************************************* */ - template - void VectorValuesUnordered::append(const CONTAINER& dimensions) { - size_t i = size(); - values_.resize(size() + dimensions.size()); - BOOST_FOREACH(size_t dim, dimensions) { - values_[i] = Vector(dim); - ++ i; - } - } - - /* ************************************************************************* */ - template - VectorValuesUnordered VectorValuesUnordered::Zero(const CONTAINER& dimensions) { - VectorValuesUnordered ret; - ret.values_.resize(dimensions.size()); - size_t i = 0; - BOOST_FOREACH(size_t dim, dimensions) { - ret.values_[i] = Vector::Zero(dim); - ++ i; - } - return ret; - } - - namespace internal { - /* ************************************************************************* */ - // Helper function, extracts vectors with variable indices - // in the first and last iterators, and concatenates them in that order into the - // output. - template - const Vector extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) { - // Find total dimensionality - size_t dim = 0; - for(ITERATOR j = first; j != last; ++j) - // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) - if(!allowNonexistant || values.exists(*j)) - dim += values.dim(*j); - - // Copy vectors - Vector ret(dim); - size_t varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) - if(!allowNonexistant || values.exists(*j)) { - ret.segment(varStart, values.dim(*j)) = values[*j]; - varStart += values.dim(*j); - } - } - return ret; - } - - /* ************************************************************************* */ - // Helper function, writes to the variables in values - // with indices iterated over by first and last, interpreting vector as the - // concatenated vectors to write. - template - void writeVectorValuesSlices(const VECTOR& vector, VectorValuesUnordered& values, ITERATOR first, ITERATOR last) { - // Copy vectors - size_t varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - values[*j] = vector.segment(varStart, values[*j].rows()); - varStart += values[*j].rows(); - } - assert(varStart == vector.rows()); - } - } - } // \namespace gtsam