From 4bfde71d8aa4889eb82f7b2764e418a914992796 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 20 Jan 2023 12:43:09 -0800 Subject: [PATCH] Deprecate `filter` in favor of `extract`, which copies to std::map without boost. --- gtsam/nonlinear/Values-inl.h | 29 ++++-- gtsam/nonlinear/Values.h | 140 +++++++++++---------------- gtsam/nonlinear/tests/testValues.cpp | 20 +++- gtsam/nonlinear/utilities.h | 85 ++++++++-------- gtsam/sfm/ShonanAveraging.cpp | 26 ++--- gtsam/sfm/ShonanAveraging.h | 4 +- 6 files changed, 152 insertions(+), 152 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 0370c5ceea..598963761d 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -90,7 +90,25 @@ namespace gtsam { } }; - /* ************************************************************************* */ +/* ************************************************************************* */ + template + std::map + Values::extract(const std::function& filterFcn) const { + std::map result; + for (const auto& key_value : *this) { + // Check if key matches + if (filterFcn(key_value.key)) { + // Check if type matches (typically does as symbols matched with types) + if (auto t = + dynamic_cast*>(&key_value.value)) + result[key_value.key] = t->value(); + } + } + return result; + } + +/* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 template class Values::Filtered { public: @@ -164,7 +182,6 @@ namespace gtsam { const_const_iterator constEnd_; }; - /* ************************************************************************* */ template class Values::ConstFiltered { public: @@ -215,8 +232,6 @@ namespace gtsam { } }; - /* ************************************************************************* */ - /** Constructor from a Filtered view copies out all values */ template Values::Values(const Values::Filtered& view) { for(const auto key_value: view) { @@ -225,7 +240,6 @@ namespace gtsam { } } - /* ************************************************************************* */ template Values::Values(const Values::ConstFiltered& view) { for(const auto key_value: view) { @@ -234,13 +248,11 @@ namespace gtsam { } } - /* ************************************************************************* */ Values::Filtered inline Values::filter(const std::function& filterFcn) { return filter(filterFcn); } - /* ************************************************************************* */ template Values::Filtered Values::filter(const std::function& filterFcn) { @@ -248,19 +260,18 @@ namespace gtsam { std::placeholders::_1), *this); } - /* ************************************************************************* */ Values::ConstFiltered inline Values::filter(const std::function& filterFcn) const { return filter(filterFcn); } - /* ************************************************************************* */ template Values::ConstFiltered Values::filter(const std::function& filterFcn) const { return ConstFiltered(std::bind(&filterHelper, filterFcn, std::placeholders::_1), *this); } +#endif /* ************************************************************************* */ template<> diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index cfe6347b50..79ddb02674 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,9 +29,11 @@ #include #include #include -#include #include #include +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +#include +#endif #include #include @@ -126,14 +128,6 @@ namespace gtsam { typedef KeyValuePair value_type; - /** A filtered view of a Values, returned from Values::filter. */ - template - class Filtered; - - /** A filtered view of a const Values, returned from Values::filter. */ - template - class ConstFiltered; - /** Default constructor creates an empty Values class */ Values() {} @@ -153,14 +147,6 @@ namespace gtsam { /** Construct from a Values and an update vector: identical to other.retract(delta) */ Values(const Values& other, const VectorValues& delta); - /** Constructor from a Filtered view copies out all values */ - template - Values(const Filtered& view); - - /** Constructor from a Filtered or ConstFiltered view */ - template - Values(const ConstFiltered& view); - /// @name Testable /// @{ @@ -322,30 +308,26 @@ namespace gtsam { /** Return a VectorValues of zero vectors for each variable in this Values */ VectorValues zeroVectors() const; - /** - * Return a filtered view of this Values class, without copying any data. - * When iterating over the filtered view, only the key-value pairs - * with a key causing \c filterFcn to return \c true are visited. Because - * the object Filtered returned from filter() is only a - * view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. - * @param filterFcn The function that determines which key-value pairs are - * included in the filtered view, for which this function returns \c true - * on their keys. - * @return A filtered view of the original Values class, which references - * the original Values class. - */ - Filtered - filter(const std::function& filterFcn); + // Count values of given type \c ValueType + template + size_t count() const { + size_t i = 0; + for (const auto key_value : *this) { + if (dynamic_cast*>(&key_value.value)) + ++i; + } + return i; + } /** - * Return a filtered view of this Values class, without copying any data. + * Extract a subset of values of the given type \c ValueType. + * * In this templated version, only key-value pairs whose value matches the * template argument \c ValueType and whose key causes the function argument * \c filterFcn to return true are visited when iterating over the filtered - * view. Because the object Filtered returned from filter() is only - * a view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. + * view. This replaces the fancier but very boost-dependent \c filter methods + * that were previously available up to GTSAM 4.2. + * * @tparam ValueType The type that the value in a key-value pair must match * to be included in the filtered view. Currently, base classes are not * resolved so the type must match exactly, except if ValueType = Value, in @@ -354,61 +336,47 @@ namespace gtsam { * included in the filtered view, for which this function returns \c true * on their keys (default: always return true so that filter() only * filters by type, matching \c ValueType). - * @return A filtered view of the original Values class, which references - * the original Values class. + * @return An Eigen aligned map on Key with the filtered values. */ - template - Filtered + template + std::map // , std::less, Eigen::aligned_allocator + extract(const std::function& filterFcn = &_truePredicate) const; + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** A filtered view of a Values, returned from Values::filter. */ + template + class Filtered; + + /** A filtered view of a const Values, returned from Values::filter. */ + template + class ConstFiltered; + + /** Constructor from a Filtered view copies out all values */ + template + Values(const Filtered& view); + + /** Constructor from a Filtered or ConstFiltered view */ + template + Values(const ConstFiltered& view); + + /// A filtered view of the original Values class. + Filtered GTSAM_DEPRECATED + filter(const std::function& filterFcn); + + /// A filtered view of the original Values class, also filter on type. + template + Filtered GTSAM_DEPRECATED filter(const std::function& filterFcn = &_truePredicate); - /** - * Return a filtered view of this Values class, without copying any data. - * When iterating over the filtered view, only the key-value pairs - * with a key causing \c filterFcn to return \c true are visited. Because - * the object Filtered returned from filter() is only a - * view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. - * @param filterFcn The function that determines which key-value pairs are - * included in the filtered view, for which this function returns \c true - * on their keys. - * @return A filtered view of the original Values class, which references - * the original Values class. - */ - ConstFiltered + /// A filtered view of the original Values class, const version. + ConstFiltered GTSAM_DEPRECATED filter(const std::function& filterFcn) const; - /** - * Return a filtered view of this Values class, without copying any data. - * In this templated version, only key-value pairs whose value matches the - * template argument \c ValueType and whose key causes the function argument - * \c filterFcn to return true are visited when iterating over the filtered - * view. Because the object Filtered returned from filter() is only - * a view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. - * @tparam ValueType The type that the value in a key-value pair must match - * to be included in the filtered view. Currently, base classes are not - * resolved so the type must match exactly, except if ValueType = Value, in - * which case no type filtering is done. - * @param filterFcn The function that determines which key-value pairs are - * included in the filtered view, for which this function returns \c true - * on their keys. - * @return A filtered view of the original Values class, which references - * the original Values class. - */ - template - ConstFiltered - filter(const std::function& filterFcn = &_truePredicate) const; - - // Count values of given type \c ValueType - template - size_t count() const { - size_t i = 0; - for (const auto key_value : *this) { - if (dynamic_cast*>(&key_value.value)) - ++i; - } - return i; - } + /// A filtered view of the original Values class, also on type, const. + template + ConstFiltered GTSAM_DEPRECATED filter( + const std::function& filterFcn = &_truePredicate) const; +#endif private: // Filters based on ValueType (if not Value) and also based on the user- diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 2465903db1..85dd2f4b3c 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -343,6 +343,7 @@ TEST(Values, filter) { values.insert(2, pose2); values.insert(3, pose3); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 // Filter by key int i = 0; Values::Filtered filtered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); @@ -395,8 +396,6 @@ TEST(Values, filter) { ++ i; } EXPECT_LONGS_EQUAL(2, (long)i); - EXPECT_LONGS_EQUAL(2, (long)values.count()); - EXPECT_LONGS_EQUAL(2, (long)values.count()); // construct a values with the view Values actualSubValues2(pose_filtered); @@ -404,6 +403,16 @@ TEST(Values, filter) { expectedSubValues2.insert(1, pose1); expectedSubValues2.insert(3, pose3); EXPECT(assert_equal(expectedSubValues2, actualSubValues2)); +#endif + + // Test counting by type. + EXPECT_LONGS_EQUAL(2, (long)values.count()); + EXPECT_LONGS_EQUAL(2, (long)values.count()); + + // Filter by type using extract. + auto extracted_pose3s = values.extract(); + EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size()); + } /* ************************************************************************* */ @@ -419,6 +428,7 @@ TEST(Values, Symbol_filter) { values.insert(X(2), pose2); values.insert(Symbol('y', 3), pose3); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 int i = 0; for(const auto key_value: values.filter(Symbol::ChrTest('y'))) { if(i == 0) { @@ -433,6 +443,12 @@ TEST(Values, Symbol_filter) { ++ i; } LONGS_EQUAL(2, (long)i); +#endif + +// Test extract with filter on symbol: + auto extracted_pose3s = values.extract(Symbol::ChrTest('y')); + EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size()); + } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index d2b38d3743..985e1a5f4e 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -90,19 +90,19 @@ KeySet createKeySet(std::string s, const Vector& I) { /// Extract all Point2 values into a single matrix [x y] Matrix extractPoint2(const Values& values) { - Values::ConstFiltered points = values.filter(); + const auto points = values.extract(); // Point2 is aliased as a gtsam::Vector in the wrapper - Values::ConstFiltered points2 = values.filter(); + const auto points2 = values.extract(); Matrix result(points.size() + points2.size(), 2); size_t j = 0; for (const auto& key_value : points) { - result.row(j++) = key_value.value; + result.row(j++) = key_value.second; } for (const auto& key_value : points2) { - if (key_value.value.rows() == 2) { - result.row(j++) = key_value.value; + if (key_value.second.rows() == 2) { + result.row(j++) = key_value.second; } } return result; @@ -110,19 +110,19 @@ Matrix extractPoint2(const Values& values) { /// Extract all Point3 values into a single matrix [x y z] Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); + const auto points = values.extract(); // Point3 is aliased as a gtsam::Vector in the wrapper - Values::ConstFiltered points2 = values.filter(); + const auto points2 = values.extract(); Matrix result(points.size() + points2.size(), 3); size_t j = 0; for (const auto& key_value : points) { - result.row(j++) = key_value.value; + result.row(j++) = key_value.second; } for (const auto& key_value : points2) { - if (key_value.value.rows() == 3) { - result.row(j++) = key_value.value; + if (key_value.second.rows() == 3) { + result.row(j++) = key_value.second; } } return result; @@ -130,34 +130,40 @@ Matrix extractPoint3(const Values& values) { /// Extract all Pose3 values Values allPose2s(const Values& values) { - return values.filter(); + Values result; + for(const auto& key_value: values.extract()) + result.insert(key_value.first, key_value.second); + return result; } /// Extract all Pose2 values into a single matrix [x y theta] Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); + const auto poses = values.extract(); Matrix result(poses.size(), 3); size_t j = 0; for(const auto& key_value: poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + result.row(j++) << key_value.second.x(), key_value.second.y(), key_value.second.theta(); return result; } /// Extract all Pose3 values Values allPose3s(const Values& values) { - return values.filter(); + Values result; + for(const auto& key_value: values.extract()) + result.insert(key_value.first, key_value.second); + return result; } /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] Matrix extractPose3(const Values& values) { - Values::ConstFiltered poses = values.filter(); + const auto poses = values.extract(); Matrix result(poses.size(), 12); size_t j = 0; for(const auto& key_value: poses) { - result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); - result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); - result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation(); + result.row(j).segment(0, 3) << key_value.second.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.second.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.second.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.second.translation(); j++; } return result; @@ -172,20 +178,19 @@ Matrix extractPose3(const Values& values) { /// variables x1, x2, ..., x200 of type Vector each 5-dimensional, will return a /// 200x5 matrix with row i containing xi. Matrix extractVectors(const Values& values, char c) { - Values::ConstFiltered vectors = - values.filter(Symbol::ChrTest(c)); + const auto vectors = values.extract(Symbol::ChrTest(c)); if (vectors.size() == 0) { return Matrix(); } - auto dim = vectors.begin()->value.size(); + auto dim = vectors.begin()->second.size(); Matrix result(vectors.size(), dim); Eigen::Index rowi = 0; for (const auto& kv : vectors) { - if (kv.value.size() != dim) { + if (kv.second.size() != dim) { throw std::runtime_error( "Tried to extract different-sized vectors into a single matrix"); } - result.row(rowi) = kv.value; + result.row(rowi) = kv.second; ++rowi; } return result; @@ -196,14 +201,14 @@ void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, sigma); Sampler sampler(model, seed); - for (const auto& key_value : values.filter()) { - values.update(key_value.key, - key_value.value + Point2(sampler.sample())); + for (const auto& key_value : values.extract()) { + values.update(key_value.first, + key_value.second + Point2(sampler.sample())); } - for (const auto& key_value : values.filter()) { - if (key_value.value.rows() == 2) { - values.update(key_value.key, - key_value.value + Point2(sampler.sample())); + for (const auto& key_value : values.extract()) { + if (key_value.second.rows() == 2) { + values.update(key_value.first, + key_value.second + Point2(sampler.sample())); } } } @@ -214,8 +219,8 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( Vector3(sigmaT, sigmaT, sigmaR)); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + for(const auto& key_value: values.extract()) { + values.update(key_value.first, key_value.second.retract(sampler.sample())); } } @@ -224,14 +229,14 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, sigma); Sampler sampler(model, seed); - for (const auto& key_value : values.filter()) { - values.update(key_value.key, - key_value.value + Point3(sampler.sample())); + for (const auto& key_value : values.extract()) { + values.update(key_value.first, + key_value.second + Point3(sampler.sample())); } - for (const auto& key_value : values.filter()) { - if (key_value.value.rows() == 3) { - values.update(key_value.key, - key_value.value + Point3(sampler.sample())); + for (const auto& key_value : values.extract()) { + if (key_value.second.rows() == 3) { + values.update(key_value.first, + key_value.second + Point3(sampler.sample())); } } } diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index c00669e365..ea4171238a 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -207,9 +207,9 @@ Matrix ShonanAveraging::StiefelElementMatrix(const Values &values) { const size_t N = values.size(); const size_t p = values.at(0).rows(); Matrix S(p, N * d); - for (const auto it : values.filter()) { - S.middleCols(it.key * d) = - it.value.matrix().leftCols(); // project Qj to Stiefel + for (const auto& it : values.extract()) { + S.middleCols(it.first * d) = + it.second.matrix().leftCols(); // project Qj to Stiefel } return S; } @@ -218,11 +218,11 @@ Matrix ShonanAveraging::StiefelElementMatrix(const Values &values) { template <> Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const { Values result; - for (const auto it : values.filter()) { - assert(it.value.rows() == p); - const auto &M = it.value.matrix(); + for (const auto& it : values.extract()) { + assert(it.second.rows() == p); + const auto &M = it.second.matrix(); const Rot2 R = Rot2::atan2(M(1, 0), M(0, 0)); - result.insert(it.key, R); + result.insert(it.first, R); } return result; } @@ -230,11 +230,11 @@ Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const { template <> Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const { Values result; - for (const auto it : values.filter()) { - assert(it.value.rows() == p); - const auto &M = it.value.matrix(); + for (const auto& it : values.extract()) { + assert(it.second.rows() == p); + const auto &M = it.second.matrix(); const Rot3 R = Rot3::ClosestTo(M.topLeftCorner<3, 3>()); - result.insert(it.key, R); + result.insert(it.first, R); } return result; } @@ -326,8 +326,8 @@ double ShonanAveraging::cost(const Values &values) const { } // Finally, project each dxd rotation block to SO(d) Values result; - for (const auto it : values.filter()) { - result.insert(it.key, SO(it.value.matrix())); + for (const auto& it : values.extract()) { + result.insert(it.first, SO(it.second.matrix())); } return graph.error(result); } diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index e035da4c78..b40916828f 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -366,8 +366,8 @@ class GTSAM_EXPORT ShonanAveraging { template static Values LiftTo(size_t p, const Values &values) { Values result; - for (const auto it : values.filter()) { - result.insert(it.key, SOn::Lift(p, it.value.matrix())); + for (const auto it : values.extract()) { + result.insert(it.first, SOn::Lift(p, it.second.matrix())); } return result; }