diff --git a/Makefile.am b/Makefile.am index b496ab80e..3c9b964b3 100644 --- a/Makefile.am +++ b/Makefile.am @@ -14,6 +14,7 @@ install-exec-hook: install -d $(prefix)/include && \ install -d $(prefix)/include/colamd && \ cp -f colamd/colamd.h $(prefix)/include/colamd/ && \ + cp -f colamd/ccolamd.h $(prefix)/include/colamd/ && \ cp -f colamd/UFconfig.h $(prefix)/include/colamd/ && \ cp -f colamd/libcolamd.a $(prefix)/lib/ && \ install -d $(prefix)/include/ldl && \ diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index db802dd87..c0cbeb371 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -17,6 +17,7 @@ extern "C" { #include +#include } #include @@ -180,37 +181,57 @@ std::pair, set > FactorGraph::removeSingleto * @param n_row colamd arg 2: number of columns in A * @param nrNonZeros number of non-zero entries in A * @param columns map from keys to a sparse column of non-zero row indices + * @param lastKeys set of keys that should appear last in the ordering */ template -void colamd(int n_col, int n_row, int nrNonZeros, const map >& columns, Ordering& ordering) { +void colamd(int n_col, int n_row, int nrNonZeros, const map >& columns, + Ordering& ordering, const set& lastKeys) { // Convert to compressed column major format colamd wants it in (== MATLAB format!) vector initialOrder; - int Alen = nrNonZeros*30; /* colamd arg 3: size of the array A TODO: use Tim's function ! */ +// int Alen = nrNonZeros*30; /* colamd arg 3: size of the array A TODO: use Tim's function ! */ + int Alen = ccolamd_recommended(nrNonZeros, n_row, n_col); /* colamd arg 3: size of the array A */ int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */ int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */ + int * cmember = new int[n_col]; /* Constraint set of A, of size n_col */ p[0] = 0; int j = 1; int count = 0; typedef typename map >::const_iterator iterator; + bool front_exists = false; for(iterator it = columns.begin(); it != columns.end(); it++) { const Key& key = it->first; const vector& column = it->second; initialOrder.push_back(key); BOOST_FOREACH(int i, column) A[count++] = i; // copy sparse column p[j] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + if (lastKeys.find(key)==lastKeys.end()) { + cmember[j-1] = 0; + front_exists = true; + } else { + cmember[j-1] = 1; // force lastKeys to be at the end + } j+=1; } + if (!front_exists) { // if only 1 entries, set everything to 0... + for(int j = 0; j < n_col; j++) + cmember[j] = 0; + } double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ int stats[COLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ // call colamd, result will be in p ************************************************* /* TODO: returns (1) if successful, (0) otherwise*/ +#if 0 ::colamd(n_row, n_col, Alen, A, p, knobs, stats); +#else + ::ccolamd(n_row, n_col, Alen, A, p, knobs, stats, cmember); +#endif // ********************************************************************************** delete [] A; // delete symbolic A + delete [] cmember; // Convert elimination ordering in p to an ordering for(int j = 0; j < n_col; j++) @@ -220,7 +241,8 @@ void colamd(int n_col, int n_row, int nrNonZeros, const map >& /* ************************************************************************* */ template -void FactorGraph::getOrdering(Ordering& ordering, boost::optional&> interested) const{ +void FactorGraph::getOrdering(Ordering& ordering, const set& lastKeys, + 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. @@ -246,7 +268,7 @@ void FactorGraph::getOrdering(Ordering& ordering, boost::optional::getOrdering(Ordering& ordering, boost::optional boost::shared_ptr FactorGraph::getOrdering_() const{ boost::shared_ptr ordering(new Ordering); - getOrdering(*ordering); + set lastKeys; + getOrdering(*ordering, lastKeys); return ordering; } @@ -262,7 +285,8 @@ boost::shared_ptr FactorGraph::getOrdering_() const{ template Ordering FactorGraph::getOrdering() const { Ordering ordering; - getOrdering(ordering); + set lastKeys; + getOrdering(ordering, lastKeys); return ordering; } @@ -270,7 +294,15 @@ Ordering FactorGraph::getOrdering() const { template Ordering FactorGraph::getOrdering(const set& interested) const { Ordering ordering; - getOrdering(ordering, interested); + set lastKeys; + getOrdering(ordering, lastKeys, interested); + return ordering; +} + +template +Ordering FactorGraph::getConstrainedOrdering(const set& lastKeys) const { + Ordering ordering; + getOrdering(ordering, lastKeys); return ordering; } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 2c4034ac2..c633eba33 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -106,9 +106,10 @@ namespace gtsam { /** * Compute colamd ordering, including I/O and shared pointer version */ - void getOrdering(Ordering& ordering, boost::optional&> interested = boost::none) const; + void getOrdering(Ordering& ordering, const std::set& lastKeys, boost::optional&> interested = boost::none) const; Ordering getOrdering() const; Ordering getOrdering(const std::set& interested) const; + Ordering getConstrainedOrdering(const std::set& lastKeys) const; boost::shared_ptr getOrdering_() const; /** diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index f5c1e0940..e8802bf34 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -226,7 +226,10 @@ namespace gtsam { //// 8 - eliminate and add orphans back in // create an ordering for the new and contaminated factors - Ordering ordering = factors->getOrdering(); + // newKeys are passed in: those variables will be forced to the end in the ordering + set newKeysSet; + newKeysSet.insert(newKeys.begin(), newKeys.end()); + Ordering ordering = factors->getConstrainedOrdering(newKeysSet); // eliminate into a Bayes net BayesNet bayesNet = _eliminate(*factors, cached_, ordering);