diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index a354e0139f..04c6440a2b 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -163,14 +163,14 @@ namespace gtsam { &ValuesCastHelper::cast)), constBegin_( boost::make_transform_iterator( boost::make_filter_iterator(filter, - ((const Values&) values).begin(), - ((const Values&) values).end()), + values._begin(), + values._end()), &ValuesCastHelper::cast)), constEnd_( boost::make_transform_iterator( boost::make_filter_iterator(filter, - ((const Values&) values).end(), - ((const Values&) values).end()), + values._end(), + values._end()), &ValuesCastHelper::cast)) { } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 161df2cba1..74f22a27df 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -108,8 +108,11 @@ namespace gtsam { typedef KeyValuePair value_type; + /// @name Constructors + /// @{ + /** Default constructor creates an empty Values class */ - Values() {} + Values() = default; /** Copy constructor duplicates all keys and values */ Values(const Values& other); @@ -127,6 +130,7 @@ namespace gtsam { /** Construct from a Values and an update vector: identical to other.retract(delta) */ Values(const Values& other, const VectorValues& delta); + /// @} /// @name Testable /// @{ @@ -137,6 +141,8 @@ namespace gtsam { bool equals(const Values& other, double tol=1e-9) const; /// @} + /// @name Standard Interface + /// @{ /** Retrieve a variable by key \c j. The type of the value associated with * this key is supplied as a template argument to this function. @@ -177,6 +183,42 @@ namespace gtsam { /** whether the config is empty */ bool empty() const { return values_.empty(); } + /// @} + /// @name Iterator + /// @{ + + struct deref_iterator { + using const_iterator_type = typename KeyValueMap::const_iterator; + const_iterator_type it_; + deref_iterator(const_iterator_type it) : it_(it) {} + ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; } + boost::shared_ptr operator->() { + return boost::make_shared(it_->first, *(it_->second)); + } + bool operator==(const deref_iterator& other) const { + return it_ == other.it_; + } + bool operator!=(const deref_iterator& other) const { return it_ != other.it_; } + deref_iterator& operator++() { + ++it_; + return *this; + } + }; + + deref_iterator begin() const { return deref_iterator(values_.begin()); } + deref_iterator end() const { return deref_iterator(values_.end()); } + + /** Find an element by key, returning an iterator, or end() if the key was + * not found. */ + deref_iterator find(Key j) const { return deref_iterator(values_.find(j)); } + + /** Find the element greater than or equal to the specified key. */ + deref_iterator lower_bound(Key j) const { return deref_iterator(values_.lower_bound(j)); } + + /** Find the lowest-ordered element greater than the specified key. */ + deref_iterator upper_bound(Key j) const { return deref_iterator(values_.upper_bound(j)); } + + /// @} /// @name Manifold Operations /// @{ @@ -334,8 +376,8 @@ namespace gtsam { static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { return KeyValuePair(key_value.first, *key_value.second); } - 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); } + 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); } iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); } const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } @@ -347,22 +389,12 @@ namespace gtsam { * not found. */ iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); } - /** Find an element by key, returning an iterator, or end() if the key was - * not found. */ - const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); } - /** Find the element greater than or equal to the specified key. */ iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); } - /** Find the element greater than or equal to the specified key. */ - const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); } - /** Find the lowest-ordered element greater than the specified key. */ iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); } - /** Find the lowest-ordered element greater than the specified key. */ - const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); } - /** A filtered view of a Values, returned from Values::filter. */ template class Filtered; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 644b8c84f1..758d9a5a32 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -195,7 +195,14 @@ TEST(Values, basic_functions) values.insert(6, M1); values.insert(8, M2); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + size_t count = 0; + for (const auto& it : values) { + count += 1; + if (it.key == 2 || it.key == 4) EXPECT_LONGS_EQUAL(3, it.value.dim()); + if (it.key == 6 || it.key == 8) EXPECT_LONGS_EQUAL(6, it.value.dim()); + } + EXPECT_LONGS_EQUAL(4, count); + // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); EXPECT_LONGS_EQUAL(4, values_c.find(4)->key); @@ -211,7 +218,6 @@ TEST(Values, basic_functions) EXPECT_LONGS_EQUAL(6, values_c.upper_bound(4)->key); EXPECT_LONGS_EQUAL(4, values.upper_bound(3)->key); EXPECT_LONGS_EQUAL(4, values_c.upper_bound(3)->key); -#endif } /* ************************************************************************* */