Added Values filtering by key test and Value type
Richard Roberts
parent
cebd4631d2
commit
29d787a0ad
|
|
@ -30,6 +30,7 @@
|
|||
#include <boost/pool/pool_alloc.hpp>
|
||||
#include <boost/ptr_container/ptr_map.hpp>
|
||||
#include <boost/iterator/transform_iterator.hpp>
|
||||
#include <boost/iterator/filter_iterator.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/ptr_container/serialize_ptr_map.hpp>
|
||||
|
|
@ -81,13 +82,6 @@ namespace gtsam {
|
|||
/// A shared_ptr to this class
|
||||
typedef boost::shared_ptr<Values> 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<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
|
||||
|
|
@ -111,6 +113,28 @@ namespace gtsam {
|
|||
typedef boost::transform_iterator<
|
||||
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
|
||||
|
||||
/// Mutable filtered iterator
|
||||
typedef boost::filter_iterator<boost::function<bool(const KeyValuePair&)>, iterator> filter_iterator;
|
||||
|
||||
/// Const filtered iterator
|
||||
typedef boost::filter_iterator<boost::function<bool(const ConstKeyValuePair&)>, const_iterator> const_filter_iterator;
|
||||
|
||||
/// Mutable type-filtered iterator
|
||||
template<class ValueType>
|
||||
struct type_filter_iterator {
|
||||
typedef boost::transform_iterator<
|
||||
std::pair<const Key, ValueType&>(*)(KeyValuePair),
|
||||
boost::filter_iterator<bool(*)(const KeyValuePair&), iterator> > type;
|
||||
};
|
||||
|
||||
/// Const type-filtered iterator
|
||||
template<class ValueType>
|
||||
struct const_type_filter_iterator {
|
||||
typedef boost::transform_iterator<
|
||||
std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair),
|
||||
boost::filter_iterator<bool(*)(const ConstKeyValuePair&), const_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<bool(Key)>& filter) const {
|
||||
return boost::make_filter_iterator(
|
||||
boost::function<bool(const ConstKeyValuePair&)>(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<bool(Key)>& filter) const {
|
||||
return boost::make_filter_iterator(
|
||||
boost::function<bool(const ConstKeyValuePair&)>(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<bool(Key)>& filter) {
|
||||
return boost::make_filter_iterator(
|
||||
boost::function<bool(const KeyValuePair&)>(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<bool(Key)>& filter) {
|
||||
return boost::make_filter_iterator(
|
||||
boost::function<bool(const KeyValuePair&)>(boost::bind(filter, boost::bind(&KeyValuePair::first, _1))),
|
||||
end(), end()); }
|
||||
|
||||
private:
|
||||
template<class ValueType, class KeyValue>
|
||||
static bool _typeFilterHelper(const KeyValue& key_value) {
|
||||
return typeid(ValueType) == typeid(key_value.second);
|
||||
}
|
||||
|
||||
template<class ValueType>
|
||||
static std::pair<const Key, const ValueType&> _typeCastHelperConst(ConstKeyValuePair key_value) {
|
||||
return std::make_pair<const Key, const ValueType&>(key_value.first, static_cast<const ValueType&>(key_value.second)); }
|
||||
|
||||
template<class ValueType>
|
||||
static std::pair<const Key, ValueType&> _typeCastHelper(KeyValuePair key_value) {
|
||||
return std::make_pair<const Key, ValueType&>(key_value.first, static_cast<ValueType&>(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<class ValueType>
|
||||
typename const_type_filter_iterator<ValueType>::type beginFilterByType() const {
|
||||
return boost::make_transform_iterator(
|
||||
boost::filter_iterator<bool(*)(ConstKeyValuePair), const_iterator>(&_typeFilterHelper<ValueType,ConstKeyValuePair>, begin(), end()),
|
||||
&_typeCastHelperConst<ValueType>); }
|
||||
|
||||
/** Iterator over a subset of the Key-Value pairs, for which the Value type
|
||||
* matches the template parameter \c ValueType.
|
||||
*/
|
||||
template<class ValueType>
|
||||
typename const_type_filter_iterator<ValueType>::type endFilterByType() const {
|
||||
return boost::make_transform_iterator(
|
||||
boost::filter_iterator<bool(*)(ConstKeyValuePair), const_iterator>(&_typeFilterHelper<ValueType,ConstKeyValuePair>, end(), end()),
|
||||
&_typeCastHelperConst<ValueType>); }
|
||||
|
||||
/** Iterator over a subset of the Key-Value pairs, for which the Value type
|
||||
* matches the template parameter \c ValueType.
|
||||
*/
|
||||
template<class ValueType>
|
||||
typename type_filter_iterator<ValueType>::type beginFilterByType() {
|
||||
return boost::make_transform_iterator(
|
||||
boost::make_filter_iterator(&_typeFilterHelper<ValueType,KeyValuePair>, begin(), end()),
|
||||
&_typeCastHelper<ValueType>); }
|
||||
|
||||
/** Iterator over a subset of the Key-Value pairs, for which the Value type
|
||||
* matches the template parameter \c ValueType.
|
||||
*/
|
||||
template<class ValueType>
|
||||
typename type_filter_iterator<ValueType>::type endFilterByType() {
|
||||
return boost::make_transform_iterator(
|
||||
boost::make_filter_iterator(&_typeFilterHelper<ValueType,KeyValuePair>, end(), end()),
|
||||
&_typeCastHelper<ValueType>); }
|
||||
|
||||
/// @name Manifold Operations
|
||||
/// @{
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@ using namespace boost::assign;
|
|||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
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<Key>(), _1, 2));
|
||||
it != values.endFilterByKey(boost::bind(std::greater_equal<Key>(), _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<Pose3>::type it = values.beginFilterByType<Pose3>();
|
||||
it != values.endFilterByType<Pose3>(); ++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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue