diff --git a/.cproject b/.cproject index e75e9b8e8..f54554f76 100644 --- a/.cproject +++ b/.cproject @@ -287,7 +287,7 @@ - + make -j2 check @@ -295,7 +295,15 @@ true true - + + make + -j2 + tests/testSPQRUtil.run + true + true + true + + make -j2 clean @@ -303,162 +311,10 @@ true true - + make -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - tests/testDSFVector.run - true - true - true - - - make - -j2 - tests/testTestableAssertions.run - true - true - true - - - make - -j2 - tests/testVector.run - true - true - true - - - make - -j2 - tests/testMatrix.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testNumericalDerivative.run - true - true - true - - - make - -j2 - tests/testBlockMatrices.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - tests/testCholesky.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run - true - true - true - - - make - -j2 - vSFMexample.run + testGaussianFactor.run true true true @@ -489,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -497,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -545,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -553,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -561,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -577,12 +428,291 @@ make - tests/testBayesTree true false true + + make + -j2 + check + true + true + true + + + make + -j2 + testGaussianFactorGraph.run + true + true + true + + + make + -j2 + testGaussianISAM.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + -j2 + testIterative.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testNonlinearFactor.run + true + true + true + + + make + -j2 + testNonlinearFactorGraph.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testSubgraphPreconditioner.run + true + true + true + + + make + -j2 + testTupleConfig.run + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + + testInference.run + true + false + true + + + make + + testGaussianFactor.run + true + false + true + + + make + + testJunctionTree.run + true + false + true + + + make + + testSymbolicBayesNet.run + true + false + true + + + make + + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testGaussianJunctionTree + true + true + true + + + make + -j2 + testSerialization.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -623,7 +753,7 @@ true true - + make -j2 check @@ -631,22 +761,6 @@ true true - - make - -j2 - tests/testSPQRUtil.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -679,69 +793,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -766,6 +817,30 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + make -j2 @@ -918,87 +993,23 @@ true true - + make -j2 - testRot3.run + SimpleRotation.run true true true - + make -j2 - testRot2.run + all true true true - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - + make -j2 clean @@ -1006,15 +1017,7 @@ true true - - make - -j2 - testPoint2.run - true - true - true - - + make -j2 check @@ -1022,39 +1025,78 @@ true true - + make -j2 - tests/testVectorValues.run + testGaussianConditional.run true true true - + make -j2 - tests/testNoiseModel.run + testGaussianFactor.run true true true - + make -j2 - tests/testGaussianFactor.run + timeGaussianFactor.run true true true - + make -j2 - tests/testHessianFactor.run + timeVectorConfig.run true true true - + + make + -j2 + testVectorBTree.run + true + true + true + + + make + -j2 + testVectorMap.run + true + true + true + + + make + -j2 + testNoiseModel.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + make + testErrors.run + true + false + true + + make -j2 check @@ -1062,23 +1104,7 @@ true true - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/testGaussianFactorGraph.run - true - true - true - - + make -j2 tests/testGaussianJunctionTree.run @@ -1086,7 +1112,39 @@ true true - + + make + -j2 + tests/testGaussianFactor.run + true + true + true + + + make + -j2 + tests/testGaussianConditional.run + true + true + true + + + make + -j2 + tests/timeSLAMlike.run + true + true + true + + + make + -j2 + check + true + true + true + + make -j2 clean @@ -1094,18 +1152,170 @@ true true - + make -j2 - tests/testKalmanFilter.run + testBTree.run true true true - + make -j2 - tests/testGaussianDensity.run + testDSF.run + true + true + true + + + make + -j2 + testDSFVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run true true true @@ -1126,6 +1336,278 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testStereoCamera.run + true + true + true + + + make + -j2 + tests/testRot3M.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j2 + tests/testPoint3.run + true + true + true + + + make + -j2 + tests/testCalibratedCamera.run + true + true + true + + + make + -j2 + tests/timeRot3.run + true + true + true + + + make + -j2 + tests/timePose3.run + true + true + true + + + make + -j2 + tests/timeStereoCamera.run + true + true + true + + + make + -j2 + tests/testPoint2.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPlanarSLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testPose3Factor.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + + testSimulated3D.run + true + false + true + + + make + -j2 + tests/testGaussianISAM2 + true + true + true + cmake .. @@ -1493,15 +1975,39 @@ true true - + make -j2 - testGaussianFactor.run + tests/testDSFVector.run true true true - + + make + -j2 + tests/testTestableAssertions.run + true + true + true + + + make + -j2 + tests/testVector.run + true + true + true + + + make + -j2 + tests/testMatrix.run + true + true + true + + make -j2 check @@ -1509,31 +2015,23 @@ true true - + make -j2 - install + tests/testNumericalDerivative.run true true true - + make -j2 - tests/testSpirit.run + tests/testBlockMatrices.run true true true - - make - -j2 - tests/testWrap.run - true - true - true - - + make -j2 clean @@ -1541,47 +2039,31 @@ true true - + make -j2 - all + tests/testCholesky.run true true true - + make -j2 - all + tests/testVectorValues.run true true true - + make -j2 - clean + tests/testNoiseModel.run true true true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - + make -j2 tests/testGaussianFactor.run @@ -1589,7 +2071,23 @@ true true - + + make + -j2 + tests/testHessianFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + make -j2 tests/testGaussianConditional.run @@ -1597,127 +2095,23 @@ true true - + make -j2 - tests/timeSLAMlike.run + tests/testGaussianFactorGraph.run true true true - + make -j2 - tests/testPose2.run + tests/testGaussianJunctionTree.run true true true - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - tests/testStereoCamera.run - true - true - true - - - make - -j2 - tests/testRot3M.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - tests/testPoint3.run - true - true - true - - - make - -j2 - tests/testCalibratedCamera.run - true - true - true - - - make - -j2 - tests/timeRot3.run - true - true - true - - - make - -j2 - tests/timePose3.run - true - true - true - - - make - -j2 - tests/timeStereoCamera.run - true - true - true - - - make - -j2 - tests/testPoint2.run - true - true - true - - - make - -j2 - check - true - true - true - - + make -j2 clean @@ -1725,608 +2119,18 @@ true true - + make -j2 - install + tests/testKalmanFilter.run true true true - + make -j2 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testGaussianFactorGraph.run - true - true - true - - - make - -j2 - testGaussianISAM.run - true - true - true - - - make - testGraph.run - true - false - true - - - make - -j2 - testIterative.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testNonlinearFactor.run - true - true - true - - - make - -j2 - testNonlinearFactorGraph.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testSubgraphPreconditioner.run - true - true - true - - - make - -j2 - testTupleConfig.run - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - testInference.run - true - false - true - - - make - testGaussianFactor.run - true - false - true - - - make - testJunctionTree.run - true - false - true - - - make - testSymbolicBayesNet.run - true - false - true - - - make - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testGaussianJunctionTree - true - true - true - - - make - -j2 - testSerialization.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testGaussianConditional.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - timeGaussianFactor.run - true - true - true - - - make - -j2 - timeVectorConfig.run - true - true - true - - - make - -j2 - testVectorBTree.run - true - true - true - - - make - -j2 - testVectorMap.run - true - true - true - - - make - -j2 - testNoiseModel.run - true - true - true - - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - - testErrors.run - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPlanarSLAM.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - testPose2Prior.run - true - true - true - - - make - -j2 - testPose2SLAM.run - true - true - true - - - make - -j2 - testPose3Config.run - true - true - true - - - make - -j2 - testPose3SLAM.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - -j2 - tests/testGaussianISAM2 + tests/testGaussianDensity.run true true true @@ -2411,6 +2215,38 @@ true true + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run + true + true + true + make -j2 @@ -2454,20 +2290,99 @@ cmake .. - true false true - + make - -j2 - all + + nonlinear.testValues.run true true true - + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + make -j2 clean @@ -2475,6 +2390,101 @@ true true + + make + -j2 + testPoint2.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 08bf9edca..745498c29 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -34,6 +34,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -82,20 +86,11 @@ namespace gtsam { /// A shared_ptr to this class typedef boost::shared_ptr shared_ptr; - /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator - struct KeyValuePair { - const Key first; - Value& second; - KeyValuePair(Key key, Value& value) : first(key), second(value) {} - }; + /// A key-value pair, which you get by dereferencing iterators + typedef std::pair KeyValuePair; - /// 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) {} - }; + /// A key-value pair, which you get by dereferencing iterators + typedef std::pair ConstKeyValuePair; /// Mutable forward iterator, with value type KeyValuePair typedef boost::transform_iterator< @@ -113,26 +108,40 @@ 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; + class Filtered : public boost::transformed_range< + std::pair(*)(KeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > { + private: + typedef boost::transformed_range< + std::pair(*)(KeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > Base; + + Filtered(const Base& base) : Base(base) {} + + friend class Values; }; - /// Const type-filtered iterator template - struct const_type_filter_iterator { - typedef boost::transform_iterator< - std::pair(*)(ConstKeyValuePair), - boost::filter_iterator > type; + class ConstFiltered : public boost::transformed_range< + std::pair(*)(ConstKeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > { + private: + typedef boost::transformed_range< + std::pair(*)(ConstKeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > Base; + + ConstFiltered(const Base& base) : Base(base) {} + + friend class Values; }; /** Default constructor creates an empty Values class */ @@ -215,86 +224,6 @@ 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 /// @{ @@ -350,8 +279,118 @@ namespace gtsam { */ Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) 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 boost::function& filterFcn) { + return filter(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 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 (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 + Filtered + filter(const boost::function& filterFcn = (boost::lambda::_1, true)) { + return boost::adaptors::transform( + boost::adaptors::filter( + boost::make_iterator_range(begin(), end()), + boost::function( + boost::bind(&filterHelper, filterFcn, _1))), + &castHelper); + } + + /** + * 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 + filter(const boost::function& filterFcn) const { + return filter(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 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 boost::function& filterFcn = (boost::lambda::_1, true)) const { + return boost::adaptors::transform( + boost::adaptors::filter( + boost::make_iterator_range(begin(), end()), + boost::function( + boost::bind(&filterHelper, filterFcn, _1))), + &castHelper); + } + private: - /** Serialization function */ + // Filters based on ValueType (if not Value) and also based on the user- + // supplied \c filter function. + template + static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + // Filter and check the type + return filter(key_value.first) && (typeid(ValueType) == typeid(key_value.second) || typeid(ValueType) == typeid(Value)); + } + + // Cast to the derived ValueType + template + static std::pair castHelper(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + return std::pair(key_value.first, static_cast(key_value.second)); + } + + /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 55854b339..bdf3e1191 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -273,35 +273,36 @@ TEST(Values, filter) { // 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) { + Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + BOOST_FOREACH(const Values::Filtered::value_type& key_value, filtered) { if(i == 0) { - LONGS_EQUAL(2, it->first); - EXPECT(typeid(Pose2) == typeid(it->second)); - EXPECT(assert_equal(pose2, dynamic_cast(it->second))); + LONGS_EQUAL(2, key_value.first); + EXPECT(typeid(Pose2) == typeid(key_value.second)); + EXPECT(assert_equal(pose2, dynamic_cast(key_value.second))); } else if(i == 1) { - LONGS_EQUAL(3, it->first); - EXPECT(typeid(Pose3) == typeid(it->second)); - EXPECT(assert_equal(pose3, dynamic_cast(it->second))); + LONGS_EQUAL(3, key_value.first); + EXPECT(typeid(Pose3) == typeid(key_value.second)); + EXPECT(assert_equal(pose3, dynamic_cast(key_value.second))); } else { EXPECT(false); } + ++ i; } LONGS_EQUAL(2, i); // Filter by type i = 0; - for(Values::type_filter_iterator::type it = values.beginFilterByType(); - it != values.endFilterByType(); ++it, ++i) { + BOOST_FOREACH(const Values::Filtered::value_type& key_value, values.filter()) { if(i == 0) { - LONGS_EQUAL(1, it->first); - EXPECT(assert_equal(pose1, it->second)); + LONGS_EQUAL(1, key_value.first); + EXPECT(assert_equal(pose1, key_value.second)); } else if(i == 1) { - LONGS_EQUAL(3, it->first); - EXPECT(assert_equal(pose3, it->second)); + LONGS_EQUAL(3, key_value.first); + EXPECT(assert_equal(pose3, key_value.second)); } else { EXPECT(false); } + ++ i; } LONGS_EQUAL(2, i); }