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

re-factored and commented getOrdering

release/4.3a0
Frank Dellaert 2009-09-12 01:50:47 +00:00
parent 597af891cb
commit a5ae9c12ef
1 changed files with 48 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
cpp/LinearFactorGraph.cpp
View File

@ -12,6 +12,7 @@
#include <colamd/colamd.h> #include <colamd/colamd.h>
#include "ChordalBayesNet.h"
#include "LinearFactorGraph.h" #include "LinearFactorGraph.h"
using namespace std; using namespace std;
@ -228,83 +229,64 @@ pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Ordering LinearFactorGraph::getOrdering() const { Ordering LinearFactorGraph::getOrdering() const {
// FD: no comments whatsoever, must be totally obvious ! // 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.
typedef string Key; // default case with string keys
map<Key, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
int nrNonZeros = 0; // number of non-zero entries
int n_row = factors_.size(); /* colamd arg 1: number of rows in A */
int * _symbolicMatrix; // loop over all factors = rows
int * _symbolicColumns; for (int i = 0; i < n_row; i++) {
int _symbolicLength; shared_factor factor = factors_[i];
int _symbolicColLength; for (map<Key, Matrix>::const_iterator lit = factor->begin(); lit != factor->end(); lit++) {
std::vector<int> _symbolicElimintationOrder; const Key& key = lit->first;
columns[key].push_back(i);
Ordering result; nrNonZeros++;
map<string, vector<int> > symbolToMatrixElement;
_symbolicLength = 0;
_symbolicColLength = 0;
int matrixRow = 0;
int symNRows = 0;
int symNCols = 0;
for(vector<LinearFactor::shared_ptr>::const_iterator it = begin(); it != end(); it++)
{
symNRows++;
for(map<string, Matrix>::const_iterator lit = (*it)->begin(); lit != (*it)->end(); lit++)
{
_symbolicLength++;
symbolToMatrixElement[lit->first].push_back(matrixRow);
} }
matrixRow++;
} }
int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */
if(n_col == 0) return Ordering(); // empty ordering
symNCols = (int)(symbolToMatrixElement.size()); // Now convert to compressed column major format colamd wants it in (== MATLAB format!)
_symbolicColLength = symNCols + 1; vector<Key> initialOrder;
int Alen = nrNonZeros*30; /* colamd arg 3: size of the array A */
if(symNCols == 0) {result.clear(); return result;} int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */
//printf("%d %d\n", _symbolicLength, _symbolicColLength); int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */
{
_symbolicMatrix = new int[_symbolicLength]; p[0] = 0;
_symbolicColumns = new int[_symbolicColLength]; int j = 1;
int count = 0; int count = 0;
_symbolicColumns[0] = 0; for(map<Key, vector<int> >::const_iterator it = columns.begin(); it != columns.end(); it++)
int colCount = 1;
for(map<string, vector<int> >::iterator it = symbolToMatrixElement.begin(); it != symbolToMatrixElement.end(); it++)
{ {
for(vector<int>::iterator rit = it->second.begin(); rit != it->second.end(); rit++) const Key& key = it->first;
_symbolicMatrix[count++] = (*rit); const vector<int>& column = it->second;
_symbolicColumns[colCount++] = count; 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
j+=1;
} }
vector<string> initialOrder;
for(map<string, vector<int> >::iterator it = symbolToMatrixElement.begin(); it != symbolToMatrixElement.end(); it++)
initialOrder.push_back(it->first);
int * tempColumnOrdering = new int[_symbolicColLength];
for(int i = 0; i < _symbolicColLength; i++) tempColumnOrdering[i] = _symbolicColumns[i];
int * tempSymbolicMatrix = new int[_symbolicLength*30];
for(int i = 0; i < _symbolicLength; i++) tempSymbolicMatrix[i] = _symbolicMatrix[i];
int stats [COLAMD_STATS] ;
//for(int i = 0; i < _symbolicColLength; i++) printf("!%d\n", tempColumnOrdering[i]);
colamd(symNRows, symNCols, _symbolicLength*30, tempSymbolicMatrix, tempColumnOrdering, (double *) NULL, stats) ;
_symbolicElimintationOrder.clear();
for(int i = 0; i < _symbolicColLength; i++)
_symbolicElimintationOrder.push_back(tempColumnOrdering[i]);
//for(int i = 0; i < _symbolicColLength; i++) printf("!%d\n", tempColumnOrdering[i]);
delete [] tempColumnOrdering;
delete [] tempSymbolicMatrix;
result.clear();
for(vector<int>::const_iterator it = _symbolicElimintationOrder.begin(); it != _symbolicElimintationOrder.end(); it++)
{
//printf("%d\n", (*it));
if((*it) == -1) continue;
result.push_back(initialOrder[(*it)]);
} }
delete [] _symbolicMatrix; double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
delete [] _symbolicColumns; 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*/
colamd(n_row, n_col, Alen, A, p, knobs, stats);
// **********************************************************************************
delete [] A; // delete symbolic A
// Convert elimination ordering in p to an ordering
Ordering result;
for(int j = 0; j < n_col; j++)
result.push_back(initialOrder[j]);
delete [] p; // delete colamd result vector
return result; return result;
} }
/* ************************************************************************* */