diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 394728bee..9e766b166 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -441,12 +441,14 @@ boost::shared_ptr GaussianFactorGraph::conjugateGradientDescent_( #ifdef USE_SPQR /* ************************************************************************* */ -cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vector& orderedDimensions) const { +cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vector& orderedDimensions, + cholmod_common *cc) const { Dimensions colIndices; size_t numCols; boost::tie(colIndices, numCols) = columnIndices_(ordering); - typedef pair RowValue; // the pair of row index and non-zero value + // sort the data from row ordering to the column ordering + typedef pair RowValue; // the pair of row index and non-zero value int row_start = 0, column_start = 0; size_t nnz = 0; vector > ivs_vector; @@ -468,28 +470,20 @@ cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vec } // assemble the CHOLMOD data structure - cholmod_sparse* A = new cholmod_sparse(); - long* p = new long[numCols + 1]; // starting and ending index in A->i for each column - long* i = new long[nnz]; // row indices of nnz entries - double* x = new double[nnz]; // the values of nnz entries + int numRows = row_start; + cholmod_sparse *A = cholmod_l_allocate_sparse(numRows, numCols, nnz, 1, 1, 0, CHOLMOD_REAL, cc); + long* p = (long*)(A->p); // starting index in A->i for each column + 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; - int p_index = 1; - int i_index = 0; BOOST_FOREACH(const vector& ivs, ivs_vector) { - p[p_index] = p[p_index-1] + ivs.size(); + *(p2++) = *(p++) + ivs.size(); BOOST_FOREACH(const RowValue& iv, ivs) { - i[i_index] = iv.first; - x[i_index++] = iv.second; + *(i++) = iv.first; + *(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 vector orderedIndices; @@ -508,20 +502,16 @@ cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vec /* ************************************************************************* */ -cholmod_dense* GaussianFactorGraph::cholmodRhs() const { - cholmod_dense* b = new cholmod_dense(); - // compute the number of rows - b->nrow = 0; - BOOST_FOREACH(const sharedFactor& factor,factors_) - b->nrow += factor->numberOfRows(); +// learn from spqr_mx_get_dense +cholmod_dense* GaussianFactorGraph::cholmodRhs(cholmod_common *cc) const { - b->ncol = 1; b->nzmax = b->nrow; b->d = b->nrow; - b->xtype = CHOLMOD_REAL; - b->dtype = CHOLMOD_DOUBLE; + int nrow = 0; + BOOST_FOREACH(const sharedFactor& factor,factors_) + nrow += factor->numberOfRows(); + cholmod_dense* b = cholmod_l_allocate_dense(nrow, 1, nrow, CHOLMOD_REAL, cc); // fill the data - double* x = new double[b->nrow]; - double* x_current = x; + double* x_current = (double*)b->x; Vector::const_iterator it_b, it_sigmas, it_b_end; BOOST_FOREACH(const sharedFactor& factor,factors_) { it_b = factor->get_b().begin(); @@ -530,27 +520,31 @@ cholmod_dense* GaussianFactorGraph::cholmodRhs() const { for(; it_b != it_b_end; ) *(x_current++) = *(it_b++) / *(it_sigmas++); } - b->x = x; return b; } /* ************************************************************************* */ -VectorConfig GaussianFactorGraph::optimizeSPQR(const Ordering& ordering) const +VectorConfig GaussianFactorGraph::optimizeSPQR(const Ordering& ordering) { // set up the default parameters cholmod_common Common, *cc ; cc = &Common ; 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 vector orderedDimensions; - cholmod_sparse* A = cholmodSparse(ordering, orderedDimensions); - cholmod_dense* b = cholmodRhs(); + cholmod_sparse* A = cholmodSparse(ordering, orderedDimensions, cc); + cholmod_dense* b = cholmodRhs(cc); // QR - double tol = 1e-30; - cholmod_dense* x = SuiteSparseQR_min2norm (0, tol, A, b, cc) ; + int ord_method = SPQR_ORDERING_BEST; // matlab uses 7 + double tol = SPQR_NO_TOL; // matlab uses SPQR_DEFAULT_TOL + cholmod_dense* x = SuiteSparseQR (ord_method, tol, A, b, cc) ; // create the update vector VectorConfig config; diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 172087d1c..d085a5c80 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -270,18 +270,19 @@ namespace gtsam { * 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 */ - cholmod_sparse* cholmodSparse(const Ordering& ordering, std::vector& orderedDimensions) const; + cholmod_sparse* cholmodSparse(const Ordering& ordering, std::vector& orderedDimensions, + cholmod_common *cc) const; /** * Convert the RHS to CHOLMOD's column-major matrix format * 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 */ - VectorConfig optimizeSPQR(const Ordering& ordering) const; + VectorConfig optimizeSPQR(const Ordering& ordering); #endif }; } // namespace gtsam diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 7cd681171..4fd8c11d4 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -422,7 +422,10 @@ TEST( GaussianFactorGraph, cholmodSparse ) Ordering ord; ord += "x2","l1","x1"; vector 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->nrow == 8); @@ -446,9 +449,6 @@ TEST( GaussianFactorGraph, cholmodSparse ) for(int index=0; index<14; 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_finish(cc) ; } @@ -459,7 +459,10 @@ TEST( GaussianFactorGraph, cholmodRhs ) // create a small linear factor graph 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->nrow == 8); @@ -472,9 +475,6 @@ TEST( GaussianFactorGraph, cholmodRhs ) for(int index=0; index<8; 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_finish(cc); }