diff --git a/cpp/Factorization.h b/cpp/Factorization.h index 923cb6fc1..381609bff 100644 --- a/cpp/Factorization.h +++ b/cpp/Factorization.h @@ -45,37 +45,13 @@ namespace gtsam { boost::shared_ptr linearize(const NonlinearGraph& g, const Config& config) const { return g.linearize(config); } - }; - -#ifdef USE_SPQR - /** - * A linear system solver using factorization - */ - template - class FactorizationSPQR { - private: - boost::shared_ptr ordering_; - - public: - FactorizationSPQR(boost::shared_ptr ordering, bool old=true) - : ordering_(ordering) { - if (!ordering) throw std::invalid_argument("FactorizationSPQR constructor: ordering = NULL"); - } /** - * solve for the optimal displacement in the tangent space, and then solve - * the resulted linear system + * Does not do anything here in Factorization. */ - VectorConfig optimize(GaussianFactorGraph& fg) const { - return fg.optimizeSPQR(*ordering_); - } - - /** - * linearize the non-linear graph around the current config - */ - boost::shared_ptr linearize(const NonlinearGraph& g, const Config& config) const { - return g.linearize(config); + boost::shared_ptr prepareLinear(const GaussianFactorGraph& fg) const { + return boost::shared_ptr(new Factorization(*this)); } }; -#endif + } diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 9e766b166..fb65ac5d3 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -19,6 +19,10 @@ #include "inference-inl.h" #include "iterative.h" +#ifdef USE_SPQR +#include ; +#endif + using namespace std; using namespace gtsam; using namespace boost::assign; @@ -439,134 +443,4 @@ boost::shared_ptr GaussianFactorGraph::conjugateGradientDescent_( maxIterations))); } -#ifdef USE_SPQR -/* ************************************************************************* */ -cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vector& orderedDimensions, - cholmod_common *cc) const { - Dimensions colIndices; - size_t numCols; - boost::tie(colIndices, numCols) = columnIndices_(ordering); - - // 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; - ivs_vector.resize(numCols); - Vector sigmas; - SymbolMap::const_iterator jA; - BOOST_FOREACH(const sharedFactor& factor,factors_) { // iterate over all the factors - for(jA = factor->begin(); jA!=factor->end(); jA++) { // iterate over all matrices in the factor - column_start = colIndices.at(jA->first) - 1; // find the first column index for this key - sigmas = factor->get_sigmas(); - for (size_t i = 0; i < jA->second.size1(); i++) // interate over all the non-zero entries in the submatrix - for (size_t j = 0; j < jA->second.size2(); j++) - if (jA->second(i, j) != 0.0) { - ivs_vector[j + column_start].push_back(make_pair(i + row_start, jA->second(i, j) / sigmas[i])); - nnz++; - } - } - row_start += factor->numberOfRows(); - } - - // assemble the CHOLMOD data structure - 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; - BOOST_FOREACH(const vector& ivs, ivs_vector) { - *(p2++) = *(p++) + ivs.size(); - BOOST_FOREACH(const RowValue& iv, ivs) { - *(i++) = iv.first; - *(x++) = iv.second; - } - } - - // order the column indices w.r.t. the given ordering - vector orderedIndices; - BOOST_FOREACH(const Symbol& key, ordering) - orderedIndices.push_back(colIndices[key] - 1); - orderedIndices.push_back(numCols); - - // record the dimensions for each key as the same order in the {ordering} - vector::const_iterator it1 = orderedIndices.begin(); - vector::const_iterator it2 = orderedIndices.begin(); it2++; - while(it2 != orderedIndices.end()) - orderedDimensions.push_back(*(it2++) - *(it1++)); - - return A; -} - - -/* ************************************************************************* */ -// learn from spqr_mx_get_dense -cholmod_dense* GaussianFactorGraph::cholmodRhs(cholmod_common *cc) const { - - 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_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(); - it_b_end = factor->get_b().end(); - it_sigmas = factor->get_sigmas().begin(); - for(; it_b != it_b_end; ) - *(x_current++) = *(it_b++) / *(it_sigmas++); - } - - return b; -} - -/* ************************************************************************* */ -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, cc); - cholmod_dense* b = cholmodRhs(cc); - - // QR - 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; - double *x_start = (double*)x->x, *x_end; - vector::const_iterator itDim = orderedDimensions.begin(); - BOOST_FOREACH(const Symbol& key, ordering) { - Vector v(*itDim); - x_end = x_start + *itDim; - copy(x_start, x_end, v.data().begin()); - config.insert(key, v); - itDim++; - x_start = x_end; - } - - // free memory - cholmod_l_free_sparse(&A, cc); - cholmod_l_free_dense (&b, cc) ; - cholmod_l_free_dense (&x, cc); - cholmod_l_finish(cc); - - return config; -} -#endif - /* ************************************************************************* */ diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index d085a5c80..d2ae8d830 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -14,10 +14,6 @@ #include -#ifdef USE_SPQR -#include -#endif - #include "FactorGraph.h" #include "Errors.h" #include "GaussianFactor.h" @@ -264,25 +260,5 @@ namespace gtsam { boost::shared_ptr conjugateGradientDescent_( const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3, size_t maxIterations = 0) const; - -#ifdef USE_SPQR - /** - * 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, - 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(cholmod_common *cc) const; - - /** - * optimizing using SparseQR package - */ - VectorConfig optimizeSPQR(const Ordering& ordering); -#endif }; } // namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 8cd78e771..fd170310a 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -303,7 +303,8 @@ AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serializati # adding SparseQR support if USE_SPQR AM_CXXFLAGS += -I$(HOME)/include/SuiteSparse -DUSE_SPQR -AM_LDFLAGS += -L$(HOME)/lib/SuiteSparse -lcholmod -lsqpr +AM_LDFLAGS += -L$(HOME)/lib/SuiteSparse -lufconfig -lamd -lcamd -lcolamd -lbtf -lklu -lldl -lccolamd -lumfpack -lcholmod -lcxsparse -lspqr +#AM_LDFLAGS += -L$(HOME)/lib/SuiteSparse -lsqpr endif # adding cblas implementation - split into a default linux version using the diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index 526ec209a..24fd15db9 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -220,12 +220,13 @@ namespace gtsam { // linearize all factors once boost::shared_ptr linear = solver_->linearize(*graph_, *config_); + NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linear), error_, lambda_); if (verbosity >= LINEAR) linear->print("linear"); // try lambda steps with successively larger lambda until we achieve descent if (verbosity >= LAMBDA) cout << "Trying Lambda for the first time" << endl; - return try_lambda(*linear, verbosity, lambdaFactor, lambdaMode); + return prepared.try_lambda(*linear, verbosity, lambdaFactor, lambdaMode); } /* ************************************************************************* */ diff --git a/cpp/NonlinearOptimizer.h b/cpp/NonlinearOptimizer.h index 431f5959b..115f96bb5 100644 --- a/cpp/NonlinearOptimizer.h +++ b/cpp/NonlinearOptimizer.h @@ -92,10 +92,17 @@ namespace gtsam { public: /** - * Constructor + * Constructor that evaluates new error */ NonlinearOptimizer(shared_graph graph, shared_config config, shared_solver solver, - double lambda = 1e-5); + const double lambda = 1e-5); + + /** + * Constructor that does not do any computation + */ + NonlinearOptimizer(shared_graph graph, shared_config config, shared_solver solver, + const double error, const double lambda): graph_(graph), config_(config), + error_(error), lambda_(lambda), solver_(solver) {} /** * Copy constructor diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 4fd8c11d4..f055e7045 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -413,91 +413,6 @@ TEST( GaussianFactorGraph, sparse ) 10.,10., -10.,-10., 10., 10., 5.,5.,-5.,-5., 5., 5.,-5.,-5.), ijs); } -#ifdef USE_SPQR -/* ************************************************************************* */ -TEST( GaussianFactorGraph, cholmodSparse ) -{ - // create a small linear factor graph - GaussianFactorGraph fg = createGaussianFactorGraph(); - - Ordering ord; ord += "x2","l1","x1"; - vector 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); - CHECK(A->nzmax == 14); - CHECK(A->stype == 0); - CHECK(A->xtype == CHOLMOD_REAL); - CHECK(A->dtype == CHOLMOD_DOUBLE); - CHECK(A->itype == CHOLMOD_LONG); - CHECK(A->packed == 1); - CHECK(A->sorted == 1); - - long p[7] = {0, 2, 4, 6, 8, 11, 14}; - for(long index=0; index<7; index++) - CHECK(p[index] == *((long*)A->p + index)); - - long i[14] = {2, 6, 3, 7, 4, 6, 5, 7, 0, 2, 4, 1, 3, 5}; - for(long index=0; index<14; index++) - CHECK(i[index] == *((long*)A->i + index)); - - double x[14] = {10, -5, 10, -5, 5, 5, 5, 5, 10, -10, -5, 10, -10, -5}; - for(int index=0; index<14; index++) - CHECK(x[index] == *((double*)A->x + index)); - - cholmod_l_free_sparse(&A, cc); - cholmod_l_finish(cc) ; -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, cholmodRhs ) -{ - // create a small linear factor graph - GaussianFactorGraph fg = createGaussianFactorGraph(); - - cholmod_common Common, *cc; - cc = &Common; - cholmod_l_start (cc); - cholmod_dense* b = fg.cholmodRhs(cc); - - CHECK(b->ncol == 1); - CHECK(b->nrow == 8); - CHECK(b->nzmax == 8); - CHECK(b->d == 8); - CHECK(b->xtype == CHOLMOD_REAL); - CHECK(b->dtype == CHOLMOD_DOUBLE); - - double x[8] = {-1., -1., 2., -1., 0., 1., -1., 1.5}; - for(int index=0; index<8; index++) - CHECK(x[index] == *((double*)b->x + index)); - - cholmod_l_free_dense(&b, cc); - cholmod_l_finish(cc); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, optimizeSPQR ) -{ - // create a graph - GaussianFactorGraph fg = createGaussianFactorGraph(); - - // create an ordering - Ordering ord; ord += "x2","l1","x1"; - - // optimize the graph - VectorConfig actual = fg.optimizeSPQR(ord); - - // verify - VectorConfig expected = createCorrectDelta(); - - CHECK(assert_equal(expected,actual)); -} -#endif - /* ************************************************************************* */ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 0dd5c1ad5..6a5b57942 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -201,35 +201,6 @@ TEST( NonlinearOptimizer, Factorization ) CHECK(assert_equal(expected, *optimized.config(), 1e-5)); } -/* ************************************************************************* */ -#ifdef USE_SPQR -TEST( NonlinearOptimizer, FactorizationSPQR ) -{ - typedef NonlinearOptimizer > Optimizer; - - boost::shared_ptr config(new Pose2Config); - config->insert(1, Pose2(0.,0.,0.)); - config->insert(2, Pose2(1.5,0.,0.)); - - boost::shared_ptr graph(new Pose2Graph); - graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); - graph->addPrior(2, Pose2(1.5,0.,0.), Isotropic::Sigma(3, 1e-10)); - - boost::shared_ptr ordering(new Ordering); - ordering->push_back(Pose2Config::Key(1)); - ordering->push_back(Pose2Config::Key(2)); - Optimizer::shared_solver solver(new FactorizationSPQR(ordering)); - - Optimizer optimizer(graph, config, solver); - Optimizer optimized = optimizer.iterateLM(); - - Pose2Config expected; - expected.insert(1, Pose2(0.,0.,0.)); - expected.insert(2, Pose2(1.5,0.,0.)); - CHECK(assert_equal(expected, *optimized.config(), 1e-5)); -} -#endif - /* ************************************************************************* */ TEST( NonlinearOptimizer, SubgraphSolver ) {