From 9f444923808efcba41e2c2e9b587c3e0e1f7e2e2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 12 Jul 2013 22:27:46 +0000 Subject: [PATCH] Cleanups and standardizing linear algebra functions in VectorValuesUnordered --- gtsam/linear/VectorValuesUnordered.cpp | 91 +++++++++++++++++--------- gtsam/linear/VectorValuesUnordered.h | 53 ++++++++------- 2 files changed, 91 insertions(+), 53 deletions(-) diff --git a/gtsam/linear/VectorValuesUnordered.cpp b/gtsam/linear/VectorValuesUnordered.cpp index 90e356e5d..ea805ebe5 100644 --- a/gtsam/linear/VectorValuesUnordered.cpp +++ b/gtsam/linear/VectorValuesUnordered.cpp @@ -34,20 +34,6 @@ namespace gtsam { using boost::adaptors::map_values; using boost::accumulate; - /* ************************************************************************* */ - namespace internal - { - bool structureCompareOp(const boost::tuple& vv) - { - return vv.get<0>().first == vv.get<1>().first && vv.get<0>().second.size() == vv.get<1>().second.size(); - } - - bool hasSameStructure(const VectorValuesUnordered& vv1, const VectorValuesUnordered& vv2) - { - return accumulate(combine(vv1, vv2) | transformed(structureCompareOp), true, std::logical_and()); - } - } - /* ************************************************************************* */ VectorValuesUnordered::VectorValuesUnordered(const VectorValuesUnordered& first, const VectorValuesUnordered& second) { @@ -98,7 +84,7 @@ namespace gtsam { } /* ************************************************************************* */ - const Vector VectorValuesUnordered::asVector() const + const Vector VectorValuesUnordered::vector() const { // Count dimensions DenseIndex totalDim = 0; @@ -143,6 +129,24 @@ namespace gtsam { this->values_.swap(other.values_); } + /* ************************************************************************* */ + namespace internal + { + bool structureCompareOp(const boost::tuple& vv) + { + return vv.get<0>().first == vv.get<1>().first + && vv.get<0>().second.size() == vv.get<1>().second.size(); + } + } + + /* ************************************************************************* */ + bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered other) const + { + return accumulate(combine(*this, other) + | transformed(internal::structureCompareOp), true, std::logical_and()); + } + /* ************************************************************************* */ double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const { @@ -180,7 +184,7 @@ namespace gtsam { { if(this->size() != c.size()) throw invalid_argument("VectorValues::operator+ called with different vector sizes"); - assert_throw(internal::hasSameStructure(*this, c), + assert_throw(hasSameStructure(c), invalid_argument("VectorValues::operator+ called with different vector sizes")); VectorValuesUnordered result; @@ -191,12 +195,41 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + VectorValuesUnordered VectorValuesUnordered::add(const VectorValuesUnordered& c) const + { + return *this + c; + } + + /* ************************************************************************* */ + VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+= called with different vector sizes"); + assert_throw(hasSameStructure(c), + invalid_argument("VectorValues::operator+= called with different vector sizes")); + + iterator j1 = begin(); + const_iterator j2 = c.begin(); + // The result.end() hint here should result in constant-time inserts + for(; j1 != end(); ++j1, ++j2) + j1->second += j2->second; + + return *this; + } + + /* ************************************************************************* */ + VectorValuesUnordered& VectorValuesUnordered::addInPlace(const VectorValuesUnordered& c) + { + return *this += c; + } + /* ************************************************************************* */ VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const { if(this->size() != c.size()) throw invalid_argument("VectorValues::operator- called with different vector sizes"); - assert_throw(internal::hasSameStructure(*this, c), + assert_throw(hasSameStructure(c), invalid_argument("VectorValues::operator- called with different vector sizes")); VectorValuesUnordered result; @@ -208,27 +241,23 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) + VectorValuesUnordered VectorValuesUnordered::subtract(const VectorValuesUnordered& c) const { - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+= called with different vector sizes"); - assert_throw(internal::hasSameStructure(*this, c), - invalid_argument("VectorValues::operator+= called with different vector sizes")); - - iterator j1 = begin(); - const_iterator j2 = begin(); - // The result.end() hint here should result in constant-time inserts - for(; j1 != end(); ++j1, ++j2) - j1->second += j2->second; + return *this - c; + } + /* ************************************************************************* */ + VectorValuesUnordered& VectorValuesUnordered::operator*=(double alpha) + { + BOOST_FOREACH(Vector& v, *this | map_values) + v *= alpha; return *this; } /* ************************************************************************* */ - void scal(double alpha, VectorValuesUnordered& x) + VectorValuesUnordered& VectorValuesUnordered::scaleInPlace(double alpha) { - BOOST_FOREACH(Vector& v, x | map_values) - v *= alpha; + return *this *= alpha; } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValuesUnordered.h b/gtsam/linear/VectorValuesUnordered.h index 0834a4d69..60ca7bf5f 100644 --- a/gtsam/linear/VectorValuesUnordered.h +++ b/gtsam/linear/VectorValuesUnordered.h @@ -95,7 +95,7 @@ namespace gtsam { typedef Values::const_iterator const_iterator; ///< Const iterator over vector values 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 boost::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair @@ -193,16 +193,17 @@ namespace gtsam { /// @{ /** Retrieve the entire solution as a single vector */ - const Vector asVector() const; + const Vector vector() 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. - */ + /** Swap the data in this VectorValues with another. */ void swap(VectorValuesUnordered& other); + /** Check if this VectorValues has the same structure (keys and dimensions) as another */ + bool hasSameStructure(const VectorValuesUnordered other) const; + /// @} /// @name Linear algebra operations /// @{ @@ -218,23 +219,35 @@ namespace gtsam { /** Squared vector L2 norm */ double squaredNorm() const; - /** - * + operator does element-wise addition. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ + /** Element-wise addition, synonym for add(). Both VectorValues must have the same structure + * (checked when NDEBUG is not defined). */ VectorValuesUnordered operator+(const VectorValuesUnordered& c) const; - /** - * + operator does element-wise subtraction. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ + /** Element-wise addition, synonym for operator+(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ + VectorValuesUnordered add(const VectorValuesUnordered& c) const; + + /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ + VectorValuesUnordered& operator+=(const VectorValuesUnordered& c); + + /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ + VectorValuesUnordered& addInPlace(const VectorValuesUnordered& c); + + /** Element-wise subtraction, synonym for subtract(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ VectorValuesUnordered operator-(const VectorValuesUnordered& c) const; - /** - * += operator does element-wise addition. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ - VectorValuesUnordered& operator+=(const VectorValuesUnordered& c); + /** Element-wise subtraction, synonym for operator-(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ + VectorValuesUnordered subtract(const VectorValuesUnordered& c) const; + + /** Element-wise scaling by a constant in-place. */ + VectorValuesUnordered& operator*=(double alpha); + + /** Element-wise scaling by a constant in-place. */ + VectorValuesUnordered& scaleInPlace(double alpha); /// @} @@ -242,7 +255,6 @@ namespace gtsam { /// @name Matlab syntactic sugar for linear algebra operations /// @{ - //inline VectorValuesUnordered add(const VectorValuesUnordered& c) const { return *this + c; } //inline VectorValuesUnordered scale(const double a, const VectorValuesUnordered& c) const { return a * (*this); } /// @} @@ -257,9 +269,6 @@ namespace gtsam { // return result; //} - // TODO: linear algebra interface seems to have been added for SPCG. - friend void scal(double alpha, VectorValuesUnordered& x); - //// 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())