12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1398 from borglab/feature/deprecate_filter2

release/4.3a0
Frank Dellaert 2023-01-20 15:02:58 -08:00 committed by GitHub
parent 5029e3ddc6 603d665342
commit 22e9e80f43
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 160 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/nonlinear/Values-inl.h
View File

@ -90,7 +90,25 @@ namespace gtsam {
}
};
/* ************************************************************************* */
/* ************************************************************************* */
template <class ValueType>
std::map<Key, ValueType>
Values::extract(const std::function<bool(Key)>& filterFcn) const {
std::map<Key, ValueType> 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<const GenericValue<ValueType>*>(&key_value.value))
result[key_value.key] = t->value();
}
}
return result;
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
template<class ValueType>
class Values::Filtered {
public:
@ -164,7 +182,6 @@ namespace gtsam {
const_const_iterator constEnd_;
};
/* ************************************************************************* */
template<class ValueType>
class Values::ConstFiltered {
public:
@ -215,8 +232,6 @@ namespace gtsam {
}
};
/* ************************************************************************* */
/** Constructor from a Filtered view copies out all values */
template<class ValueType>
Values::Values(const Values::Filtered<ValueType>& view) {
for(const auto key_value: view) {
@ -225,7 +240,6 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class ValueType>
Values::Values(const Values::ConstFiltered<ValueType>& view) {
for(const auto key_value: view) {
@ -234,13 +248,11 @@ namespace gtsam {
}
}
/* ************************************************************************* */
Values::Filtered<Value>
inline Values::filter(const std::function<bool(Key)>& filterFcn) {
return filter<Value>(filterFcn);
}
/* ************************************************************************* */
template<class ValueType>
Values::Filtered<ValueType>
Values::filter(const std::function<bool(Key)>& filterFcn) {
@ -248,19 +260,18 @@ namespace gtsam {
std::placeholders::_1), *this);
}
/* ************************************************************************* */
Values::ConstFiltered<Value>
inline Values::filter(const std::function<bool(Key)>& filterFcn) const {
return filter<Value>(filterFcn);
}
/* ************************************************************************* */
template<class ValueType>
Values::ConstFiltered<ValueType>
Values::filter(const std::function<bool(Key)>& filterFcn) const {
return ConstFiltered<ValueType>(std::bind(&filterHelper<ValueType>,
filterFcn, std::placeholders::_1), *this);
}
#endif
/* ************************************************************************* */
template<>

156
gtsam/nonlinear/Values.h
View File

@ -29,9 +29,11 @@
#include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Key.h>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/ptr_container/serialize_ptr_map.hpp>
#include <boost/shared_ptr.hpp>
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
#include <boost/iterator/filter_iterator.hpp>
#endif
#include <string>
#include <utility>
@ -126,14 +128,6 @@ namespace gtsam {
typedef KeyValuePair value_type;
/** A filtered view of a Values, returned from Values::filter. */
template<class ValueType = Value>
class Filtered;
/** A filtered view of a const Values, returned from Values::filter. */
template<class ValueType = Value>
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<class ValueType>
Values(const Filtered<ValueType>& view);
/** Constructor from a Filtered or ConstFiltered view */
template<class ValueType>
Values(const ConstFiltered<ValueType>& view);
/// @name Testable
/// @{
@ -322,83 +308,6 @@ 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<Value> returned from filter() is only a
* <em>view</em> 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<Value>
filter(const std::function<bool(Key)>& filterFcn);
/**
* 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<Value> returned from filter() is only
* a <em>view</em> 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 (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.
*/
template<class ValueType>
Filtered<ValueType>
filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
/**
* 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<Value> returned from filter() is only a
* <em>view</em> 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<Value>
filter(const std::function<bool(Key)>& 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<Value> returned from filter() is only
* a <em>view</em> 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<class ValueType>
ConstFiltered<ValueType>
filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
// Count values of given type \c ValueType
template<class ValueType>
size_t count() const {
@ -410,6 +319,65 @@ namespace gtsam {
return i;
}
/**
* 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. 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
* 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 (default: always return true so that filter() only
* filters by type, matching \c ValueType).
* @return An Eigen aligned map on Key with the filtered values.
*/
template <class ValueType>
std::map<Key, ValueType> // , std::less<Key>, Eigen::aligned_allocator<ValueType>
extract(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** A filtered view of a Values, returned from Values::filter. */
template <class ValueType = Value>
class Filtered;
/** A filtered view of a const Values, returned from Values::filter. */
template <class ValueType = Value>
class ConstFiltered;
/** Constructor from a Filtered view copies out all values */
template <class ValueType>
Values(const Filtered<ValueType>& view);
/** Constructor from a Filtered or ConstFiltered view */
template <class ValueType>
Values(const ConstFiltered<ValueType>& view);
/// A filtered view of the original Values class.
Filtered<Value> GTSAM_DEPRECATED
filter(const std::function<bool(Key)>& filterFcn);
/// A filtered view of the original Values class, also filter on type.
template <class ValueType>
Filtered<ValueType> GTSAM_DEPRECATED
filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
/// A filtered view of the original Values class, const version.
ConstFiltered<Value> GTSAM_DEPRECATED
filter(const std::function<bool(Key)>& filterFcn) const;
/// A filtered view of the original Values class, also on type, const.
template <class ValueType>
ConstFiltered<ValueType> GTSAM_DEPRECATED filter(
const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
#endif
private:
// Filters based on ValueType (if not Value) and also based on the user-
// supplied \c filter function.

20
gtsam/nonlinear/tests/testValues.cpp
View File

@ -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<Value> filtered = values.filter(std::bind(std::greater_equal<Key>(), 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<Pose3>());
EXPECT_LONGS_EQUAL(2, (long)values.count<Pose2>());
// 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<Pose3>());
EXPECT_LONGS_EQUAL(2, (long)values.count<Pose2>());
// Filter by type using extract.
auto extracted_pose3s = values.extract<Pose3>();
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<Pose3>(Symbol::ChrTest('y'));
EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size());
}
/* ************************************************************************* */

85
gtsam/nonlinear/utilities.h
View File

@ -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<gtsam::Point2> points = values.filter<gtsam::Point2>();
const auto points = values.extract<gtsam::Point2>();
// Point2 is aliased as a gtsam::Vector in the wrapper
Values::ConstFiltered<gtsam::Vector> points2 = values.filter<gtsam::Vector>();
const auto points2 = values.extract<gtsam::Vector>();
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<gtsam::Point3> points = values.filter<gtsam::Point3>();
const auto points = values.extract<gtsam::Point3>();
// Point3 is aliased as a gtsam::Vector in the wrapper
Values::ConstFiltered<gtsam::Vector> points2 = values.filter<gtsam::Vector>();
const auto points2 = values.extract<gtsam::Vector>();
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<Pose2>();
Values result;
for(const auto& key_value: values.extract<Pose2>())
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<Pose2> poses = values.filter<Pose2>();
const auto poses = values.extract<Pose2>();
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<Pose3>();
Values result;
for(const auto& key_value: values.extract<Pose3>())
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<Pose3> poses = values.filter<Pose3>();
const auto poses = values.extract<Pose3>();
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<Vector> vectors =
values.filter<Vector>(Symbol::ChrTest(c));
const auto vectors = values.extract<Vector>(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<Point2>()) {
values.update<Point2>(key_value.key,
key_value.value + Point2(sampler.sample()));
for (const auto& key_value : values.extract<Point2>()) {
values.update<Point2>(key_value.first,
key_value.second + Point2(sampler.sample()));
}
for (const auto& key_value : values.filter<gtsam::Vector>()) {
if (key_value.value.rows() == 2) {
values.update<gtsam::Vector>(key_value.key,
key_value.value + Point2(sampler.sample()));
for (const auto& key_value : values.extract<gtsam::Vector>()) {
if (key_value.second.rows() == 2) {
values.update<gtsam::Vector>(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<Pose2>()) {
values.update<Pose2>(key_value.key, key_value.value.retract(sampler.sample()));
for(const auto& key_value: values.extract<Pose2>()) {
values.update<Pose2>(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<Point3>()) {
values.update<Point3>(key_value.key,
key_value.value + Point3(sampler.sample()));
for (const auto& key_value : values.extract<Point3>()) {
values.update<Point3>(key_value.first,
key_value.second + Point3(sampler.sample()));
}
for (const auto& key_value : values.filter<gtsam::Vector>()) {
if (key_value.value.rows() == 3) {
values.update<gtsam::Vector>(key_value.key,
key_value.value + Point3(sampler.sample()));
for (const auto& key_value : values.extract<gtsam::Vector>()) {
if (key_value.second.rows() == 3) {
values.update<gtsam::Vector>(key_value.first,
key_value.second + Point3(sampler.sample()));
}
}
}

26
gtsam/sfm/ShonanAveraging.cpp
View File

@ -207,9 +207,9 @@ Matrix ShonanAveraging<d>::StiefelElementMatrix(const Values &values) {
const size_t N = values.size();
const size_t p = values.at<SOn>(0).rows();
Matrix S(p, N * d);
for (const auto it : values.filter<SOn>()) {
S.middleCols<d>(it.key * d) =
it.value.matrix().leftCols<d>(); // project Qj to Stiefel
for (const auto& it : values.extract<SOn>()) {
S.middleCols<d>(it.first * d) =
it.second.matrix().leftCols<d>(); // project Qj to Stiefel
}
return S;
}
@ -218,11 +218,11 @@ Matrix ShonanAveraging<d>::StiefelElementMatrix(const Values &values) {
template <>
Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const {
Values result;
for (const auto it : values.filter<SOn>()) {
assert(it.value.rows() == p);
const auto &M = it.value.matrix();
for (const auto& it : values.extract<SOn>()) {
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<SOn>()) {
assert(it.value.rows() == p);
const auto &M = it.value.matrix();
for (const auto& it : values.extract<SOn>()) {
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<d>::cost(const Values &values) const {
}
// Finally, project each dxd rotation block to SO(d)
Values result;
for (const auto it : values.filter<Rot>()) {
result.insert(it.key, SO<d>(it.value.matrix()));
for (const auto& it : values.extract<Rot>()) {
result.insert(it.first, SO<d>(it.second.matrix()));
}
return graph.error(result);
}

4
gtsam/sfm/ShonanAveraging.h
View File

@ -366,8 +366,8 @@ class GTSAM_EXPORT ShonanAveraging {
template <class T>
static Values LiftTo(size_t p, const Values &values) {
Values result;
for (const auto it : values.filter<T>()) {
result.insert(it.key, SOn::Lift(p, it.value.matrix()));
for (const auto it : values.extract<T>()) {
result.insert(it.first, SOn::Lift(p, it.second.matrix()));
}
return result;
}