diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 659779858..08bf9edca 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -81,13 +82,6 @@ namespace gtsam { /// A shared_ptr to this class typedef boost::shared_ptr shared_ptr; - /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator - struct ConstKeyValuePair { - const Key first; - const Value& second; - ConstKeyValuePair(Key key, const Value& value) : first(key), second(value) {} - }; - /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator struct KeyValuePair { const Key first; @@ -95,6 +89,14 @@ namespace gtsam { KeyValuePair(Key key, Value& value) : first(key), second(value) {} }; + /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator + struct ConstKeyValuePair { + const Key first; + const Value& second; + ConstKeyValuePair(Key key, const Value& value) : first(key), second(value) {} + ConstKeyValuePair(const KeyValuePair& key_value) : first(key_value.first), second(key_value.second) {} + }; + /// Mutable forward iterator, with value type KeyValuePair typedef boost::transform_iterator< boost::function1, KeyValueMap::iterator> iterator; @@ -111,6 +113,28 @@ namespace gtsam { typedef boost::transform_iterator< boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + /// Mutable filtered iterator + typedef boost::filter_iterator, iterator> filter_iterator; + + /// Const filtered iterator + typedef boost::filter_iterator, const_iterator> const_filter_iterator; + + /// Mutable type-filtered iterator + template + struct type_filter_iterator { + typedef boost::transform_iterator< + std::pair(*)(KeyValuePair), + boost::filter_iterator > type; + }; + + /// Const type-filtered iterator + template + struct const_type_filter_iterator { + typedef boost::transform_iterator< + std::pair(*)(ConstKeyValuePair), + boost::filter_iterator > type; + }; + /** Default constructor creates an empty Values class */ Values() {} @@ -191,6 +215,86 @@ namespace gtsam { reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); } reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); } + /** Iterator over a subset of the Key-Value pairs, for which the given + * \c filter function returns true when evaluated on the Key. */ + const_filter_iterator beginFilterByKey(const boost::function& filter) const { + return boost::make_filter_iterator( + boost::function(boost::bind(filter, boost::bind(&ConstKeyValuePair::first, _1))), + begin(), end()); } + + /** Iterator over a subset of the Key-Value pairs, for which the given + * \c filter function returns true when evaluated on the Key. */ + const_filter_iterator endFilterByKey(const boost::function& filter) const { + return boost::make_filter_iterator( + boost::function(boost::bind(filter, boost::bind(&ConstKeyValuePair::first, _1))), + end(), end()); } + + /** Iterator over a subset of the Key-Value pairs, for which the given + * \c filter function returns true when evaluated on the Key. */ + filter_iterator beginFilterByKey(const boost::function& filter) { + return boost::make_filter_iterator( + boost::function(boost::bind(filter, boost::bind(&KeyValuePair::first, _1))), + begin(), end()); } + + /** Iterator over a subset of the Key-Value pairs, for which the given + * \c filter function returns true when evaluated on the Key. */ + filter_iterator endFilterByKey(const boost::function& filter) { + return boost::make_filter_iterator( + boost::function(boost::bind(filter, boost::bind(&KeyValuePair::first, _1))), + end(), end()); } + + private: + template + static bool _typeFilterHelper(const KeyValue& key_value) { + return typeid(ValueType) == typeid(key_value.second); + } + + template + static std::pair _typeCastHelperConst(ConstKeyValuePair key_value) { + return std::make_pair(key_value.first, static_cast(key_value.second)); } + + template + static std::pair _typeCastHelper(KeyValuePair key_value) { + return std::make_pair(key_value.first, static_cast(key_value.second)); } + + public: + + /** Iterator over a subset of the Key-Value pairs, for which the Value type + * matches the template parameter \c ValueType. + */ + template + typename const_type_filter_iterator::type beginFilterByType() const { + return boost::make_transform_iterator( + boost::filter_iterator(&_typeFilterHelper, begin(), end()), + &_typeCastHelperConst); } + + /** Iterator over a subset of the Key-Value pairs, for which the Value type + * matches the template parameter \c ValueType. + */ + template + typename const_type_filter_iterator::type endFilterByType() const { + return boost::make_transform_iterator( + boost::filter_iterator(&_typeFilterHelper, end(), end()), + &_typeCastHelperConst); } + + /** Iterator over a subset of the Key-Value pairs, for which the Value type + * matches the template parameter \c ValueType. + */ + template + typename type_filter_iterator::type beginFilterByType() { + return boost::make_transform_iterator( + boost::make_filter_iterator(&_typeFilterHelper, begin(), end()), + &_typeCastHelper); } + + /** Iterator over a subset of the Key-Value pairs, for which the Value type + * matches the template parameter \c ValueType. + */ + template + typename type_filter_iterator::type endFilterByType() { + return boost::make_transform_iterator( + boost::make_filter_iterator(&_typeFilterHelper, end(), end()), + &_typeCastHelper); } + /// @name Manifold Operations /// @{ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 1ea70e213..1e9752127 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -26,6 +26,7 @@ using namespace boost::assign; #include #include #include +#include #include using namespace gtsam; @@ -259,6 +260,47 @@ TEST(Values, update) CHECK(assert_equal(expected,config0)); } +/* ************************************************************************* */ +TEST(Values, filter) { + Values values; + values.insert(0, Pose2()); + values.insert(1, Pose3()); + values.insert(2, Pose2()); + values.insert(3, Pose3()); + + // Filter by key + int i = 0; + for(Values::filter_iterator it = values.beginFilterByKey(boost::bind(std::greater_equal(), _1, 2)); + it != values.endFilterByKey(boost::bind(std::greater_equal(), _1, 2)); ++it, ++i) { + if(i == 0) { + LONGS_EQUAL(2, it->first); + EXPECT(typeid(Pose2) == typeid(it->second)); + } else if(i == 1) { + LONGS_EQUAL(3, it->first); + EXPECT(typeid(Pose3) == typeid(it->second)); + } else { + EXPECT(false); + } + } + LONGS_EQUAL(2, i); + + // Filter by type + i = 0; + for(Values::type_filter_iterator::type it = values.beginFilterByType(); + it != values.endFilterByType(); ++it, ++i) { + if(i == 0) { + LONGS_EQUAL(1, it->first); + EXPECT(assert_equal(Pose3(), it->second)); + } else if(i == 1) { + LONGS_EQUAL(3, it->first); + EXPECT(assert_equal(Pose3(), it->second)); + } else { + EXPECT(false); + } + } + LONGS_EQUAL(2, i); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */