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

fixed a bug in SPQR calling, and the Victoria Park data-set just flies

release/4.3a0
Kai Ni 2010-06-01 02:20:10 +00:00
parent d6267c0440
commit ebfd979cc4
3 changed files with 42 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
cpp/GaussianFactorGraph.cpp
View File

@ -441,12 +441,14 @@ boost::shared_ptr<VectorConfig> GaussianFactorGraph::conjugateGradientDescent_(
#ifdef USE_SPQR #ifdef USE_SPQR
/* ************************************************************************* */ /* ************************************************************************* */
cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vector<size_t>& orderedDimensions) const { cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vector<size_t>& orderedDimensions,
cholmod_common *cc) const {
Dimensions colIndices; Dimensions colIndices;
size_t numCols; size_t numCols;
boost::tie(colIndices, numCols) = columnIndices_(ordering); boost::tie(colIndices, numCols) = columnIndices_(ordering);
typedef pair<int, int> RowValue; // the pair of row index and non-zero value // sort the data from row ordering to the column ordering
typedef pair<int, double> RowValue; // the pair of row index and non-zero value
int row_start = 0, column_start = 0; int row_start = 0, column_start = 0;
size_t nnz = 0; size_t nnz = 0;
vector<vector<RowValue> > ivs_vector; vector<vector<RowValue> > ivs_vector;
@ -468,28 +470,20 @@ cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vec
} }
// assemble the CHOLMOD data structure // assemble the CHOLMOD data structure
cholmod_sparse* A = new cholmod_sparse(); int numRows = row_start;
long* p = new long[numCols + 1]; // starting and ending index in A->i for each column cholmod_sparse *A = cholmod_l_allocate_sparse(numRows, numCols, nnz, 1, 1, 0, CHOLMOD_REAL, cc);
long* i = new long[nnz]; // row indices of nnz entries long* p = (long*)(A->p); // starting index in A->i for each column
double* x = new double[nnz]; // the values of nnz entries long* p2 = p+1; // ending index in A->i for each column
long* i = (long*)(A->i); // row indices of nnz entries
double* x = (double*)A->x; // the values of nnz entries
p[0] = 0; p[0] = 0;
int p_index = 1;
int i_index = 0;
BOOST_FOREACH(const vector<RowValue>& ivs, ivs_vector) { BOOST_FOREACH(const vector<RowValue>& ivs, ivs_vector) {
p[p_index] = p[p_index-1] + ivs.size(); *(p2++) = *(p++) + ivs.size();
BOOST_FOREACH(const RowValue& iv, ivs) { BOOST_FOREACH(const RowValue& iv, ivs) {
i[i_index] = iv.first; *(i++) = iv.first;
x[i_index++] = iv.second; *(x++) = iv.second;
} }
p_index ++;
} }
A->nrow = row_start; A->ncol = numCols; A->nzmax = nnz;
A->p = p; A->i = i; A->x = x;
A->stype = 0;
A->xtype = CHOLMOD_REAL;
A->dtype = CHOLMOD_DOUBLE;
A->itype = CHOLMOD_LONG;
A->packed = 1; A->sorted = 1;
// order the column indices w.r.t. the given ordering // order the column indices w.r.t. the given ordering
vector<size_t> orderedIndices; vector<size_t> orderedIndices;
@ -508,20 +502,16 @@ cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vec
/* ************************************************************************* */ /* ************************************************************************* */
cholmod_dense* GaussianFactorGraph::cholmodRhs() const { // learn from spqr_mx_get_dense
cholmod_dense* b = new cholmod_dense(); cholmod_dense* GaussianFactorGraph::cholmodRhs(cholmod_common *cc) const {
// compute the number of rows
b->nrow = 0;
BOOST_FOREACH(const sharedFactor& factor,factors_)
b->nrow += factor->numberOfRows();
b->ncol = 1; b->nzmax = b->nrow; b->d = b->nrow; int nrow = 0;
b->xtype = CHOLMOD_REAL; BOOST_FOREACH(const sharedFactor& factor,factors_)
b->dtype = CHOLMOD_DOUBLE; nrow += factor->numberOfRows();
cholmod_dense* b = cholmod_l_allocate_dense(nrow, 1, nrow, CHOLMOD_REAL, cc);
// fill the data // fill the data
double* x = new double[b->nrow]; double* x_current = (double*)b->x;
double* x_current = x;
Vector::const_iterator it_b, it_sigmas, it_b_end; Vector::const_iterator it_b, it_sigmas, it_b_end;
BOOST_FOREACH(const sharedFactor& factor,factors_) { BOOST_FOREACH(const sharedFactor& factor,factors_) {
it_b = factor->get_b().begin(); it_b = factor->get_b().begin();
@ -530,27 +520,31 @@ cholmod_dense* GaussianFactorGraph::cholmodRhs() const {
for(; it_b != it_b_end; ) for(; it_b != it_b_end; )
*(x_current++) = *(it_b++) / *(it_sigmas++); *(x_current++) = *(it_b++) / *(it_sigmas++);
} }
b->x = x;
return b; return b;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig GaussianFactorGraph::optimizeSPQR(const Ordering& ordering) const VectorConfig GaussianFactorGraph::optimizeSPQR(const Ordering& ordering)
{ {
// set up the default parameters // set up the default parameters
cholmod_common Common, *cc ; cholmod_common Common, *cc ;
cc = &Common ; cc = &Common ;
cholmod_l_start(cc) ; cholmod_l_start(cc) ;
cc->metis_memory = 0.0 ;
cc->SPQR_grain = 4 ;
cc->SPQR_small = 1e6 ;
cc->SPQR_nthreads = 2 ; // number of TBB threads (0 = default)
// get the A matrix and rhs in the compress column-format // get the A matrix and rhs in the compress column-format
vector<size_t> orderedDimensions; vector<size_t> orderedDimensions;
cholmod_sparse* A = cholmodSparse(ordering, orderedDimensions); cholmod_sparse* A = cholmodSparse(ordering, orderedDimensions, cc);
cholmod_dense* b = cholmodRhs(); cholmod_dense* b = cholmodRhs(cc);
// QR // QR
double tol = 1e-30; int ord_method = SPQR_ORDERING_BEST; // matlab uses 7
cholmod_dense* x = SuiteSparseQR_min2norm<double> (0, tol, A, b, cc) ; double tol = SPQR_NO_TOL; // matlab uses SPQR_DEFAULT_TOL
cholmod_dense* x = SuiteSparseQR<double> (ord_method, tol, A, b, cc) ;
// create the update vector // create the update vector
VectorConfig config; VectorConfig config;

7
cpp/GaussianFactorGraph.h
View File

@ -270,18 +270,19 @@ namespace gtsam {
* Convert to CHOLMOD's compressed-column form, refer to CHOLMOD's user guide for details. * Convert to CHOLMOD's compressed-column form, refer to CHOLMOD's user guide for details.
* The returned pointer needs to be free by calling cholmod_l_free_sparse * The returned pointer needs to be free by calling cholmod_l_free_sparse
*/ */
cholmod_sparse* cholmodSparse(const Ordering& ordering, std::vector<std::size_t>& orderedDimensions) const; cholmod_sparse* cholmodSparse(const Ordering& ordering, std::vector<std::size_t>& orderedDimensions,
cholmod_common *cc) const;
/** /**
* Convert the RHS to CHOLMOD's column-major matrix format * Convert the RHS to CHOLMOD's column-major matrix format
* The returned pointer needs to be free by calling cholmod_l_free_sparse * The returned pointer needs to be free by calling cholmod_l_free_sparse
*/ */
cholmod_dense* cholmodRhs() const; cholmod_dense* cholmodRhs(cholmod_common *cc) const;
/** /**
* optimizing using SparseQR package * optimizing using SparseQR package
*/ */
VectorConfig optimizeSPQR(const Ordering& ordering) const; VectorConfig optimizeSPQR(const Ordering& ordering);
#endif #endif
}; };
} // namespace gtsam } // namespace gtsam

16
cpp/testGaussianFactorGraph.cpp
View File

@ -422,7 +422,10 @@ TEST( GaussianFactorGraph, cholmodSparse )
Ordering ord; ord += "x2","l1","x1"; Ordering ord; ord += "x2","l1","x1";
vector<size_t> dimensions; vector<size_t> dimensions;
cholmod_sparse* A = fg.cholmodSparse(ord, dimensions); cholmod_common Common, *cc ;
cc = &Common ;
cholmod_l_start (cc) ;
cholmod_sparse* A = fg.cholmodSparse(ord, dimensions, cc);
CHECK(A->ncol == 6); CHECK(A->ncol == 6);
CHECK(A->nrow == 8); CHECK(A->nrow == 8);
@ -446,9 +449,6 @@ TEST( GaussianFactorGraph, cholmodSparse )
for(int index=0; index<14; index++) for(int index=0; index<14; index++)
CHECK(x[index] == *((double*)A->x + index)); CHECK(x[index] == *((double*)A->x + index));
cholmod_common Common, *cc ;
cc = &Common ;
cholmod_l_start (cc) ;
cholmod_l_free_sparse(&A, cc); cholmod_l_free_sparse(&A, cc);
cholmod_l_finish(cc) ; cholmod_l_finish(cc) ;
} }
@ -459,7 +459,10 @@ TEST( GaussianFactorGraph, cholmodRhs )
// create a small linear factor graph // create a small linear factor graph
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
cholmod_dense* b = fg.cholmodRhs(); cholmod_common Common, *cc;
cc = &Common;
cholmod_l_start (cc);
cholmod_dense* b = fg.cholmodRhs(cc);
CHECK(b->ncol == 1); CHECK(b->ncol == 1);
CHECK(b->nrow == 8); CHECK(b->nrow == 8);
@ -472,9 +475,6 @@ TEST( GaussianFactorGraph, cholmodRhs )
for(int index=0; index<8; index++) for(int index=0; index<8; index++)
CHECK(x[index] == *((double*)b->x + index)); CHECK(x[index] == *((double*)b->x + index));
cholmod_common Common, *cc;
cc = &Common;
cholmod_l_start (cc);
cholmod_l_free_dense(&b, cc); cholmod_l_free_dense(&b, cc);
cholmod_l_finish(cc); cholmod_l_finish(cc);
} }