From b9a6004a5ad44f82c192e71d66f2a19b3034ea67 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 4 Sep 2011 01:05:09 +0000 Subject: [PATCH] Decoumentation and re-arranging --- gtsam/linear/VectorValues.cpp | 105 +++---- gtsam/linear/VectorValues.h | 512 +++++++++++++++++++++------------- 2 files changed, 364 insertions(+), 253 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 32bb88298..b72eaf1ae 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -22,35 +22,36 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -VectorValues::VectorValues(Index nVars, size_t varDim) : varStarts_(nVars+1) { +VectorValues::VectorValues(Index nVars, size_t varDim) : + varStarts_(nVars + 1) { varStarts_[0] = 0; size_t varStart = 0; - for(Index j=1; j<=nVars; ++j) + for (Index j = 1; j <= nVars; ++j) varStarts_[j] = (varStart += varDim); values_.resize(varStarts_.back()); } /* ************************************************************************* */ -VectorValues::VectorValues(const std::vector& dimensions, const Vector& values) : - values_(values), varStarts_(dimensions.size()+1) { +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); - } + BOOST_FOREACH(size_t dim, dimensions) + varStarts_[++var] = (varStart += dim); assert(varStarts_.back() == (size_t) values.size()); } /* ************************************************************************* */ -VectorValues::VectorValues(const std::vector& dimensions, const double* values) : - varStarts_(dimensions.size()+1) { +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); - } + BOOST_FOREACH(size_t dim, dimensions) + varStarts_[++var] = (varStart += dim); values_ = Vector_(varStart, values); } @@ -62,55 +63,39 @@ VectorValues VectorValues::SameStructure(const VectorValues& otherValues) { return ret; } -/* ************************************************************************* */ -VectorValues::mapped_type VectorValues::operator[](Index variable) { - checkVariable(variable); - const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start; - return values_.segment(start, n); -} - -/* ************************************************************************* */ -VectorValues::const_mapped_type VectorValues::operator[](Index variable) const { - checkVariable(variable); - const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start; - return values_.segment(start, n); -} - -/* ************************************************************************* */ -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; -} - /* ************************************************************************* */ void VectorValues::print(const std::string& str) const { - std::cout << str << ": " << varStarts_.size()-1 << " elements\n"; - for(Index var=0; varvalues_ += c.values_.head(varStarts_.back()); +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); +} + +/* ************************************************************************* */ +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); } /* ************************************************************************* */ @@ -121,12 +106,28 @@ VectorValues VectorValues::zero(const VectorValues& x) { } /* ************************************************************************* */ -size_t VectorValues::dim(Index variable) const { - checkVariable(variable); - const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start; - return n; +Index VectorValues::push_back_preallocated(const Vector& vector) { + Index var = varStarts_.size() - 1; + varStarts_.push_back(varStarts_.back() + vector.size()); + this->operator[](var) = vector; // This will assert that values_ has enough allocated space. + return var; +} + +/* ************************************************************************* */ +VectorValues VectorValues::operator+(const VectorValues& c) const { + assert(varStarts_ == c.varStarts_); + VectorValues result; + result.varStarts_ = varStarts_; + result.values_ = values_.head(varStarts_.back()) + + c.values_.head(varStarts_.back()); + return result; +} + +/* ************************************************************************* */ +void VectorValues::operator+=(const VectorValues& c) { + assert(varStarts_ == c.varStarts_); + this->values_ += c.values_.head(varStarts_.back()); } /* ************************************************************************* */ - diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 614389132..a0dba25d6 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -26,251 +26,361 @@ namespace gtsam { -class VectorValues : public Testable { -protected: - Vector values_; - std::vector varStarts_; // start at 0 with size nVars + 1 + /** + * 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: public Testable { + protected: + Vector values_; + std::vector varStarts_; // start at 0 with size nVars + 1 -public: - template class _impl_iterator; // Forward declaration of iterator implementation - typedef boost::shared_ptr shared_ptr; - typedef _impl_iterator iterator; - typedef _impl_iterator const_iterator; - typedef SubVector value_reference_type; - typedef ConstSubVector const_value_reference_type; - typedef SubVector mapped_type; - typedef ConstSubVector const_mapped_type; + public: + // Forward declaration of iterator implementation + template class _impl_iterator; + typedef _impl_iterator iterator; + typedef _impl_iterator const_iterator; - /** - * 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) {} - VectorValues(const VectorValues &V) : values_(V.values_), varStarts_(V.varStarts_) {} + // 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; - /** Construct from a container of variable dimensions (in variable order). */ - template - VectorValues(const CONTAINER& dimensions); + /** Constructors that simply reserve space */ - /** Construct to hold nVars vectors of varDim dimension each. */ - VectorValues(Index nVars, size_t varDim); + /** + * 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) { + } - /** 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); + /** Construct from a container of variable dimensions (in variable order). */ + template + VectorValues(const CONTAINER& dimensions); - /** Construct forom the variable dimensions in varaible order and a double array that contains actual values */ - VectorValues(const std::vector& dimensions, const double* values); + /** Construct to hold nVars vectors of varDim dimension each. */ + VectorValues(Index nVars, size_t 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& otherValues); + /** Constructors that actually initialize values */ - /** Element access */ - mapped_type operator[](Index variable); - const_mapped_type operator[](Index variable) const; + /** 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); - /** dimension of a particular element */ - size_t dim(Index variable) const; + /** 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 elements */ - Index size() const { return varStarts_.size()-1; } + /** Copy constructor */ + VectorValues(const VectorValues &V) : + values_(V.values_), varStarts_(V.varStarts_) { + } - /** Total dimensionality used (could be smaller than what has been allocated - * with reserve(...) ). - */ - size_t dim() const { return varStarts_.back(); } + /** 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); - /* dot product */ - double dot(const VectorValues& V) const { return gtsam::dot(this->values_, V.values_) ; } + /** Reserve space for a total number of variables and dimensionality */ + void reserve(Index nVars, size_t totalDims) { + values_.conservativeResize(totalDims); + varStarts_.reserve(nVars + 1); + } - /** 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; - /** 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); } + /** equals required by Testable for unit testing */ + bool equals(const VectorValues& x, double tol = 1e-9) const; - /** Reference the entire solution vector (const version). */ - const Vector& vector() const { return values_; } + /** Number of elements */ + Index size() const { + return varStarts_.size() - 1; + } - /** Reference the entire solution vector. */ - Vector& vector() { return values_; } + /** dimension of a particular element */ + size_t dim(Index j) const; - /** Reserve space for a total number of variables and dimensionality */ - void reserve(Index nVars, size_t totalDims) { values_.conservativeResize(totalDims); varStarts_.reserve(nVars+1); } + /** Total dimensionality used (could be smaller than what has been allocated + * with reserve(...) ). + */ + size_t dim() const { + return varStarts_.back(); + } - /** 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; + /** Total dimensions capacity allocated */ + size_t dimCapacity() const { + return values_.size(); + } - /** 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); + /** Reference the entire solution vector (const version). */ + const Vector& vector() const { + return values_; + } - /** - * 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); + /** Reference the entire solution vector. */ + Vector& vector() { + return values_; + } - /** Set all elements to zero */ - void makeZero() { values_.setZero(); } + /** Individual element access */ + mapped_type operator[](Index j); + const_mapped_type operator[](Index j) const; - /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ") const; + /** 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); + } - /** equals required by Testable for unit testing */ - bool equals(const VectorValues& x, double tol=1e-9) const; + /** 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; - /** - * + operator simply adds Vectors. This checks for structural equivalence - * when NDEBUG is not defined. - */ - VectorValues operator+(const VectorValues& c) const; + /** 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); - void operator+=(const VectorValues& c); + /** Set all elements to zero */ + void makeZero() { + values_.setZero(); + } - /** - * Iterator (handles both iterator and const_iterator depending on whether - * the template type is const. - */ - template - class _impl_iterator { - protected: - C& config_; - Index curVariable_; + /** Copy structure of x, but set all values to zero */ + static VectorValues zero(const VectorValues& x); - _impl_iterator(C& config, Index curVariable) : config_(config), curVariable_(curVariable) {} - void checkCompat(const _impl_iterator& r) { assert(&config_ == &r.config_); } - friend class VectorValues; + /** + * 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); - 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 */ + double dot(const VectorValues& V) const { + return gtsam::dot(this->values_, V.values_); + } - /** Copy structure of x, but set all values to zero */ - static VectorValues zero(const VectorValues& x); + /** + * + operator simply adds Vectors. This checks for structural equivalence + * when NDEBUG is not defined. + */ + VectorValues operator+(const VectorValues& c) const; -protected: - void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); } + void operator+=(const VectorValues& c); + /** + * Iterator (handles both iterator and const_iterator depending on whether + * the template type is const. + */ + template + class _impl_iterator { + protected: + C& config_; + Index curVariable_; -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; } + _impl_iterator(C& config, Index curVariable) : + config_(config), curVariable_(curVariable) { + } + void checkCompat(const _impl_iterator& r) { + assert(&config_ == &r.config_); + } + friend class VectorValues; - 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] ; - } + 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_]; + } + }; - 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] ; - } + protected: + void checkVariable(Index j) const { + assert(j < varStarts_.size()-1); + } - // 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; - } + 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; + } -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 + 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]; + } + + // check whether there's a zero in the vector + friend bool anyZero(const VectorValues& x, double tol = 1e-5) { + bool flag = false; + size_t i = 0; + for (const double *v = x.values_.data(); i < (size_t) x.values_.size(); + ++v) { + if (*v < tol && *v > -tol) { + flag = true; + break; + } + ++i; + } + return flag; + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(values_); + ar & BOOST_SERIALIZATION_NVP(varStarts_); + } + }; + // \class VectorValues definition /// Implementations of functions -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()); -} - -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); - - // 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; + 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()); } - return result; -} -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; + 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); + + // 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; } -} -struct DimSpec: public std::vector { + 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; + } + } - typedef std::vector Base ; - typedef boost::shared_ptr shared_ptr; + struct DimSpec: public std::vector { - 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() ; - } - } -}; + 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].size(); + } + } + }; } // \namespace gtsam