diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index b988e9aa2..9934f80ed 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -216,7 +216,7 @@ void colamd(int n_col, int n_row, int nrNonZeros, const map >& /* ************************************************************************* */ template -void FactorGraph::getOrdering(Ordering& ordering) const{ +void FactorGraph::getOrdering(Ordering& ordering, boost::optional&> interested) const{ // A factor graph is really laid out in row-major format, each factor a row // Below, we compute a symbolic matrix stored in sparse columns. @@ -225,11 +225,16 @@ void FactorGraph::getOrdering(Ordering& ordering) const{ int n_row = factors_.size(); /* colamd arg 1: number of rows in A */ // loop over all factors = rows + bool hasInterested = interested.is_initialized(); for (int i = 0; i < n_row; i++) { if (factors_[i]==NULL) continue; list keys = factors_[i]->keys(); - BOOST_FOREACH(const Symbol& key, keys) columns[key].push_back(i); - nrNonZeros+= keys.size(); + BOOST_FOREACH(const Symbol& key, keys) { + if (!hasInterested || interested->find(key) != interested->end()) { + columns[key].push_back(i); + nrNonZeros++; + } + } } int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */ if(n_col != 0) @@ -253,6 +258,14 @@ Ordering FactorGraph::getOrdering() const { return ordering; } +/* ************************************************************************* */ +template +Ordering FactorGraph::getOrdering(const set& interested) const { + Ordering ordering; + getOrdering(ordering, interested); + return ordering; +} + /* ************************************************************************* */ /** O(1) */ /* ************************************************************************* */ diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 6487f4b08..2c4034ac2 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -106,8 +106,9 @@ namespace gtsam { /** * Compute colamd ordering, including I/O and shared pointer version */ - void getOrdering(Ordering& ordering) const; + void getOrdering(Ordering& ordering, boost::optional&> interested = boost::none) const; Ordering getOrdering() const; + Ordering getOrdering(const std::set& interested) const; boost::shared_ptr getOrdering_() const; /** diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 0a10cc4dc..d5e9e2e79 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -11,6 +11,7 @@ using namespace std; #include #include #include // for operator += +#include // for operator += #include // for operator += using namespace boost::assign; @@ -432,6 +433,16 @@ TEST( GaussianFactorGraph, getOrdering) CHECK(assert_equal(expected,actual)); } +TEST( GaussianFactorGraph, getOrdering2) +{ + Ordering expected; + expected += "l1","x1"; + GaussianFactorGraph fg = createGaussianFactorGraph(); + set interested; interested += "l1","x1"; + Ordering actual = fg.getOrdering(interested); + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ TEST( GaussianFactorGraph, optimize ) {