diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 40e95188e..4cab86714 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -40,6 +40,170 @@ namespace gtsam { ValueCloneAllocator() {} }; + /* ************************************************************************* */ + template + struct _ValuesKeyValuePair { + const Key key; ///< The key + ValueType& value; ///< The value + + _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {} + }; + + /* ************************************************************************* */ + template + struct _ValuesConstKeyValuePair { + const Key key; ///< The key + const ValueType& value; ///< The value + + _ValuesConstKeyValuePair(Key _key, const ValueType& _value) : key(_key), value(_value) {} + _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} + }; + + /* ************************************************************************* */ + template + class Values::Filtered { + public: + /** A key-value pair, with the value a specific derived Value type. */ + typedef _ValuesKeyValuePair KeyValuePair; + typedef _ValuesConstKeyValuePair ConstKeyValuePair; + typedef KeyValuePair value_type; + + typedef + boost::transform_iterator< + KeyValuePair(*)(Values::KeyValuePair), + boost::filter_iterator< + boost::function, + Values::iterator> > + iterator; + + typedef iterator const_iterator; + + typedef + boost::transform_iterator< + ConstKeyValuePair(*)(Values::ConstKeyValuePair), + boost::filter_iterator< + boost::function, + Values::const_iterator> > + const_const_iterator; + + iterator begin() { return begin_; } + iterator end() { return end_; } + const_iterator begin() const { return begin_; } + const_iterator end() const { return end_; } + const_const_iterator beginConst() const { return constBegin_; } + const_const_iterator endConst() const { return constEnd_; } + + /** Returns the number of values in this view */ + size_t size() const { + size_t i = 0; + for (const_const_iterator it = beginConst(); it != endConst(); ++it) + ++i; + return i; + } + + private: + Filtered(const boost::function& filter, Values& values) : + begin_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, values.begin(), values.end()), + &castHelper)), + end_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, values.end(), values.end()), + &castHelper)), + constBegin_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, ((const Values&)values).begin(), ((const Values&)values).end()), + &castHelper)), + constEnd_(boost::make_transform_iterator( + boost::make_filter_iterator( + filter, ((const Values&)values).end(), ((const Values&)values).end()), + &castHelper)) {} + + friend class Values; + iterator begin_; + iterator end_; + const_const_iterator constBegin_; + const_const_iterator constEnd_; + }; + + /* ************************************************************************* */ + template + class Values::ConstFiltered { + public: + /** A const key-value pair, with the value a specific derived Value type. */ + typedef _ValuesConstKeyValuePair KeyValuePair; + typedef KeyValuePair value_type; + + typedef typename Filtered::const_const_iterator iterator; + typedef typename Filtered::const_const_iterator const_iterator; + + /** Conversion from Filtered to ConstFiltered */ + ConstFiltered(const Filtered& rhs) : + begin_(rhs.beginConst()), + end_(rhs.endConst()) {} + + iterator begin() { return begin_; } + iterator end() { return end_; } + const_iterator begin() const { return begin_; } + const_iterator end() const { return end_; } + + /** Returns the number of values in this view */ + size_t size() const { + size_t i = 0; + for (const_iterator it = begin(); it != end(); ++it) + ++i; + return i; + } + + private: + friend class Values; + const_iterator begin_; + const_iterator end_; + ConstFiltered(const boost::function& filter, const Values& values) { + // We remove the const from values to create a non-const Filtered + // view, then pull the const_iterators out of it. + const Filtered filtered(filter, const_cast(values)); + begin_ = filtered.beginConst(); + end_ = filtered.endConst(); + } + }; + + /* ************************************************************************* */ + template + Values::Values(const Values::ConstFiltered& view) { + BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { + Key key = key_value.key; + insert(key, key_value.value); + } + } + + /* ************************************************************************* */ + Values::Filtered + inline Values::filter(const boost::function& filterFcn) { + return filter(filterFcn); + } + + /* ************************************************************************* */ + template + Values::Filtered + Values::filter(const boost::function& filterFcn) { + return Filtered(boost::bind(&filterHelper, filterFcn, _1), *this); + } + + /* ************************************************************************* */ + Values::ConstFiltered + inline Values::filter(const boost::function& filterFcn) const { + return filter(filterFcn); + } + + /* ************************************************************************* */ + template + Values::ConstFiltered + Values::filter(const boost::function& filterFcn) const { + return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); + } + /* ************************************************************************* */ template const ValueType& Values::at(Key j) const { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 37708d9d9..d9778bb0f 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -122,133 +122,15 @@ namespace gtsam { typedef KeyValuePair value_type; - private: - template - struct _KeyValuePair { - const Key key; ///< The key - ValueType& value; ///< The value - - _KeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {} - }; - - template - struct _ConstKeyValuePair { - const Key key; ///< The key - const ValueType& value; ///< The value - - _ConstKeyValuePair(Key _key, const ValueType& _value) : key(_key), value(_value) {} - _ConstKeyValuePair(const _KeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} - }; - public: + /** A filtered view of a Values, returned from Values::filter. */ template - class Filtered { - public: - /** A key-value pair, with the value a specific derived Value type. */ - typedef _KeyValuePair KeyValuePair; - typedef _ConstKeyValuePair ConstKeyValuePair; - typedef KeyValuePair value_type; - - typedef - boost::transform_iterator< - KeyValuePair(*)(Values::KeyValuePair), - boost::filter_iterator< - boost::function, - Values::iterator> > - iterator; - - typedef iterator const_iterator; - - typedef - boost::transform_iterator< - ConstKeyValuePair(*)(Values::ConstKeyValuePair), - boost::filter_iterator< - boost::function, - Values::const_iterator> > - const_const_iterator; - - iterator begin() { return begin_; } - iterator end() { return end_; } - const_iterator begin() const { return begin_; } - const_iterator end() const { return end_; } - const_const_iterator beginConst() const { return constBegin_; } - const_const_iterator endConst() const { return constEnd_; } - - /** Returns the number of values in this view */ - size_t size() const { - size_t i = 0; - for (const_const_iterator it = beginConst(); it != endConst(); ++it) - ++i; - return i; - } - - private: - Filtered(const boost::function& filter, Values& values) : - begin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.begin(), values.end()), - &castHelper)), - end_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.end(), values.end()), - &castHelper)), - constBegin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).begin(), ((const Values&)values).end()), - &castHelper)), - constEnd_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).end(), ((const Values&)values).end()), - &castHelper)) {} - - friend class Values; - iterator begin_; - iterator end_; - const_const_iterator constBegin_; - const_const_iterator constEnd_; - }; + class Filtered; + /** A filtered view of a const Values, returned from Values::filter. */ template - class ConstFiltered { - public: - /** A const key-value pair, with the value a specific derived Value type. */ - typedef _ConstKeyValuePair KeyValuePair; - typedef KeyValuePair value_type; - - typedef typename Filtered::const_const_iterator iterator; - typedef typename Filtered::const_const_iterator const_iterator; - - /** Conversion from Filtered to ConstFiltered */ - ConstFiltered(const Filtered& rhs) : - begin_(rhs.beginConst()), - end_(rhs.endConst()) {} - - iterator begin() { return begin_; } - iterator end() { return end_; } - const_iterator begin() const { return begin_; } - const_iterator end() const { return end_; } - - /** Returns the number of values in this view */ - size_t size() const { - size_t i = 0; - for (const_iterator it = begin(); it != end(); ++it) - ++i; - return i; - } - - private: - friend class Values; - const_iterator begin_; - const_iterator end_; - ConstFiltered(const boost::function& filter, const Values& values) { - // We remove the const from values to create a non-const Filtered - // view, then pull the const_iterators out of it. - const Filtered filtered(filter, const_cast(values)); - begin_ = filtered.beginConst(); - end_ = filtered.endConst(); - } - }; + class ConstFiltered; /** Default constructor creates an empty Values class */ Values() {} @@ -256,23 +138,9 @@ namespace gtsam { /** Copy constructor duplicates all keys and values */ Values(const Values& other); - /** Constructor from a Filtered view copies out all values */ + /** Constructor from a Filtered or ConstFiltered view */ template - Values(const Filtered& view) { - BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { - Key key = key_value.key; - insert(key, key_value.value); - } - } - - /** Constructor from Const Filtered view */ - template - Values(const ConstFiltered& view) { - BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { - Key key = key_value.key; - insert(key, key_value.value); - } - } + Values(const ConstFiltered& view); /// @name Testable /// @{ @@ -323,12 +191,6 @@ namespace gtsam { /** Get a zero VectorValues of the correct structure */ VectorValues zeroVectors(const Ordering& ordering) const; - private: - static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { - return ConstKeyValuePair(key_value.first, *key_value.second); } - static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { - return KeyValuePair(key_value.first, *key_value.second); } - public: const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } @@ -411,9 +273,7 @@ namespace gtsam { * the original Values class. */ Filtered - filter(const boost::function& filterFcn) { - return filter(filterFcn); - } + filter(const boost::function& filterFcn); /** * Return a filtered view of this Values class, without copying any data. @@ -436,9 +296,7 @@ namespace gtsam { */ template Filtered - filter(const boost::function& filterFcn = (boost::lambda::_1, true)) { - return Filtered(boost::bind(&filterHelper, filterFcn, _1), *this); - } + filter(const boost::function& filterFcn = (boost::lambda::_1, true)); /** * Return a filtered view of this Values class, without copying any data. @@ -454,9 +312,7 @@ namespace gtsam { * the original Values class. */ ConstFiltered - filter(const boost::function& filterFcn) const { - return filter(filterFcn); - } + filter(const boost::function& filterFcn) const; /** * Return a filtered view of this Values class, without copying any data. @@ -478,9 +334,7 @@ namespace gtsam { */ template ConstFiltered - filter(const boost::function& filterFcn = (boost::lambda::_1, true)) const { - return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); - } + filter(const boost::function& filterFcn = (boost::lambda::_1, true)) const; private: // Filters based on ValueType (if not Value) and also based on the user- @@ -505,6 +359,12 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(values_); } + static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { + return ConstKeyValuePair(key_value.first, *key_value.second); } + + static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { + return KeyValuePair(key_value.first, *key_value.second); } + }; /* ************************************************************************* */