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

added a variant of getOrdering

release/4.3a0
Kai Ni 2010-05-24 08:57:22 +00:00
parent 981082e2a4
commit c70e818a79
3 changed files with 29 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cpp/FactorGraph-inl.h
View File

@ -216,7 +216,7 @@ void colamd(int n_col, int n_row, int nrNonZeros, const map<Key, vector<int> >&
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
void FactorGraph<Factor>::getOrdering(Ordering& ordering) const{ void FactorGraph<Factor>::getOrdering(Ordering& ordering, boost::optional<const set<Symbol>&> interested) const{
// A factor graph is really laid out in row-major format, each factor a row // 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. // Below, we compute a symbolic matrix stored in sparse columns.
@ -225,11 +225,16 @@ void FactorGraph<Factor>::getOrdering(Ordering& ordering) const{
int n_row = factors_.size(); /* colamd arg 1: number of rows in A */ int n_row = factors_.size(); /* colamd arg 1: number of rows in A */
// loop over all factors = rows // loop over all factors = rows
bool hasInterested = interested.is_initialized();
for (int i = 0; i < n_row; i++) { for (int i = 0; i < n_row; i++) {
if (factors_[i]==NULL) continue; if (factors_[i]==NULL) continue;
list<Symbol> keys = factors_[i]->keys(); list<Symbol> keys = factors_[i]->keys();
BOOST_FOREACH(const Symbol& key, keys) columns[key].push_back(i); BOOST_FOREACH(const Symbol& key, keys) {
nrNonZeros+= keys.size(); 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 */ int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */
if(n_col != 0) if(n_col != 0)
@ -253,6 +258,14 @@ Ordering FactorGraph<Factor>::getOrdering() const {
return ordering; return ordering;
} }
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::getOrdering(const set<Symbol>& interested) const {
Ordering ordering;
getOrdering(ordering, interested);
return ordering;
}
/* ************************************************************************* */ /* ************************************************************************* */
/** O(1) */ /** O(1) */
/* ************************************************************************* */ /* ************************************************************************* */

3
cpp/FactorGraph.h
View File

@ -106,8 +106,9 @@ namespace gtsam {
/** /**
* Compute colamd ordering, including I/O and shared pointer version * Compute colamd ordering, including I/O and shared pointer version
*/ */
void getOrdering(Ordering& ordering) const; void getOrdering(Ordering& ordering, boost::optional<const std::set<Symbol>&> interested = boost::none) const;
Ordering getOrdering() const; Ordering getOrdering() const;
Ordering getOrdering(const std::set<Symbol>& interested) const;
boost::shared_ptr<Ordering> getOrdering_() const; boost::shared_ptr<Ordering> getOrdering_() const;
/** /**

11
cpp/testGaussianFactorGraph.cpp
View File

@ -11,6 +11,7 @@ using namespace std;
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator += #include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
@ -432,6 +433,16 @@ TEST( GaussianFactorGraph, getOrdering)
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
TEST( GaussianFactorGraph, getOrdering2)
{
Ordering expected;
expected += "l1","x1";
GaussianFactorGraph fg = createGaussianFactorGraph();
set<Symbol> interested; interested += "l1","x1";
Ordering actual = fg.getOrdering(interested);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, optimize ) TEST( GaussianFactorGraph, optimize )
{ {