diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index b20a9257b..e0d233e9f 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -15,6 +15,7 @@ namespace ublas = boost::numeric::ublas; // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) +using namespace std; using namespace gtsam; typedef pair& mypair; @@ -75,6 +76,15 @@ bool LinearFactor::equals(const Factor& f, double tol) const { return false; } +/* ************************************************************************* */ +set LinearFactor::keys() const { + set result; + string j; Matrix A; + FOREACH_PAIR(j,A,As) + result.insert(j); + return result; +} + /* ************************************************************************* */ VariableSet LinearFactor::variables() const { VariableSet result; diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h index 47b8b0475..c1ee42b42 100644 --- a/cpp/LinearFactor.h +++ b/cpp/LinearFactor.h @@ -144,6 +144,12 @@ public: */ int numberOfRows() const { return b.size();} + /** + * Find all variables + * @return The set of all variable keys + */ + std::set keys() const; + /** * Find all variables and their dimensions * @return The set of all variable/dimension pairs diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index 93f73534c..74abc5d1b 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -42,6 +42,18 @@ TEST( LinearFactor, linearFactor ) CHECK( lf->equals(expected) ); } +/* ************************************************************************* */ +TEST( LinearFactor, keys ) +{ + // get the factor "f2" from the small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + LinearFactor::shared_ptr lf = fg[1]; + set expected; + expected.insert("x1"); + expected.insert("x2"); + CHECK(lf->keys() == expected); +} + /* ************************************************************************* */ TEST( LinearFactor, variables ) {