From 4d6647ba01594788d3bb0573b43b778d686aefe4 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 4 Jul 2013 15:33:06 +0000 Subject: [PATCH] Working on unordered VectorValues. --- gtsam/linear/VectorValuesUnordered.cpp | 90 +++++++++++++-------- gtsam/linear/VectorValuesUnordered.h | 104 ++++++++++++------------- 2 files changed, 109 insertions(+), 85 deletions(-) diff --git a/gtsam/linear/VectorValuesUnordered.cpp b/gtsam/linear/VectorValuesUnordered.cpp index ff796213f..0d853a178 100644 --- a/gtsam/linear/VectorValuesUnordered.cpp +++ b/gtsam/linear/VectorValuesUnordered.cpp @@ -18,15 +18,35 @@ #include +#include #include #include -#include #include +#include using namespace std; namespace gtsam { + using boost::combine; + using boost::adaptors::transformed; + 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()); + } +} + /* ************************************************************************* */ void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const { std::cout << str << ": " << size() << " elements\n"; @@ -41,8 +61,8 @@ bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) c return false; 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)) + 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; @@ -51,11 +71,10 @@ bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) c /* ************************************************************************* */ 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); + DenseIndex totalDim = 0; + BOOST_FOREACH(const value_type& v, *this) + totalDim += v.second.size(); // Copy vectors Vector result; @@ -104,7 +123,7 @@ double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const 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, + 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")); @@ -128,43 +147,52 @@ double VectorValuesUnordered::squaredNorm() const { } /* ************************************************************************* */ -VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const { - VectorValuesUnordered result = SameStructure(*this); +VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const +{ if(this->size() != c.size()) throw invalid_argument("VectorValues::operator+ 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() == c.values_[j].size()) - result.values_[j] = this->values_[j] + c.values_[j]; - else - 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")); + + VectorValuesUnordered result; + // The result.end() hint here should result in constant-time inserts + for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) + result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); + return result; } /* ************************************************************************* */ -VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const { - VectorValuesUnordered result = SameStructure(*this); +VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const +{ if(this->size() != c.size()) throw invalid_argument("VectorValues::operator- 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() == c.values_[j].size()) - result.values_[j] = this->values_[j] - c.values_[j]; - else - 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")); + + VectorValuesUnordered result; + // The result.end() hint here should result in constant-time inserts + for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) + result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); + return result; } /* ************************************************************************* */ -void VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) { +VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) +{ if(this->size() != c.size()) throw invalid_argument("VectorValues::operator+= 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() == c.values_[j].size()) - this->values_[j] += c.values_[j]; - else - 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; } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValuesUnordered.h b/gtsam/linear/VectorValuesUnordered.h index 5eff65df2..52379e13e 100644 --- a/gtsam/linear/VectorValuesUnordered.h +++ b/gtsam/linear/VectorValuesUnordered.h @@ -21,12 +21,7 @@ #include #include -#include -#include -#include #include -#include -#include namespace gtsam { @@ -91,6 +86,7 @@ namespace gtsam { */ class GTSAM_EXPORT VectorValuesUnordered { protected: + typedef VectorValuesUnordered This; typedef FastMap Values; ///< Typedef for the collection of Vectors making up a VectorValues Values values_; ///< Collection of Vectors making up this VectorValues @@ -227,7 +223,7 @@ namespace gtsam { * += operator does element-wise addition. Both VectorValues must have the * same structure (checked when NDEBUG is not defined). */ - void operator+=(const VectorValuesUnordered& c); + VectorValuesUnordered& operator+=(const VectorValuesUnordered& c); /// @} @@ -235,63 +231,63 @@ 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); } + //inline VectorValuesUnordered add(const VectorValuesUnordered& c) const { return *this + c; } + //inline VectorValuesUnordered scale(const double a, const VectorValuesUnordered& c) const { return a * (*this); } /// @} /** * scale a vector by a scalar */ - friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) { - VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v); - for(Key j = 0; j < v.size(); ++j) - result.values_[j] = a * v.values_[j]; - return result; - } + //friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) { + // VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v); + // 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(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(Key j = 0; j < x.size(); ++j) - if(x.values_[j].size() == y.values_[j].size()) - y.values_[j] += alpha * x.values_[j]; - else - throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - } - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void sqrt(VectorValuesUnordered &x) { - for(Key j = 0; j < x.size(); ++j) - x.values_[j] = x.values_[j].cwiseSqrt(); - } + ///// TODO: linear algebra interface seems to have been added for SPCG. + //friend void scal(double alpha, VectorValuesUnordered& x) { + // 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(Key j = 0; j < x.size(); ++j) + // if(x.values_[j].size() == y.values_[j].size()) + // y.values_[j] += alpha * x.values_[j]; + // else + // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + //} + ///// TODO: linear algebra interface seems to have been added for SPCG. + //friend void sqrt(VectorValuesUnordered &x) { + // for(Key j = 0; j < x.size(); ++j) + // x.values_[j] = x.values_[j].cwiseSqrt(); + //} - /// TODO: linear algebra interface seems to have been added for SPCG. - 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(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 - throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - } + ///// TODO: linear algebra interface seems to have been added for SPCG. + //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(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 + // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + //} - /// TODO: linear algebra interface seems to have been added for SPCG. - 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(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 - throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - } + ///// TODO: linear algebra interface seems to have been added for SPCG. + //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(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 + // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + //} private: /** Serialization function */